diff --git a/README.md b/README.md index 2c6b7a3..1b5ed88 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ -# LSM6DS3 +# LSM6DS -Biblioteca adaptada do [driver da ST](https://github.com/STMicroelectronics/STMems_Standard_C_drivers/tree/master/lsm6ds3_STdC) para utilizar o sensor de 6 eixos LSM6DS3 (Os arquivos `lsm6ds3_reg.h/.c` não foram modificados) +Biblioteca adaptada do driver da ST para utilizar tanto o sensor de 6 eixos [LSM6DS3](https://github.com/STMicroelectronics/lsm6ds3-pid) quanto o [LSM6DSO](https://github.com/STMicroelectronics/stm32-lsm6dso) # Utilização ## Cube: @@ -11,7 +11,10 @@ Biblioteca adaptada do [driver da ST](https://github.com/STMicroelectronics/STMe * CPOL deve ter valor 1 (High) e CPHA deve ter valor 1 (2 Edge) * É necessário escolher um pino e setar como GPIO_Output, esse pino será usado como Chip Select -## No arquivo lsm6ds3.c: +## No arquivo lsm6ds.c: + +Neste arquivo, no momento em que a placa é ligada, já é identificado qual sensor está sendo utilizado. + * Descomentar a linha com o protocolo usado e comentar a outra * Para I2C: ```c @@ -61,35 +64,35 @@ Biblioteca adaptada do [driver da ST](https://github.com/STMicroelectronics/STMe * Criar uma struct que contém os valores desejados de sensibilidades e de frequências de amostragem do acelerômetro e do giroscópio ```c // Exemplo - lsm6ds3_settings_t lsm6ds3_settings = { - .lsm6ds3_xl_fs = LSM6DS3_16g, - .lsm6ds3_fs_g = LSM6DS3_2000dps, - .lsm6ds3_odr_xl = LSM6DS3_XL_ODR_6k66Hz, - .lsm6ds3_odr_g = LSM6DS3_GY_ODR_1k66Hz + lsm6ds_settings_t lsm6ds_settings = { + .lsm6ds_xl_fs = LSM6DS_16g, + .lsm6ds_fs_g = LSM6DS_2000dps, + .lsm6ds_odr_xl = LSM6DS_XL_ODR_6k66Hz, + .lsm6ds_odr_g = LSM6DS_GY_ODR_1k66Hz }; ``` * Para inicializar o sensor, use a função ```c - int8_t lsm6ds3_init(lsm6ds3_settings_t lsm6ds3_settings) + int8_t lsm6ds_init(lsm6ds_settings_t lsm6ds_settings) ``` -* A função `lsm6ds3_init` retorna `1` caso haja erro na leitura do registrador `WHO_AM_I` do sensor +* A função `lsm6ds_init` retorna `1` caso haja erro na leitura do registrador `WHO_AM_I` do sensor ## Para obter as leituras do sensor: * Primeiro é necessário atualizar os dados brutos: * Se os pinos INT não estão sendo usados, use a função: ```c - void lsm6ds3_update_data() + void lsm6ds_update_data() ``` * Se os pinos INT estão sendo usados, use a função: ```c - void lsm6ds3_update_data_interrupt() + void lsm6ds_update_data_interrupt() ``` * Depois disso, para obter as leituras já convertidas, use as funções ```c - float* lsm6ds3_get_acc_data_mg() - float* lsm6ds3_get_gyro_data_mdps() + float* lsm6ds_get_acc_data_mg() + float* lsm6ds_get_gyro_data_mdps() ``` --------------------- -Equipe ThundeRatz de Robótica \ No newline at end of file +Equipe ThundeRatz de Robótica diff --git a/cmake/FindLSM6DS3.cmake b/cmake/FindLSM6DS3.cmake new file mode 100644 index 0000000..0dab8fc --- /dev/null +++ b/cmake/FindLSM6DS3.cmake @@ -0,0 +1,27 @@ +set(LSM6DS3_PATH ./lib/LSM6DS3) +set(CUBE_INC_PATH ${CMAKE_SOURCE_DIR}/cube/Inc) + +file(GLOB_RECURSE SOURCES LIST_DIRECTORIES false "${LSM6DS3_PATH}/src/*") + +list(APPEND LIB_SOURCES + ${SOURCES} +) + +list(APPEND LIB_INCLUDE_DIRECTORIES + ${LSM6DS3_PATH}/inc + ${LSM6DS3_PATH}/inc/types + ${LSM6DS3_PATH}/inc/reg + ${LSM6DS3_PATH}/inc/proxys + ${LSM6DS3_PATH}/inc/interfaces + ${LSM6DS3_PATH}/inc/platforms +) + +if(NOT EXISTS ${CUBE_INC_PATH}/i2c.h) + list(APPEND LIB_INCLUDE_DIRECTORIES + ${LSM6DS3_PATH}/inc/cube_mock/i2c_mock + ) +elseif(NOT EXISTS ${CUBE_INC_PATH}/spi.h) + list(APPEND LIB_INCLUDE_DIRECTORIES + ${LSM6DS3_PATH}/inc/cube_mock/spi_mock + ) +endif() diff --git a/inc/cube_mock/i2c_mock/i2c.h b/inc/cube_mock/i2c_mock/i2c.h new file mode 100644 index 0000000..2afd1e7 --- /dev/null +++ b/inc/cube_mock/i2c_mock/i2c.h @@ -0,0 +1,17 @@ +/** + * @file i2c.h + * + * @brief This file is a mock file for when "i2c.h" is not defined + * + * @author Bruno Machado + * @author Pedro Machado + * + * @date 08/2023 + */ + + +#ifndef __I2C_H__ +#define __I2C_H__ + + +#endif diff --git a/inc/cube_mock/spi_mock/spi.h b/inc/cube_mock/spi_mock/spi.h new file mode 100644 index 0000000..8d4b825 --- /dev/null +++ b/inc/cube_mock/spi_mock/spi.h @@ -0,0 +1,17 @@ +/** + * @file spi.h + * + * @brief This file is a mock file for when "spi.h" is not defined + * + * @author Bruno Machado + * @author Pedro Machado + * + * @date 08/2023 + */ + +#ifndef __SPI_H__ +#define __SPI_H__ + + + +#endif diff --git a/inc/interfaces/lsm6ds_c_interface.h b/inc/interfaces/lsm6ds_c_interface.h new file mode 100644 index 0000000..7da0ee5 --- /dev/null +++ b/inc/interfaces/lsm6ds_c_interface.h @@ -0,0 +1,75 @@ +/** + * @file lsm6ds_c_interface.h + * + * @brief This file is a declaration for a c compatible interface + * + * @author Bruno Machado + * @author Pedro Machado + * + * @date 08/2023 + */ + +#ifndef __LSM6DS_C_INTERFACE_H__ +#define __LSM6DS_C_INTERFACE_H__ + +#include "lsm6ds_settings_type.h" +#include "lsm6ds_pinout.h" +#include "lsm6ds_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @brief Initialize the sensor and set acc/gyro sensitivities and data rates + * + * @param lsm6ds_settings Struct of acc/gyro sensitivities and data rates settings + * @param I2C_pinout_config Struct of sensor pinout configurations in I2C mode + * + */ +int8_t lsm6ds_init_I2C(lsm6ds_settings_t lsm6ds_settings, lsm6ds_I2C_pinout_t I2C_pinout_config); + +/** + * @brief Initialize the sensor and set acc/gyro sensitivities and data rates + * + * @param lsm6ds_settings Struct of acc/gyro sensitivities and data rates settings + * @param SPI_pinout_config Struct of sensor pinout configurations in SPI mode + * + */ +int8_t lsm6ds_init_SPI(lsm6ds_settings_t lsm6ds_settings, lsm6ds_SPI_pinout_t SPI_pinout_config); + +/** + * @brief Update sensor data if available (Use this function if not using interrupt pins) + */ +void lsm6ds_update_data(); + +/** + * @brief Update sensor data (Use this function when using interrupt pins) + */ +void lsm6ds_update_data_ready_interrupt(); + +/** + * @brief Update sensor data (Use this function if using fifo continuous mode with interrupt pins) + */ +void lsm6ds_update_data_fifo_full_interrupt(); + +/** + * @brief Resets fifo data + */ +void lsm6ds_reset_fifo(); + +/** + * @brief Get accelerometer data array in mg + */ +float* lsm6ds_get_acc_data_mg(); + +/** + * @brief Get gyroscope data array in mdps + */ +float* lsm6ds_get_gyro_data_mdps(); + +#ifdef __cplusplus +} +#endif + +#endif // __LSM6DS_C_INTERFACE_H__ diff --git a/inc/interfaces/lsm6ds_interface.hpp b/inc/interfaces/lsm6ds_interface.hpp new file mode 100644 index 0000000..575055d --- /dev/null +++ b/inc/interfaces/lsm6ds_interface.hpp @@ -0,0 +1,83 @@ +/** + * @file lsm6ds_interface.hpp + * + * @brief This file is a declaration of the LSM6DS_Interface class + * + * @author Bruno Machado + * @author Pedro Machado + * + * @date 08/2023 + */ + +#ifndef __LSM6DS_INTERFACE_HPP__ +#define __LSM6DS_INTERFACE_HPP__ + +#include "lsm6ds_proxy.hpp" + +class LSM6DS_Interface { + public: + /** + * @brief Construct a new LSM6DS_Interface object + * + */ + LSM6DS_Interface() = default; + + /** + * @brief Destroy the LSM6DS_Interface object + * + */ + ~LSM6DS_Interface() = default; + + /** + * @brief Initialize the sensor and set acc/gyro sensitivities and data rates + * + * @param lsm6ds_settings Struct of acc/gyro sensitivities and data rates settings + * @param I2C_pinout_config Struct of sensor pinout configurations in I2C mode + * + */ + int8_t init(lsm6ds_settings_t lsm6ds_settings, lsm6ds_I2C_pinout_t I2C_pinout_config); + + /** + * @brief Initialize the sensor and set acc/gyro sensitivities and data rates + * + * @param lsm6ds_settings Struct of acc/gyro sensitivities and data rates settings + * @param SPI_pinout_config Struct of sensor pinout configurations in SPI mode + * + */ + int8_t init(lsm6ds_settings_t lsm6ds_settings, lsm6ds_SPI_pinout_t SPI_pinout_config); + + /** + * @brief Update sensor data if available (Use this function if not using interrupt pins) + */ + void update_data(); + + /** + * @brief Update sensor data (Use this function when using interrupt pins) + */ + void update_data_ready_interrupt(); + + /** + * @brief Update sensor data (Use this function if using fifo continuous mode with interrupt pins) + */ + void update_data_fifo_full_interrupt(); + + /** + * @brief Resets fifo data + */ + void reset_fifo(); + + /** + * @brief Get accelerometer data array in mg + */ + float* get_acc_data_mg(); + + /** + * @brief Get gyroscope data array in mdps + */ + float* get_gyro_data_mdps(); + + private: + std::unique_ptr lsm6ds_sensor; +}; + +#endif // __LSM6DS_INTERFACE_HPP__ diff --git a/inc/lsm6ds3.h b/inc/lsm6ds3.h deleted file mode 100644 index 7faf3a5..0000000 --- a/inc/lsm6ds3.h +++ /dev/null @@ -1,63 +0,0 @@ -/** - * @file lsm6ds3.h - * - * @brief This file provide functions to get data from the sensor lsm6ds3 - * - * @author Bruno Guo - * - * @date 11/2019 - */ - -#ifndef LSM6DS3_H -#define LSM6DS3_H - -#include - -/* Struct of acc/gyro sensitivities and data rates settings */ -typedef struct lsm6ds3_settings { - lsm6ds3_xl_fs_t lsm6ds3_xl_fs; - lsm6ds3_fs_g_t lsm6ds3_fs_g; - lsm6ds3_odr_xl_t lsm6ds3_odr_xl; - lsm6ds3_odr_g_t lsm6ds3_odr_g; -} lsm6ds3_settings_t; - -/* Error codes */ -typedef int8_t LSM6DS3_error; -#define LSM6DS3_ERROR_NONE ((LSM6DS3_error) 0) -#define LSM6DS3_ERROR_WHO_AM_I ((LSM6DS3_error) 1) -#define LSM6DS3_ERROR_WRITE_REGISTER ((LSM6DS3_error) 2) - -/** - * @brief Initialize the sensor and set acc/gyro sensitivities and data rates - * - * @param lsm6ds3_settings Struct of acc/gyro sensitivities and data rates settings - * - */ -int8_t lsm6ds3_init(lsm6ds3_settings_t lsm6ds3_settings); - -/** - * @brief Update sensor data if available (Use this function if not using interrupt pins) - */ -void lsm6ds3_update_data(); - -/** - * @brief Update sensor data (Use this function when using interrupt pins) - */ -void lsm6ds3_update_data_interrupt(); - -/** - * @brief Get a pointer to accelerometer data array in mg - */ -float* lsm6ds3_get_acc_data_mg(); - -/** - * @brief Get a pointer to gyroscope data array in mdps - */ -float* lsm6ds3_get_gyro_data_mdps(); - -/** - * @brief Get temperature in Celsius - */ -float lsm6ds3_get_temperature_degC(); - -#endif // LSM6DS3_H diff --git a/inc/lsm6ds3_reg.h b/inc/lsm6ds3_reg.h deleted file mode 100644 index 80e6438..0000000 --- a/inc/lsm6ds3_reg.h +++ /dev/null @@ -1,1733 +0,0 @@ -/* - ****************************************************************************** - * @file lsm6ds3_reg.h - * @author Sensors Software Solution Team - * @brief This file contains all the functions prototypes for the - * lsm6ds3_reg.c driver. - ****************************************************************************** - * @attention - * - *

© Copyright (c) 2019 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 LSM6DS3_REGS_H -#define LSM6DS3_REGS_H - -#ifdef __cplusplus -extern "C" { -#endif - -/* Includes ------------------------------------------------------------------*/ -#include -#include - -/** @addtogroup LSM6DS3 - * @{ - * - */ - -/** @defgroup STMicroelectronics sensors common types - * @{ - * - */ - -#ifndef MEMS_SHARED_TYPES -#define MEMS_SHARED_TYPES - -typedef struct { - 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; -} bitwise_t; - -#define PROPERTY_DISABLE (0U) -#define PROPERTY_ENABLE (1U) - -/** @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 (* stmdev_write_ptr)(void*, uint8_t, uint8_t*, uint16_t); -typedef int32_t (* stmdev_read_ptr) (void*, uint8_t, uint8_t*, uint16_t); - -typedef struct { - /** Component mandatory fields **/ - stmdev_write_ptr write_reg; - stmdev_read_ptr read_reg; - - /** Customizable optional pointer **/ - void* handle; -} stmdev_ctx_t; - -/** - * @} - * - */ - -#endif /* MEMS_SHARED_TYPES */ - -#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 LSM6DS3_Infos - * @{ - * - */ - -/** I2C Device Address 8 bit format if SA0=0 -> 0xD5 if SA0=1 -> 0xD7 **/ -#define LSM6DS3_I2C_ADD_L 0xD5U -#define LSM6DS3_I2C_ADD_H 0xD7U - -/** Device Identification (Who am I) **/ -#define LSM6DS3_ID 0x69U - -/** - * @} - * - */ - -#define LSM6DS3_FUNC_CFG_ACCESS 0x01U -typedef struct { - uint8_t not_used_01 : 7; - uint8_t func_cfg_en : 1; -} lsm6ds3_func_cfg_access_t; - -#define LSM6DS3_SENSOR_SYNC_TIME_FRAME 0x04U -typedef struct { - uint8_t tph : 8; -} lsm6ds3_sensor_sync_time_frame_t; - -#define LSM6DS3_FIFO_CTRL1 0x06U -typedef struct { - uint8_t fth : 8; -} lsm6ds3_fifo_ctrl1_t; - -#define LSM6DS3_FIFO_CTRL2 0x07U -typedef struct { - uint8_t fth : 4; - uint8_t not_used_01 : 2; - uint8_t timer_pedo_fifo_drdy : 1; - uint8_t timer_pedo_fifo_en : 1; -} lsm6ds3_fifo_ctrl2_t; - -#define LSM6DS3_FIFO_CTRL3 0x08U -typedef struct { - uint8_t dec_fifo_xl : 3; - uint8_t dec_fifo_gyro : 3; - uint8_t not_used_01 : 2; -} lsm6ds3_fifo_ctrl3_t; - -#define LSM6DS3_FIFO_CTRL4 0x09U -typedef struct { - uint8_t dec_ds3_fifo : 3; - uint8_t dec_ds4_fifo : 3; - uint8_t only_high_data : 1; - uint8_t not_used_01 : 1; -} lsm6ds3_fifo_ctrl4_t; - -#define LSM6DS3_FIFO_CTRL5 0x0AU -typedef struct { - uint8_t fifo_mode : 3; - uint8_t odr_fifo : 4; - uint8_t not_used_01 : 1; -} lsm6ds3_fifo_ctrl5_t; - -#define LSM6DS3_ORIENT_CFG_G 0x0BU -typedef struct { - uint8_t orient : 3; - uint8_t sign_g : 3;/* SignX_G) + SignY_G + SignZ_G */ - uint8_t not_used_01 : 2; -} lsm6ds3_orient_cfg_g_t; - -#define LSM6DS3_INT1_CTRL 0x0DU -typedef struct { - uint8_t int1_drdy_xl : 1; - uint8_t int1_drdy_g : 1; - uint8_t int1_boot : 1; - uint8_t int1_fth : 1; - uint8_t int1_fifo_ovr : 1; - uint8_t int1_full_flag : 1; - uint8_t int1_sign_mot : 1; - uint8_t int1_step_detector : 1; -} lsm6ds3_int1_ctrl_t; - -#define LSM6DS3_INT2_CTRL 0x0EU -typedef struct { - uint8_t int2_drdy_xl : 1; - uint8_t int2_drdy_g : 1; - uint8_t int2_drdy_temp : 1; - uint8_t int2_fth : 1; - uint8_t int2_fifo_ovr : 1; - uint8_t int2_full_flag : 1; - uint8_t int2_step_count_ov : 1; - uint8_t int2_step_delta : 1; -} lsm6ds3_int2_ctrl_t; - -#define LSM6DS3_WHO_AM_I 0x0FU -#define LSM6DS3_CTRL1_XL 0x10U -typedef struct { - uint8_t bw_xl : 2; - uint8_t fs_xl : 2; - uint8_t odr_xl : 4; -} lsm6ds3_ctrl1_xl_t; - -#define LSM6DS3_CTRL2_G 0x11U -typedef struct { - uint8_t not_used_01 : 1; - uint8_t fs_g : 3; /* FS_G + FS_125 */ - uint8_t odr_g : 4; -} lsm6ds3_ctrl2_g_t; - -#define LSM6DS3_CTRL3_C 0x12U -typedef struct { - uint8_t sw_reset : 1; - uint8_t ble : 1; - uint8_t if_inc : 1; - uint8_t sim : 1; - uint8_t pp_od : 1; - uint8_t h_lactive : 1; - uint8_t bdu : 1; - uint8_t boot : 1; -} lsm6ds3_ctrl3_c_t; - -#define LSM6DS3_CTRL4_C 0x13U -typedef struct { - uint8_t stop_on_fth : 1; - uint8_t not_used_01 : 1; - uint8_t i2c_disable : 1; - uint8_t drdy_mask : 1; - uint8_t fifo_temp_en : 1; - uint8_t int2_on_int1 : 1; - uint8_t sleep_g : 1; - uint8_t xl_bw_scal_odr : 1; -} lsm6ds3_ctrl4_c_t; - -#define LSM6DS3_CTRL5_C 0x14U -typedef struct { - uint8_t st_xl : 2; - uint8_t st_g : 2; - uint8_t not_used_01 : 1; - uint8_t rounding : 3; -} lsm6ds3_ctrl5_c_t; - -#define LSM6DS3_CTRL6_C 0x15U -typedef struct { - uint8_t not_used_01 : 4; - uint8_t xl_hm_mode : 1; - uint8_t den_mode : 3;/* trig_en + lvl1_en + lvl2_en */ -} lsm6ds3_ctrl6_c_t; - -#define LSM6DS3_CTRL7_G 0x16U -typedef struct { - uint8_t not_used_01 : 2; - uint8_t rounding_status : 1; - uint8_t hpcf_g : 2; - uint8_t hp_g_rst : 1; - uint8_t hp_g_en : 1; - uint8_t g_hm_mode : 1; -} lsm6ds3_ctrl7_g_t; - -#define LSM6DS3_CTRL8_XL 0x17U -typedef struct { - uint8_t low_pass_on_6d : 1; - uint8_t not_used_01 : 1; - uint8_t hp_slope_xl_en : 1; - uint8_t not_used_02 : 2; - uint8_t hpcf_xl : 2; - uint8_t lpf2_xl_en : 1; -} lsm6ds3_ctrl8_xl_t; - -#define LSM6DS3_CTRL9_XL 0x18U -typedef struct { - uint8_t not_used_01 : 2; - uint8_t soft_en : 1; - uint8_t xen_xl : 1; - uint8_t yen_xl : 1; - uint8_t zen_xl : 1; - uint8_t not_used_02 : 2; -} lsm6ds3_ctrl9_xl_t; - -#define LSM6DS3_CTRL10_C 0x19U -typedef struct { - uint8_t sign_motion_en : 1; - uint8_t pedo_rst_step : 1; - uint8_t func_en : 1; - uint8_t xen_g : 1; - uint8_t yen_g : 1; - uint8_t zen_g : 1; - uint8_t not_used_01 : 2; -} lsm6ds3_ctrl10_c_t; - -#define LSM6DS3_MASTER_CONFIG 0x1AU -typedef struct { - uint8_t master_on : 1; - uint8_t iron_en : 1; - uint8_t pass_through_mode : 1; - uint8_t pull_up_en : 1; - uint8_t start_config : 1; - uint8_t not_used_01 : 1; - uint8_t data_valid_sel_fifo : 1; - uint8_t drdy_on_int1 : 1; -} lsm6ds3_master_config_t; - -#define LSM6DS3_WAKE_UP_SRC 0x1BU -typedef struct { - uint8_t z_wu : 1; - uint8_t y_wu : 1; - uint8_t x_wu : 1; - uint8_t wu_ia : 1; - uint8_t sleep_state_ia : 1; - uint8_t ff_ia : 1; - uint8_t not_used_01 : 2; -} lsm6ds3_wake_up_src_t; - -#define LSM6DS3_TAP_SRC 0x1CU -typedef struct { - 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_used_01 : 1; -} lsm6ds3_tap_src_t; - -#define LSM6DS3_D6D_SRC 0x1DU -typedef struct { - 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_used_01 : 1; -} lsm6ds3_d6d_src_t; - -#define LSM6DS3_STATUS_REG 0x1EU -typedef struct { - uint8_t xlda : 1; - uint8_t gda : 1; - uint8_t tda : 1; - uint8_t not_used_01 : 5; -} lsm6ds3_status_reg_t; - -#define LSM6DS3_OUT_TEMP_L 0x20U -#define LSM6DS3_OUT_TEMP_H 0x21U -#define LSM6DS3_OUTX_L_G 0x22U -#define LSM6DS3_OUTX_H_G 0x23U -#define LSM6DS3_OUTY_L_G 0x24U -#define LSM6DS3_OUTY_H_G 0x25U -#define LSM6DS3_OUTZ_L_G 0x26U -#define LSM6DS3_OUTZ_H_G 0x27U -#define LSM6DS3_OUTX_L_XL 0x28U -#define LSM6DS3_OUTX_H_XL 0x29U -#define LSM6DS3_OUTY_L_XL 0x2AU -#define LSM6DS3_OUTY_H_XL 0x2BU -#define LSM6DS3_OUTZ_L_XL 0x2CU -#define LSM6DS3_OUTZ_H_XL 0x2DU -#define LSM6DS3_SENSORHUB1_REG 0x2EU -typedef struct { - uint8_t shub1_0 : 1; - uint8_t shub1_1 : 1; - uint8_t shub1_2 : 1; - uint8_t shub1_3 : 1; - uint8_t shub1_4 : 1; - uint8_t shub1_5 : 1; - uint8_t shub1_6 : 1; - uint8_t shub1_7 : 1; -} lsm6ds3_sensorhub1_reg_t; - -#define LSM6DS3_SENSORHUB2_REG 0x2FU -typedef struct { - uint8_t shub2_0 : 1; - uint8_t shub2_1 : 1; - uint8_t shub2_2 : 1; - uint8_t shub2_3 : 1; - uint8_t shub2_4 : 1; - uint8_t shub2_5 : 1; - uint8_t shub2_6 : 1; - uint8_t shub2_7 : 1; -} lsm6ds3_sensorhub2_reg_t; - -#define LSM6DS3_SENSORHUB3_REG 0x30U -typedef struct { - uint8_t shub3_0 : 1; - uint8_t shub3_1 : 1; - uint8_t shub3_2 : 1; - uint8_t shub3_3 : 1; - uint8_t shub3_4 : 1; - uint8_t shub3_5 : 1; - uint8_t shub3_6 : 1; - uint8_t shub3_7 : 1; -} lsm6ds3_sensorhub3_reg_t; - -#define LSM6DS3_SENSORHUB4_REG 0x31U -typedef struct { - uint8_t shub4_0 : 1; - uint8_t shub4_1 : 1; - uint8_t shub4_2 : 1; - uint8_t shub4_3 : 1; - uint8_t shub4_4 : 1; - uint8_t shub4_5 : 1; - uint8_t shub4_6 : 1; - uint8_t shub4_7 : 1; -} lsm6ds3_sensorhub4_reg_t; - -#define LSM6DS3_SENSORHUB5_REG 0x32U -typedef struct { - uint8_t shub5_0 : 1; - uint8_t shub5_1 : 1; - uint8_t shub5_2 : 1; - uint8_t shub5_3 : 1; - uint8_t shub5_4 : 1; - uint8_t shub5_5 : 1; - uint8_t shub5_6 : 1; - uint8_t shub5_7 : 1; -} lsm6ds3_sensorhub5_reg_t; - -#define LSM6DS3_SENSORHUB6_REG 0x33U -typedef struct { - uint8_t shub6_0 : 1; - uint8_t shub6_1 : 1; - uint8_t shub6_2 : 1; - uint8_t shub6_3 : 1; - uint8_t shub6_4 : 1; - uint8_t shub6_5 : 1; - uint8_t shub6_6 : 1; - uint8_t shub6_7 : 1; -} lsm6ds3_sensorhub6_reg_t; - -#define LSM6DS3_SENSORHUB7_REG 0x34U -typedef struct { - uint8_t shub7_0 : 1; - uint8_t shub7_1 : 1; - uint8_t shub7_2 : 1; - uint8_t shub7_3 : 1; - uint8_t shub7_4 : 1; - uint8_t shub7_5 : 1; - uint8_t shub7_6 : 1; - uint8_t shub7_7 : 1; -} lsm6ds3_sensorhub7_reg_t; - -#define LSM6DS3_SENSORHUB8_REG 0x35U -typedef struct { - uint8_t shub8_0 : 1; - uint8_t shub8_1 : 1; - uint8_t shub8_2 : 1; - uint8_t shub8_3 : 1; - uint8_t shub8_4 : 1; - uint8_t shub8_5 : 1; - uint8_t shub8_6 : 1; - uint8_t shub8_7 : 1; -} lsm6ds3_sensorhub8_reg_t; - -#define LSM6DS3_SENSORHUB9_REG 0x36U -typedef struct { - uint8_t shub9_0 : 1; - uint8_t shub9_1 : 1; - uint8_t shub9_2 : 1; - uint8_t shub9_3 : 1; - uint8_t shub9_4 : 1; - uint8_t shub9_5 : 1; - uint8_t shub9_6 : 1; - uint8_t shub9_7 : 1; -} lsm6ds3_sensorhub9_reg_t; - -#define LSM6DS3_SENSORHUB10_REG 0x37U -typedef struct { - uint8_t shub10_0 : 1; - uint8_t shub10_1 : 1; - uint8_t shub10_2 : 1; - uint8_t shub10_3 : 1; - uint8_t shub10_4 : 1; - uint8_t shub10_5 : 1; - uint8_t shub10_6 : 1; - uint8_t shub10_7 : 1; -} lsm6ds3_sensorhub10_reg_t; - -#define LSM6DS3_SENSORHUB11_REG 0x38U -typedef struct { - uint8_t shub11_0 : 1; - uint8_t shub11_1 : 1; - uint8_t shub11_2 : 1; - uint8_t shub11_3 : 1; - uint8_t shub11_4 : 1; - uint8_t shub11_5 : 1; - uint8_t shub11_6 : 1; - uint8_t shub11_7 : 1; -} lsm6ds3_sensorhub11_reg_t; - -#define LSM6DS3_SENSORHUB12_REG 0x39U -typedef struct { - uint8_t shub12_0 : 1; - uint8_t shub12_1 : 1; - uint8_t shub12_2 : 1; - uint8_t shub12_3 : 1; - uint8_t shub12_4 : 1; - uint8_t shub12_5 : 1; - uint8_t shub12_6 : 1; - uint8_t shub12_7 : 1; -} lsm6ds3_sensorhub12_reg_t; - -#define LSM6DS3_FIFO_STATUS1 0x3AU -typedef struct { - uint8_t diff_fifo : 8; -} lsm6ds3_fifo_status1_t; - -#define LSM6DS3_FIFO_STATUS2 0x3BU -typedef struct { - uint8_t diff_fifo : 4; - uint8_t fifo_empty : 1; - uint8_t fifo_full : 1; - uint8_t fifo_over_run : 1; - uint8_t fth : 1; -} lsm6ds3_fifo_status2_t; - -#define LSM6DS3_FIFO_STATUS3 0x3CU -typedef struct { - uint8_t fifo_pattern : 8; -} lsm6ds3_fifo_status3_t; - -#define LSM6DS3_FIFO_STATUS4 0x3DU -typedef struct { - uint8_t fifo_pattern : 2; - uint8_t not_used_01 : 6; -} lsm6ds3_fifo_status4_t; - -#define LSM6DS3_FIFO_DATA_OUT_L 0x3EU -#define LSM6DS3_FIFO_DATA_OUT_H 0x3FU -#define LSM6DS3_TIMESTAMP0_REG 0x40U -#define LSM6DS3_TIMESTAMP1_REG 0x41U -#define LSM6DS3_TIMESTAMP2_REG 0x42U -#define LSM6DS3_STEP_TIMESTAMP_L 0x49U -#define LSM6DS3_STEP_TIMESTAMP_H 0x4AU -#define LSM6DS3_STEP_COUNTER_L 0x4BU -#define LSM6DS3_STEP_COUNTER_H 0x4CU -#define LSM6DS3_SENSORHUB13_REG 0x4DU -typedef struct { - uint8_t shub13_0 : 1; - uint8_t shub13_1 : 1; - uint8_t shub13_2 : 1; - uint8_t shub13_3 : 1; - uint8_t shub13_4 : 1; - uint8_t shub13_5 : 1; - uint8_t shub13_6 : 1; - uint8_t shub13_7 : 1; -} lsm6ds3_sensorhub13_reg_t; - -#define LSM6DS3_SENSORHUB14_REG 0x4EU -typedef struct { - uint8_t shub14_0 : 1; - uint8_t shub14_1 : 1; - uint8_t shub14_2 : 1; - uint8_t shub14_3 : 1; - uint8_t shub14_4 : 1; - uint8_t shub14_5 : 1; - uint8_t shub14_6 : 1; - uint8_t shub14_7 : 1; -} lsm6ds3_sensorhub14_reg_t; - -#define LSM6DS3_SENSORHUB15_REG 0x4FU -typedef struct { - uint8_t shub15_0 : 1; - uint8_t shub15_1 : 1; - uint8_t shub15_2 : 1; - uint8_t shub15_3 : 1; - uint8_t shub15_4 : 1; - uint8_t shub15_5 : 1; - uint8_t shub15_6 : 1; - uint8_t shub15_7 : 1; -} lsm6ds3_sensorhub15_reg_t; - -#define LSM6DS3_SENSORHUB16_REG 0x50U -typedef struct { - uint8_t shub16_0 : 1; - uint8_t shub16_1 : 1; - uint8_t shub16_2 : 1; - uint8_t shub16_3 : 1; - uint8_t shub16_4 : 1; - uint8_t shub16_5 : 1; - uint8_t shub16_6 : 1; - uint8_t shub16_7 : 1; -} lsm6ds3_sensorhub16_reg_t; - -#define LSM6DS3_SENSORHUB17_REG 0x51U -typedef struct { - uint8_t shub17_0 : 1; - uint8_t shub17_1 : 1; - uint8_t shub17_2 : 1; - uint8_t shub17_3 : 1; - uint8_t shub17_4 : 1; - uint8_t shub17_5 : 1; - uint8_t shub17_6 : 1; - uint8_t shub17_7 : 1; -} lsm6ds3_sensorhub17_reg_t; - -#define LSM6DS3_SENSORHUB18_REG 0x52U -typedef struct { - uint8_t shub18_0 : 1; - uint8_t shub18_1 : 1; - uint8_t shub18_2 : 1; - uint8_t shub18_3 : 1; - uint8_t shub18_4 : 1; - uint8_t shub18_5 : 1; - uint8_t shub18_6 : 1; - uint8_t shub18_7 : 1; -} lsm6ds3_sensorhub18_reg_t; - -#define LSM6DS3_FUNC_SRC 0x53U -typedef struct { - uint8_t sensor_hub_end_op : 1; - uint8_t si_end_op : 1; - uint8_t not_used_01 : 1; - uint8_t step_overflow : 1; - uint8_t step_detected : 1; - uint8_t tilt_ia : 1; - uint8_t sign_motion_ia : 1; - uint8_t step_count_delta_ia : 1; -} lsm6ds3_func_src_t; - -#define LSM6DS3_TAP_CFG 0x58U -typedef struct { - 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 tilt_en : 1; - uint8_t pedo_en : 1; - uint8_t timer_en : 1; -} lsm6ds3_tap_cfg_t; - -#define LSM6DS3_TAP_THS_6D 0x59U -typedef struct { - uint8_t tap_ths : 5; - uint8_t sixd_ths : 2; - uint8_t d4d_en : 1; -} lsm6ds3_tap_ths_6d_t; - -#define LSM6DS3_INT_DUR2 0x5AU -typedef struct { - uint8_t shock : 2; - uint8_t quiet : 2; - uint8_t dur : 4; -} lsm6ds3_int_dur2_t; - -#define LSM6DS3_WAKE_UP_THS 0x5BU -typedef struct { - uint8_t wk_ths : 6; - uint8_t inactivity : 1; - uint8_t single_double_tap : 1; -} lsm6ds3_wake_up_ths_t; - -#define LSM6DS3_WAKE_UP_DUR 0x5CU -typedef struct { - uint8_t sleep_dur : 4; - uint8_t timer_hr : 1; - uint8_t wake_dur : 2; - uint8_t ff_dur : 1; -} lsm6ds3_wake_up_dur_t; - -#define LSM6DS3_FREE_FALL 0x5DU -typedef struct { - uint8_t ff_ths : 3; - uint8_t ff_dur : 5; -} lsm6ds3_free_fall_t; - -#define LSM6DS3_MD1_CFG 0x5EU -typedef struct { - uint8_t int1_timer : 1; - uint8_t int1_tilt : 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_inact_state : 1; -} lsm6ds3_md1_cfg_t; - -#define LSM6DS3_MD2_CFG 0x5FU -typedef struct { - uint8_t int2_iron : 1; - uint8_t int2_tilt : 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_inact_state : 1; -} lsm6ds3_md2_cfg_t; - -#define LSM6DS3_OUT_MAG_RAW_X_L 0x66U -#define LSM6DS3_OUT_MAG_RAW_X_H 0x67U -#define LSM6DS3_OUT_MAG_RAW_Y_L 0x68U -#define LSM6DS3_OUT_MAG_RAW_Y_H 0x69U -#define LSM6DS3_OUT_MAG_RAW_Z_L 0x6AU -#define LSM6DS3_OUT_MAG_RAW_Z_H 0x6BU -#define LSM6DS3_SLV0_ADD 0x02U -typedef struct { - uint8_t rw_0 : 1; - uint8_t slave0_add : 7; -} lsm6ds3_slv0_add_t; - -#define LSM6DS3_SLV0_SUBADD 0x03U -typedef struct { - uint8_t slave0_reg : 8; -} lsm6ds3_slv0_subadd_t; - -#define LSM6DS3_SLAVE0_CONFIG 0x04U -typedef struct { - uint8_t slave0_numop : 3; - uint8_t src_mode : 1; - uint8_t aux_sens_on : 2; - uint8_t slave0_rate : 2; -} lsm6ds3_slave0_config_t; - -#define LSM6DS3_SLV1_ADD 0x05U -typedef struct { - uint8_t r_1 : 1; - uint8_t slave1_add : 7; -} lsm6ds3_slv1_add_t; - -#define LSM6DS3_SLV1_SUBADD 0x06U -typedef struct { - uint8_t slave1_reg : 8; -} lsm6ds3_slv1_subadd_t; - -#define LSM6DS3_SLAVE1_CONFIG 0x07U -typedef struct { - uint8_t slave1_numop : 3; - uint8_t not_used_01 : 3; - uint8_t slave1_rate : 2; -} lsm6ds3_slave1_config_t; - -#define LSM6DS3_SLV2_ADD 0x08U -typedef struct { - uint8_t r_2 : 1; - uint8_t slave2_add : 7; -} lsm6ds3_slv2_add_t; - -#define LSM6DS3_SLV2_SUBADD 0x09U -typedef struct { - uint8_t slave2_reg : 8; -} lsm6ds3_slv2_subadd_t; - -#define LSM6DS3_SLAVE2_CONFIG 0x0AU -typedef struct { - uint8_t slave2_numop : 3; - uint8_t not_used_01 : 3; - uint8_t slave2_rate : 2; -} lsm6ds3_slave2_config_t; - -#define LSM6DS3_SLV3_ADD 0x0BU -typedef struct { - uint8_t r_3 : 1; - uint8_t slave3_add : 7; -} lsm6ds3_slv3_add_t; - -#define LSM6DS3_SLV3_SUBADD 0x0CU -typedef struct { - uint8_t slave3_reg : 8; -} lsm6ds3_slv3_subadd_t; - -#define LSM6DS3_SLAVE3_CONFIG 0x0DU -typedef struct { - uint8_t slave3_numop : 3; - uint8_t not_used_01 : 3; - uint8_t slave3_rate : 2; -} lsm6ds3_slave3_config_t; - -#define LSM6DS3_DATAWRITE_SRC_MODE_SUB_SLV0 0x0EU -typedef struct { - uint8_t slave_dataw : 8; -} lsm6ds3_datawrite_src_mode_sub_slv0_t; - -#define LSM6DS3_PEDO_THS_REG 0x0FU -typedef struct { - uint8_t ths_min : 5; - uint8_t not_used_01 : 2; - uint8_t pedo_4g : 1; -} lsm6ds3_pedo_ths_reg_t; - -#define LSM6DS3_SM_THS 0x13U -typedef struct { - uint8_t sm_ths : 8; -} lsm6ds3_sm_ths_t; - -#define LSM6DS3_PEDO_DEB_REG 0x14U -typedef struct { - uint8_t deb_step : 3; - uint8_t deb_time : 5; -} lsm6ds3_pedo_deb_reg_t; - -#define LSM6DS3_STEP_COUNT_DELTA 0x15U -typedef struct { - uint8_t sc_delta : 8; -} lsm6ds3_step_count_delta_t; - -#define LSM6DS3_MAG_SI_XX 0x24U -#define LSM6DS3_MAG_SI_XY 0x25U -#define LSM6DS3_MAG_SI_XZ 0x26U -#define LSM6DS3_MAG_SI_YX 0x27U -#define LSM6DS3_MAG_SI_YY 0x28U -#define LSM6DS3_MAG_SI_YZ 0x29U -#define LSM6DS3_MAG_SI_ZX 0x2AU -#define LSM6DS3_MAG_SI_ZY 0x2BU -#define LSM6DS3_MAG_SI_ZZ 0x2CU -#define LSM6DS3_MAG_OFFX_L 0x2DU -#define LSM6DS3_MAG_OFFX_H 0x2EU -#define LSM6DS3_MAG_OFFY_L 0x2FU -#define LSM6DS3_MAG_OFFY_H 0x30U -#define LSM6DS3_MAG_OFFZ_L 0x31U -#define LSM6DS3_MAG_OFFZ_H 0x32U - -/** - * @defgroup LSM6DS3_Register_Union - * @brief This union group all the registers that has a bit-field - * description. - * This union is useful but not need by the driver. - * - * REMOVING this union you are compliant with: - * MISRA-C 2012 [Rule 19.2] -> " Union are not allowed " - * - * @{ - * - */ - -typedef union { - lsm6ds3_func_cfg_access_t func_cfg_access; - lsm6ds3_sensor_sync_time_frame_t sensor_sync_time_frame; - lsm6ds3_fifo_ctrl1_t fifo_ctrl1; - lsm6ds3_fifo_ctrl2_t fifo_ctrl2; - lsm6ds3_fifo_ctrl3_t fifo_ctrl3; - lsm6ds3_fifo_ctrl4_t fifo_ctrl4; - lsm6ds3_fifo_ctrl5_t fifo_ctrl5; - lsm6ds3_orient_cfg_g_t orient_cfg_g; - lsm6ds3_int1_ctrl_t int1_ctrl; - lsm6ds3_int2_ctrl_t int2_ctrl; - lsm6ds3_ctrl1_xl_t ctrl1_xl; - lsm6ds3_ctrl2_g_t ctrl2_g; - lsm6ds3_ctrl3_c_t ctrl3_c; - lsm6ds3_ctrl4_c_t ctrl4_c; - lsm6ds3_ctrl5_c_t ctrl5_c; - lsm6ds3_ctrl6_c_t ctrl6_c; - lsm6ds3_ctrl7_g_t ctrl7_g; - lsm6ds3_ctrl8_xl_t ctrl8_xl; - lsm6ds3_ctrl9_xl_t ctrl9_xl; - lsm6ds3_ctrl10_c_t ctrl10_c; - lsm6ds3_master_config_t master_config; - lsm6ds3_wake_up_src_t wake_up_src; - lsm6ds3_tap_src_t tap_src; - lsm6ds3_d6d_src_t d6d_src; - lsm6ds3_status_reg_t status_reg; - lsm6ds3_sensorhub1_reg_t sensorhub1_reg; - lsm6ds3_sensorhub2_reg_t sensorhub2_reg; - lsm6ds3_sensorhub3_reg_t sensorhub3_reg; - lsm6ds3_sensorhub4_reg_t sensorhub4_reg; - lsm6ds3_sensorhub5_reg_t sensorhub5_reg; - lsm6ds3_sensorhub6_reg_t sensorhub6_reg; - lsm6ds3_sensorhub7_reg_t sensorhub7_reg; - lsm6ds3_sensorhub8_reg_t sensorhub8_reg; - lsm6ds3_sensorhub9_reg_t sensorhub9_reg; - lsm6ds3_sensorhub10_reg_t sensorhub10_reg; - lsm6ds3_sensorhub11_reg_t sensorhub11_reg; - lsm6ds3_sensorhub12_reg_t sensorhub12_reg; - lsm6ds3_fifo_status1_t fifo_status1; - lsm6ds3_fifo_status2_t fifo_status2; - lsm6ds3_fifo_status3_t fifo_status3; - lsm6ds3_fifo_status4_t fifo_status4; - lsm6ds3_sensorhub13_reg_t sensorhub13_reg; - lsm6ds3_sensorhub14_reg_t sensorhub14_reg; - lsm6ds3_sensorhub15_reg_t sensorhub15_reg; - lsm6ds3_sensorhub16_reg_t sensorhub16_reg; - lsm6ds3_sensorhub17_reg_t sensorhub17_reg; - lsm6ds3_sensorhub18_reg_t sensorhub18_reg; - lsm6ds3_func_src_t func_src; - lsm6ds3_tap_cfg_t tap_cfg; - lsm6ds3_tap_ths_6d_t tap_ths_6d; - lsm6ds3_int_dur2_t int_dur2; - lsm6ds3_wake_up_ths_t wake_up_ths; - lsm6ds3_wake_up_dur_t wake_up_dur; - lsm6ds3_free_fall_t free_fall; - lsm6ds3_md1_cfg_t md1_cfg; - lsm6ds3_md2_cfg_t md2_cfg; - lsm6ds3_slv0_add_t slv0_add; - lsm6ds3_slv0_subadd_t slv0_subadd; - lsm6ds3_slave0_config_t slave0_config; - lsm6ds3_slv1_add_t slv1_add; - lsm6ds3_slv1_subadd_t slv1_subadd; - lsm6ds3_slave1_config_t slave1_config; - lsm6ds3_slv2_add_t slv2_add; - lsm6ds3_slv2_subadd_t slv2_subadd; - lsm6ds3_slave2_config_t slave2_config; - lsm6ds3_slv3_add_t slv3_add; - lsm6ds3_slv3_subadd_t slv3_subadd; - lsm6ds3_slave3_config_t slave3_config; - lsm6ds3_datawrite_src_mode_sub_slv0_t datawrite_src_mode_sub_slv0; - lsm6ds3_pedo_ths_reg_t pedo_ths_reg; - lsm6ds3_sm_ths_t sm_ths; - lsm6ds3_pedo_deb_reg_t pedo_deb_reg; - lsm6ds3_step_count_delta_t step_count_delta; - bitwise_t bitwise; - uint8_t byte; -} lsm6ds3_reg_t; - -/** - * @} - * - */ - -int32_t lsm6ds3_read_reg(stmdev_ctx_t* ctx, uint8_t reg, uint8_t* data, uint16_t len); - -int32_t lsm6ds3_write_reg(stmdev_ctx_t* ctx, uint8_t reg, uint8_t* data, uint16_t len); - -extern float_t lsm6ds3_from_fs2g_to_mg(int16_t lsb); - -extern float_t lsm6ds3_from_fs4g_to_mg(int16_t lsb); - -extern float_t lsm6ds3_from_fs8g_to_mg(int16_t lsb); - -extern float_t lsm6ds3_from_fs16g_to_mg(int16_t lsb); - -extern float_t lsm6ds3_from_fs125dps_to_mdps(int16_t lsb); - -extern float_t lsm6ds3_from_fs250dps_to_mdps(int16_t lsb); - -extern float_t lsm6ds3_from_fs500dps_to_mdps(int16_t lsb); - -extern float_t lsm6ds3_from_fs1000dps_to_mdps(int16_t lsb); - -extern float_t lsm6ds3_from_fs2000dps_to_mdps(int16_t lsb); - -extern float_t lsm6ds3_from_lsb_to_celsius(int16_t lsb); - -typedef enum { - LSM6DS3_GY_ORIENT_XYZ = 0, - LSM6DS3_GY_ORIENT_XZY = 1, - LSM6DS3_GY_ORIENT_YXZ = 2, - LSM6DS3_GY_ORIENT_YZX = 3, - LSM6DS3_GY_ORIENT_ZXY = 4, - LSM6DS3_GY_ORIENT_ZYX = 5, -} lsm6ds3_gy_orient_t; -int32_t lsm6ds3_gy_data_orient_set(stmdev_ctx_t* ctx, lsm6ds3_gy_orient_t val); - -int32_t lsm6ds3_gy_data_orient_get(stmdev_ctx_t* ctx, lsm6ds3_gy_orient_t* val); - -typedef enum { - LSM6DS3_GY_SIGN_PPP = 0, - LSM6DS3_GY_SIGN_PPN = 1, - LSM6DS3_GY_SIGN_PNP = 2, - LSM6DS3_GY_SIGN_NPP = 4, - LSM6DS3_GY_SIGN_NNP = 6, - LSM6DS3_GY_SIGN_NPN = 5, - LSM6DS3_GY_SIGN_PNN = 3, - LSM6DS3_GY_SIGN_NNN = 7, -} lsm6ds3_gy_sgn_t; -int32_t lsm6ds3_gy_data_sign_set(stmdev_ctx_t* ctx, lsm6ds3_gy_sgn_t val); - -int32_t lsm6ds3_gy_data_sign_get(stmdev_ctx_t* ctx, lsm6ds3_gy_sgn_t* val); - -typedef enum { - LSM6DS3_2g = 0, - LSM6DS3_16g = 1, - LSM6DS3_4g = 2, - LSM6DS3_8g = 3, -} lsm6ds3_xl_fs_t; -int32_t lsm6ds3_xl_full_scale_set(stmdev_ctx_t* ctx, lsm6ds3_xl_fs_t val); - -int32_t lsm6ds3_xl_full_scale_get(stmdev_ctx_t* ctx, lsm6ds3_xl_fs_t* val); - -typedef enum { - LSM6DS3_XL_ODR_OFF = 0, - LSM6DS3_XL_ODR_12Hz5 = 1, - LSM6DS3_XL_ODR_26Hz = 2, - LSM6DS3_XL_ODR_52Hz = 3, - LSM6DS3_XL_ODR_104Hz = 4, - LSM6DS3_XL_ODR_208Hz = 5, - LSM6DS3_XL_ODR_416Hz = 6, - LSM6DS3_XL_ODR_833Hz = 7, - LSM6DS3_XL_ODR_1k66Hz = 8, - LSM6DS3_XL_ODR_3k33Hz = 9, - LSM6DS3_XL_ODR_6k66Hz = 10, -} lsm6ds3_odr_xl_t; -int32_t lsm6ds3_xl_data_rate_set(stmdev_ctx_t* ctx, lsm6ds3_odr_xl_t val); - -int32_t lsm6ds3_xl_data_rate_get(stmdev_ctx_t* ctx, lsm6ds3_odr_xl_t* val); - -typedef enum { - LSM6DS3_250dps = 0, - LSM6DS3_125dps = 1, - LSM6DS3_500dps = 2, - LSM6DS3_1000dps = 4, - LSM6DS3_2000dps = 6, -} lsm6ds3_fs_g_t; -int32_t lsm6ds3_gy_full_scale_set(stmdev_ctx_t* ctx, lsm6ds3_fs_g_t val); - -int32_t lsm6ds3_gy_full_scale_get(stmdev_ctx_t* ctx, lsm6ds3_fs_g_t* val); - -typedef enum { - LSM6DS3_GY_ODR_OFF = 0, - LSM6DS3_GY_ODR_12Hz5 = 1, - LSM6DS3_GY_ODR_26Hz = 2, - LSM6DS3_GY_ODR_52Hz = 3, - LSM6DS3_GY_ODR_104Hz = 4, - LSM6DS3_GY_ODR_208Hz = 5, - LSM6DS3_GY_ODR_416Hz = 6, - LSM6DS3_GY_ODR_833Hz = 7, - LSM6DS3_GY_ODR_1k66Hz = 8, -} lsm6ds3_odr_g_t; -int32_t lsm6ds3_gy_data_rate_set(stmdev_ctx_t* ctx, lsm6ds3_odr_g_t val); - -int32_t lsm6ds3_gy_data_rate_get(stmdev_ctx_t* ctx, lsm6ds3_odr_g_t* val); - -int32_t lsm6ds3_block_data_update_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_block_data_update_get(stmdev_ctx_t* ctx, uint8_t* val); - -typedef enum { - LSM6DS3_XL_HIGH_PERFORMANCE = 0, - LSM6DS3_XL_NORMAL = 1, -} lsm6ds3_xl_hm_mode_t; -int32_t lsm6ds3_xl_power_mode_set(stmdev_ctx_t* ctx, lsm6ds3_xl_hm_mode_t val); - -int32_t lsm6ds3_xl_power_mode_get(stmdev_ctx_t* ctx, lsm6ds3_xl_hm_mode_t* val); - -typedef enum { - LSM6DS3_STAT_RND_DISABLE = 0, - LSM6DS3_STAT_RND_ENABLE = 1, -} lsm6ds3_rnd_stat_t; -int32_t lsm6ds3_rounding_on_status_set(stmdev_ctx_t* ctx, lsm6ds3_rnd_stat_t val); - -int32_t lsm6ds3_rounding_on_status_get(stmdev_ctx_t* ctx, lsm6ds3_rnd_stat_t* val); - -typedef enum { - LSM6DS3_GY_HIGH_PERFORMANCE = 0, - LSM6DS3_GY_NORMAL = 1, -} lsm6ds3_g_hm_mode_t; -int32_t lsm6ds3_gy_power_mode_set(stmdev_ctx_t* ctx, lsm6ds3_g_hm_mode_t val); - -int32_t lsm6ds3_gy_power_mode_get(stmdev_ctx_t* ctx, lsm6ds3_g_hm_mode_t* val); - -int32_t lsm6ds3_xl_axis_x_data_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_xl_axis_x_data_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_xl_axis_y_data_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_xl_axis_y_data_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_xl_axis_z_data_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_xl_axis_z_data_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_gy_axis_x_data_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_gy_axis_x_data_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_gy_axis_y_data_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_gy_axis_y_data_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_gy_axis_z_data_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_gy_axis_z_data_get(stmdev_ctx_t* ctx, uint8_t* val); - -typedef struct { - lsm6ds3_wake_up_src_t wake_up_src; - lsm6ds3_tap_src_t tap_src; - lsm6ds3_d6d_src_t d6d_src; - lsm6ds3_func_src_t func_src; -} lsm6ds3_all_src_t; -int32_t lsm6ds3_all_sources_get(stmdev_ctx_t* ctx, lsm6ds3_all_src_t* val); - -int32_t lsm6ds3_status_reg_get(stmdev_ctx_t* ctx, lsm6ds3_status_reg_t* val); - -int32_t lsm6ds3_xl_flag_data_ready_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_gy_flag_data_ready_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_temp_flag_data_ready_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_timestamp_raw_get(stmdev_ctx_t* ctx, uint8_t* buff); - -int32_t lsm6ds3_timestamp_rst_set(stmdev_ctx_t* ctx); - -int32_t lsm6ds3_timestamp_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_timestamp_get(stmdev_ctx_t* ctx, uint8_t* val); - -typedef enum { - LSM6DS3_LSB_6ms4 = 0, - LSM6DS3_LSB_25us = 1, -} lsm6ds3_ts_res_t; -int32_t lsm6ds3_timestamp_res_set(stmdev_ctx_t* ctx, lsm6ds3_ts_res_t val); - -int32_t lsm6ds3_timestamp_res_get(stmdev_ctx_t* ctx, lsm6ds3_ts_res_t* val); - -typedef enum { - LSM6DS3_ROUND_DISABLE = 0, - LSM6DS3_ROUND_XL = 1, - LSM6DS3_ROUND_GY = 2, - LSM6DS3_ROUND_GY_XL = 3, - LSM6DS3_ROUND_SH1_TO_SH6 = 4, - LSM6DS3_ROUND_XL_SH1_TO_SH6 = 5, - LSM6DS3_ROUND_GY_XL_SH1_TO_SH12 = 6, - LSM6DS3_ROUND_GY_XL_SH1_TO_SH6 = 7, -} lsm6ds3_rounding_t; -int32_t lsm6ds3_rounding_mode_set(stmdev_ctx_t* ctx, lsm6ds3_rounding_t val); - -int32_t lsm6ds3_rounding_mode_get(stmdev_ctx_t* ctx, lsm6ds3_rounding_t* val); - -int32_t lsm6ds3_temperature_raw_get(stmdev_ctx_t* ctx, uint8_t* buff); - -int32_t lsm6ds3_angular_rate_raw_get(stmdev_ctx_t* ctx, uint8_t* buff); - -int32_t lsm6ds3_acceleration_raw_get(stmdev_ctx_t* ctx, uint8_t* buff); - -int32_t lsm6ds3_fifo_raw_data_get(stmdev_ctx_t* ctx, uint8_t* buffer, uint8_t len); - -int32_t lsm6ds3_number_of_steps_get(stmdev_ctx_t* ctx, uint8_t* buff); - -int32_t lsm6ds3_mag_calibrated_raw_get(stmdev_ctx_t* ctx, uint8_t* buff); - -typedef enum { - LSM6DS3_USER_BANK = 0, - LSM6DS3_EMBEDDED_FUNC_BANK = 1, -} lsm6ds3_func_cfg_en_t; -int32_t lsm6ds3_mem_bank_set(stmdev_ctx_t* ctx, lsm6ds3_func_cfg_en_t val); - -int32_t lsm6ds3_mem_bank_get(stmdev_ctx_t* ctx, lsm6ds3_func_cfg_en_t* val); - -int32_t lsm6ds3_device_id_get(stmdev_ctx_t* ctx, uint8_t* buff); - -int32_t lsm6ds3_reset_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_reset_get(stmdev_ctx_t* ctx, uint8_t* val); - -typedef enum { - LSM6DS3_LSB_AT_LOW_ADD = 0, - LSM6DS3_MSB_AT_LOW_ADD = 1, -} lsm6ds3_ble_t; -int32_t lsm6ds3_data_format_set(stmdev_ctx_t* ctx, lsm6ds3_ble_t val); - -int32_t lsm6ds3_data_format_get(stmdev_ctx_t* ctx, lsm6ds3_ble_t* val); - -int32_t lsm6ds3_auto_increment_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_auto_increment_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_boot_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_boot_get(stmdev_ctx_t* ctx, uint8_t* val); - -typedef enum { - LSM6DS3_XL_ST_DISABLE = 0, - LSM6DS3_XL_ST_POSITIVE = 1, - LSM6DS3_XL_ST_NEGATIVE = 2, -} lsm6ds3_st_xl_t; -int32_t lsm6ds3_xl_self_test_set(stmdev_ctx_t* ctx, lsm6ds3_st_xl_t val); - -int32_t lsm6ds3_xl_self_test_get(stmdev_ctx_t* ctx, lsm6ds3_st_xl_t* val); - -typedef enum { - LSM6DS3_GY_ST_DISABLE = 0, - LSM6DS3_GY_ST_POSITIVE = 1, - LSM6DS3_GY_ST_NEGATIVE = 3, -} lsm6ds3_st_g_t; -int32_t lsm6ds3_gy_self_test_set(stmdev_ctx_t* ctx, lsm6ds3_st_g_t val); - -int32_t lsm6ds3_gy_self_test_get(stmdev_ctx_t* ctx, lsm6ds3_st_g_t* val); - -int32_t lsm6ds3_filter_settling_mask_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_filter_settling_mask_get(stmdev_ctx_t* ctx, uint8_t* val); - -typedef enum { - LSM6DS3_HP_CUT_OFF_8mHz1 = 0, - LSM6DS3_HP_CUT_OFF_32mHz4 = 1, - LSM6DS3_HP_CUT_OFF_2Hz07 = 2, - LSM6DS3_HP_CUT_OFF_16Hz32 = 3, -} lsm6ds3_hpcf_g_t; -int32_t lsm6ds3_gy_hp_bandwidth_set(stmdev_ctx_t* ctx, lsm6ds3_hpcf_g_t val); - -int32_t lsm6ds3_gy_hp_bandwidth_get(stmdev_ctx_t* ctx, lsm6ds3_hpcf_g_t* val); - -int32_t lsm6ds3_gy_hp_reset_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_gy_hp_reset_get(stmdev_ctx_t* ctx, uint8_t* val); - -typedef enum { - LSM6DS3_XL_HP_ODR_DIV_4 = 0, - LSM6DS3_XL_HP_ODR_DIV_100 = 1, - LSM6DS3_XL_HP_ODR_DIV_9 = 2, - LSM6DS3_XL_HP_ODR_DIV_400 = 3, -} lsm6ds3_hp_bw_t; -int32_t lsm6ds3_xl_hp_bandwidth_set(stmdev_ctx_t* ctx, lsm6ds3_hp_bw_t val); - -int32_t lsm6ds3_xl_hp_bandwidth_get(stmdev_ctx_t* ctx, lsm6ds3_hp_bw_t* val); - -typedef enum { - LSM6DS3_XL_LP_ODR_DIV_50 = 0, - LSM6DS3_XL_LP_ODR_DIV_100 = 1, - LSM6DS3_XL_LP_ODR_DIV_9 = 2, - LSM6DS3_XL_LP_ODR_DIV_400 = 3, -} lsm6ds3_lp_bw_t; -int32_t lsm6ds3_xl_lp2_bandwidth_set(stmdev_ctx_t* ctx, lsm6ds3_lp_bw_t val); - -int32_t lsm6ds3_xl_lp2_bandwidth_get(stmdev_ctx_t* ctx, lsm6ds3_lp_bw_t* val); - -typedef enum { - LSM6DS3_ANTI_ALIASING_400Hz = 0, - LSM6DS3_ANTI_ALIASING_200Hz = 1, - LSM6DS3_ANTI_ALIASING_100Hz = 2, - LSM6DS3_ANTI_ALIASING_50Hz = 3, -} lsm6ds3_bw_xl_t; -int32_t lsm6ds3_xl_filter_analog_set(stmdev_ctx_t* ctx, lsm6ds3_bw_xl_t val); - -int32_t lsm6ds3_xl_filter_analog_get(stmdev_ctx_t* ctx, lsm6ds3_bw_xl_t* val); - -typedef enum { - LSM6DS3_SPI_4_WIRE = 0, - LSM6DS3_SPI_3_WIRE = 1, -} lsm6ds3_sim_t; -int32_t lsm6ds3_spi_mode_set(stmdev_ctx_t* ctx, lsm6ds3_sim_t val); - -int32_t lsm6ds3_spi_mode_get(stmdev_ctx_t* ctx, lsm6ds3_sim_t* val); - -typedef enum { - LSM6DS3_I2C_ENABLE = 0, - LSM6DS3_I2C_DISABLE = 1, -} lsm6ds3_i2c_dis_t; -int32_t lsm6ds3_i2c_interface_set(stmdev_ctx_t* ctx, lsm6ds3_i2c_dis_t val); - -int32_t lsm6ds3_i2c_interface_get(stmdev_ctx_t* ctx, lsm6ds3_i2c_dis_t* val); - -typedef struct { - uint8_t int1_drdy_xl : 1; - uint8_t int1_drdy_g : 1; - uint8_t int1_boot : 1; - uint8_t int1_fth : 1; - uint8_t int1_fifo_ovr : 1; - uint8_t int1_full_flag : 1; - uint8_t int1_sign_mot : 1; - uint8_t int1_step_detector : 1; - uint8_t int1_timer : 1; - uint8_t int1_tilt : 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_inact_state : 1; - uint8_t drdy_on_int1 : 1; -} lsm6ds3_int1_route_t; -int32_t lsm6ds3_pin_int1_route_set(stmdev_ctx_t* ctx, lsm6ds3_int1_route_t* val); - -int32_t lsm6ds3_pin_int1_route_get(stmdev_ctx_t* ctx, lsm6ds3_int1_route_t* val); - -typedef struct { - uint8_t int2_drdy_xl : 1; - uint8_t int2_drdy_g : 1; - uint8_t int2_drdy_temp : 1; - uint8_t int2_fth : 1; - uint8_t int2_fifo_ovr : 1; - uint8_t int2_full_flag : 1; - uint8_t int2_step_count_ov : 1; - uint8_t int2_step_delta : 1; - uint8_t int2_iron : 1; - uint8_t int2_tilt : 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_inact_state : 1; - uint8_t start_config : 1; -} lsm6ds3_int2_route_t; -int32_t lsm6ds3_pin_int2_route_set(stmdev_ctx_t* ctx, lsm6ds3_int2_route_t* val); - -int32_t lsm6ds3_pin_int2_route_get(stmdev_ctx_t* ctx, lsm6ds3_int2_route_t* val); - -typedef enum { - LSM6DS3_PUSH_PULL = 0, - LSM6DS3_OPEN_DRAIN = 1, -} lsm6ds3_pp_od_t; -int32_t lsm6ds3_pin_mode_set(stmdev_ctx_t* ctx, lsm6ds3_pp_od_t val); - -int32_t lsm6ds3_pin_mode_get(stmdev_ctx_t* ctx, lsm6ds3_pp_od_t* val); - -typedef enum { - LSM6DS3_ACTIVE_HIGH = 0, - LSM6DS3_ACTIVE_LOW = 1, -} lsm6ds3_pin_pol_t; -int32_t lsm6ds3_pin_polarity_set(stmdev_ctx_t* ctx, lsm6ds3_pin_pol_t val); - -int32_t lsm6ds3_pin_polarity_get(stmdev_ctx_t* ctx, lsm6ds3_pin_pol_t* val); - -int32_t lsm6ds3_all_on_int1_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_all_on_int1_get(stmdev_ctx_t* ctx, uint8_t* val); - -typedef enum { - LSM6DS3_INT_PULSED = 0, - LSM6DS3_INT_LATCHED = 1, -} lsm6ds3_lir_t; -int32_t lsm6ds3_int_notification_set(stmdev_ctx_t* ctx, lsm6ds3_lir_t val); - -int32_t lsm6ds3_int_notification_get(stmdev_ctx_t* ctx, lsm6ds3_lir_t* val); - -int32_t lsm6ds3_wkup_src_get(stmdev_ctx_t* ctx, lsm6ds3_wake_up_src_t* val); - -int32_t lsm6ds3_wkup_threshold_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_wkup_threshold_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_wkup_dur_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_wkup_dur_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_gy_sleep_mode_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_gy_sleep_mode_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_act_mode_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_act_mode_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_act_sleep_dur_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_act_sleep_dur_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_tap_src_get(stmdev_ctx_t* ctx, lsm6ds3_tap_src_t* val); - -int32_t lsm6ds3_tap_detection_on_z_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_tap_detection_on_z_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_tap_detection_on_y_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_tap_detection_on_y_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_tap_detection_on_x_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_tap_detection_on_x_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_tap_threshold_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_tap_threshold_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_tap_shock_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_tap_shock_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_tap_quiet_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_tap_quiet_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_tap_dur_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_tap_dur_get(stmdev_ctx_t* ctx, uint8_t* val); - -typedef enum { - LSM6DS3_ONLY_DOUBLE = 1, - LSM6DS3_SINGLE_DOUBLE = 0, -} lsm6ds3_tap_md_t; -int32_t lsm6ds3_tap_mode_set(stmdev_ctx_t* ctx, lsm6ds3_tap_md_t val); - -int32_t lsm6ds3_tap_mode_get(stmdev_ctx_t* ctx, lsm6ds3_tap_md_t* val); - -typedef enum { - LSM6DS3_ODR_DIV_2_FEED = 0, - LSM6DS3_LPF2_FEED = 1, -} lsm6ds3_low_pass_on_6d_t; -int32_t lsm6ds3_6d_feed_data_set(stmdev_ctx_t* ctx, lsm6ds3_low_pass_on_6d_t val); - -int32_t lsm6ds3_6d_feed_data_get(stmdev_ctx_t* ctx, lsm6ds3_low_pass_on_6d_t* val); - -int32_t lsm6ds3_6d_src_get(stmdev_ctx_t* ctx, lsm6ds3_d6d_src_t* val); - -typedef enum { - LSM6DS3_DEG_80 = 0, - LSM6DS3_DEG_70 = 1, - LSM6DS3_DEG_60 = 2, - LSM6DS3_DEG_50 = 3, -} lsm6ds3_sixd_ths_t; -int32_t lsm6ds3_6d_threshold_set(stmdev_ctx_t* ctx, lsm6ds3_sixd_ths_t val); - -int32_t lsm6ds3_6d_threshold_get(stmdev_ctx_t* ctx, lsm6ds3_sixd_ths_t* val); - -int32_t lsm6ds3_4d_mode_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_4d_mode_get(stmdev_ctx_t* ctx, uint8_t* val); - -typedef enum { - LSM6DS3_156_mg = 0, - LSM6DS3_219_mg = 1, - LSM6DS3_250_mg = 2, - LSM6DS3_312_mg = 3, - LSM6DS3_344_mg = 4, - LSM6DS3_406_mg = 5, - LSM6DS3_469_mg = 6, - LSM6DS3_500_mg = 7, -} lsm6ds3_ff_ths_t; -int32_t lsm6ds3_ff_threshold_set(stmdev_ctx_t* ctx, lsm6ds3_ff_ths_t val); - -int32_t lsm6ds3_ff_threshold_get(stmdev_ctx_t* ctx, lsm6ds3_ff_ths_t* val); - -int32_t lsm6ds3_ff_dur_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_ff_dur_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_fifo_watermark_set(stmdev_ctx_t* ctx, uint16_t val); - -int32_t lsm6ds3_fifo_watermark_get(stmdev_ctx_t* ctx, uint16_t* val); - -typedef enum { - LSM6DS3_TRG_XL_GY_DRDY = 0, - LSM6DS3_TRG_STEP_DETECT = 1, -} lsm6ds3_tmr_ped_fifo_drdy_t; -int32_t lsm6ds3_fifo_write_trigger_set(stmdev_ctx_t* ctx, lsm6ds3_tmr_ped_fifo_drdy_t val); - -int32_t lsm6ds3_fifo_write_trigger_get(stmdev_ctx_t* ctx, lsm6ds3_tmr_ped_fifo_drdy_t* val); - -int32_t lsm6ds3_fifo_pedo_batch_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_fifo_pedo_batch_get(stmdev_ctx_t* ctx, uint8_t* val); - -typedef enum { - LSM6DS3_FIFO_XL_DISABLE = 0, - LSM6DS3_FIFO_XL_NO_DEC = 1, - LSM6DS3_FIFO_XL_DEC_2 = 2, - LSM6DS3_FIFO_XL_DEC_3 = 3, - LSM6DS3_FIFO_XL_DEC_4 = 4, - LSM6DS3_FIFO_XL_DEC_8 = 5, - LSM6DS3_FIFO_XL_DEC_16 = 6, - LSM6DS3_FIFO_XL_DEC_32 = 7, -} lsm6ds3_dec_fifo_xl_t; -int32_t lsm6ds3_fifo_xl_batch_set(stmdev_ctx_t* ctx, lsm6ds3_dec_fifo_xl_t val); - -int32_t lsm6ds3_fifo_xl_batch_get(stmdev_ctx_t* ctx, lsm6ds3_dec_fifo_xl_t* val); - -typedef enum { - LSM6DS3_FIFO_GY_DISABLE = 0, - LSM6DS3_FIFO_GY_NO_DEC = 1, - LSM6DS3_FIFO_GY_DEC_2 = 2, - LSM6DS3_FIFO_GY_DEC_3 = 3, - LSM6DS3_FIFO_GY_DEC_4 = 4, - LSM6DS3_FIFO_GY_DEC_8 = 5, - LSM6DS3_FIFO_GY_DEC_16 = 6, - LSM6DS3_FIFO_GY_DEC_32 = 7, -} lsm6ds3_dec_fifo_gyro_t; -int32_t lsm6ds3_fifo_gy_batch_set(stmdev_ctx_t* ctx, lsm6ds3_dec_fifo_gyro_t val); - -int32_t lsm6ds3_fifo_gy_batch_get(stmdev_ctx_t* ctx, lsm6ds3_dec_fifo_gyro_t* val); - -typedef enum { - LSM6DS3_FIFO_DS3_DISABLE = 0, - LSM6DS3_FIFO_DS3_NO_DEC = 1, - LSM6DS3_FIFO_DS3_DEC_2 = 2, - LSM6DS3_FIFO_DS3_DEC_3 = 3, - LSM6DS3_FIFO_DS3_DEC_4 = 4, - LSM6DS3_FIFO_DS3_DEC_8 = 5, - LSM6DS3_FIFO_DS3_DEC_16 = 6, - LSM6DS3_FIFO_DS3_DEC_32 = 7, -} lsm6ds3_dec_ds3_fifo_t; -int32_t lsm6ds3_fifo_dataset_3_batch_set(stmdev_ctx_t* ctx, lsm6ds3_dec_ds3_fifo_t val); - -int32_t lsm6ds3_fifo_dataset_3_batch_get(stmdev_ctx_t* ctx, lsm6ds3_dec_ds3_fifo_t* val); - -typedef enum { - LSM6DS3_FIFO_DS4_DISABLE = 0, - LSM6DS3_FIFO_DS4_NO_DEC = 1, - LSM6DS3_FIFO_DS4_DEC_2 = 2, - LSM6DS3_FIFO_DS4_DEC_3 = 3, - LSM6DS3_FIFO_DS4_DEC_4 = 4, - LSM6DS3_FIFO_DS4_DEC_8 = 5, - LSM6DS3_FIFO_DS4_DEC_16 = 6, - LSM6DS3_FIFO_DS4_DEC_32 = 7, -} lsm6ds3_dec_ds4_fifo_t; -int32_t lsm6ds3_fifo_dataset_4_batch_set(stmdev_ctx_t* ctx, lsm6ds3_dec_ds4_fifo_t val); - -int32_t lsm6ds3_fifo_dataset_4_batch_get(stmdev_ctx_t* ctx, lsm6ds3_dec_ds4_fifo_t* val); - -int32_t lsm6ds3_fifo_xl_gy_8bit_format_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_fifo_xl_gy_8bit_format_get(stmdev_ctx_t* ctx, uint8_t* val); - -typedef enum { - LSM6DS3_BYPASS_MODE = 0, - LSM6DS3_FIFO_MODE = 1, - LSM6DS3_STREAM_TO_FIFO_MODE = 3, - LSM6DS3_BYPASS_TO_STREAM_MODE = 4, - LSM6DS3_STREAM_MODE = 6, -} lsm6ds3_fifo_md_t; -int32_t lsm6ds3_fifo_mode_set(stmdev_ctx_t* ctx, lsm6ds3_fifo_md_t val); - -int32_t lsm6ds3_fifo_mode_get(stmdev_ctx_t* ctx, lsm6ds3_fifo_md_t* val); - -typedef enum { - LSM6DS3_FIFO_DISABLE = 0, - LSM6DS3_FIFO_12Hz5 = 1, - LSM6DS3_FIFO_26Hz = 2, - LSM6DS3_FIFO_52Hz = 3, - LSM6DS3_FIFO_104Hz = 4, - LSM6DS3_FIFO_208Hz = 5, - LSM6DS3_FIFO_416Hz = 6, - LSM6DS3_FIFO_833Hz = 7, - LSM6DS3_FIFO_1k66Hz = 8, - LSM6DS3_FIFO_3k33Hz = 9, - LSM6DS3_FIFO_6k66Hz = 10, -} lsm6ds3_odr_fifo_t; -int32_t lsm6ds3_fifo_data_rate_set(stmdev_ctx_t* ctx, lsm6ds3_odr_fifo_t val); - -int32_t lsm6ds3_fifo_data_rate_get(stmdev_ctx_t* ctx, lsm6ds3_odr_fifo_t* val); - -int32_t lsm6ds3_fifo_stop_on_wtm_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_fifo_stop_on_wtm_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_fifo_temp_batch_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_fifo_temp_batch_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_fifo_data_level_get(stmdev_ctx_t* ctx, uint16_t* val); - -int32_t lsm6ds3_fifo_full_flag_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_fifo_ovr_flag_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_fifo_wtm_flag_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_fifo_pattern_get(stmdev_ctx_t* ctx, uint16_t* val); - -typedef enum { - LSM6DS3_DEN_DISABLE = 0, - LSM6DS3_LEVEL_FIFO = 6, - LSM6DS3_LEVEL_LETCHED = 3, - LSM6DS3_LEVEL_TRIGGER = 2, - LSM6DS3_EDGE_TRIGGER = 4, -} lsm6ds3_den_mode_t; -int32_t lsm6ds3_den_mode_set(stmdev_ctx_t* ctx, lsm6ds3_den_mode_t val); - -int32_t lsm6ds3_den_mode_get(stmdev_ctx_t* ctx, lsm6ds3_den_mode_t* val); - -int32_t lsm6ds3_pedo_step_reset_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_pedo_step_reset_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_pedo_timestamp_raw_get(stmdev_ctx_t* ctx, uint8_t* buff); - -int32_t lsm6ds3_pedo_step_detect_flag_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_pedo_sens_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_pedo_sens_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_pedo_threshold_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_pedo_threshold_get(stmdev_ctx_t* ctx, uint8_t* val); - -typedef enum { - LSM6DS3_PEDO_AT_2g = 0, - LSM6DS3_PEDO_AT_4g = 1, -} lsm6ds3_pedo_fs_t; -int32_t lsm6ds3_pedo_full_scale_set(stmdev_ctx_t* ctx, lsm6ds3_pedo_fs_t val); - -int32_t lsm6ds3_pedo_full_scale_get(stmdev_ctx_t* ctx, lsm6ds3_pedo_fs_t* val); - -int32_t lsm6ds3_pedo_debounce_steps_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_pedo_debounce_steps_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_pedo_timeout_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_pedo_timeout_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_motion_sens_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_motion_sens_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_motion_event_flag_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_motion_threshold_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_motion_threshold_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_sc_delta_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_sc_delta_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_tilt_event_flag_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_tilt_sens_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_tilt_sens_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_mag_soft_iron_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_mag_soft_iron_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_mag_hard_iron_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_mag_hard_iron_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_mag_soft_iron_end_op_flag_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_mag_soft_iron_coeff_set(stmdev_ctx_t* ctx, uint8_t* buff); - -int32_t lsm6ds3_mag_soft_iron_coeff_get(stmdev_ctx_t* ctx, uint8_t* buff); - -int32_t lsm6ds3_mag_offset_set(stmdev_ctx_t* ctx, uint8_t* buff); - -int32_t lsm6ds3_mag_offset_get(stmdev_ctx_t* ctx, uint8_t* buff); - -int32_t lsm6ds3_sh_sync_sens_frame_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_sh_sync_sens_frame_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_sh_master_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_sh_master_get(stmdev_ctx_t* ctx, uint8_t* val); - -int32_t lsm6ds3_sh_pass_through_set(stmdev_ctx_t* ctx, uint8_t val); - -int32_t lsm6ds3_sh_pass_through_get(stmdev_ctx_t* ctx, uint8_t* val); - -typedef enum { - LSM6DS3_EXT_PULL_UP = 0, - LSM6DS3_INTERNAL_PULL_UP = 1, -} lsm6ds3_sh_pin_md_t; -int32_t lsm6ds3_sh_pin_mode_set(stmdev_ctx_t* ctx, lsm6ds3_sh_pin_md_t val); - -int32_t lsm6ds3_sh_pin_mode_get(stmdev_ctx_t* ctx, lsm6ds3_sh_pin_md_t* val); - -typedef enum { - LSM6DS3_XL_GY_DRDY = 0, - LSM6DS3_EXT_ON_INT2_PIN = 1, -} lsm6ds3_start_cfg_t; -int32_t lsm6ds3_sh_syncro_mode_set(stmdev_ctx_t* ctx, lsm6ds3_start_cfg_t val); - -int32_t lsm6ds3_sh_syncro_mode_get(stmdev_ctx_t* ctx, lsm6ds3_start_cfg_t* val); - -typedef struct { - lsm6ds3_sensorhub1_reg_t sh_byte_1; - lsm6ds3_sensorhub2_reg_t sh_byte_2; - lsm6ds3_sensorhub3_reg_t sh_byte_3; - lsm6ds3_sensorhub4_reg_t sh_byte_4; - lsm6ds3_sensorhub5_reg_t sh_byte_5; - lsm6ds3_sensorhub6_reg_t sh_byte_6; - lsm6ds3_sensorhub7_reg_t sh_byte_7; - lsm6ds3_sensorhub8_reg_t sh_byte_8; - lsm6ds3_sensorhub9_reg_t sh_byte_9; - lsm6ds3_sensorhub10_reg_t sh_byte_10; - lsm6ds3_sensorhub11_reg_t sh_byte_11; - lsm6ds3_sensorhub12_reg_t sh_byte_12; - lsm6ds3_sensorhub13_reg_t sh_byte_13; - lsm6ds3_sensorhub14_reg_t sh_byte_14; - lsm6ds3_sensorhub15_reg_t sh_byte_15; - lsm6ds3_sensorhub16_reg_t sh_byte_16; - lsm6ds3_sensorhub17_reg_t sh_byte_17; - lsm6ds3_sensorhub18_reg_t sh_byte_18; -} lsm6ds3_sh_read_t; -int32_t lsm6ds3_sh_read_data_raw_get(stmdev_ctx_t* ctx, lsm6ds3_sh_read_t* buff); - -typedef enum { - LSM6DS3_SLV_0 = 0, - LSM6DS3_SLV_0_1 = 1, - LSM6DS3_SLV_0_1_2 = 2, - LSM6DS3_SLV_0_1_2_3 = 3, -} lsm6ds3_aux_sens_on_t; -int32_t lsm6ds3_sh_num_of_dev_connected_set(stmdev_ctx_t* ctx, lsm6ds3_aux_sens_on_t val); - -int32_t lsm6ds3_sh_num_of_dev_connected_get(stmdev_ctx_t* ctx, lsm6ds3_aux_sens_on_t* val); - -typedef struct { - uint8_t slv0_add; - uint8_t slv0_subadd; - uint8_t slv0_data; -} lsm6ds3_sh_cfg_write_t; -int32_t lsm6ds3_sh_cfg_write(stmdev_ctx_t* ctx, lsm6ds3_sh_cfg_write_t* val); - -typedef struct { - uint8_t slv_add; - uint8_t slv_subadd; - uint8_t slv_len; -} lsm6ds3_sh_cfg_read_t; -int32_t lsm6ds3_sh_slv0_cfg_read(stmdev_ctx_t* ctx, lsm6ds3_sh_cfg_read_t* val); - -int32_t lsm6ds3_sh_slv1_cfg_read(stmdev_ctx_t* ctx, lsm6ds3_sh_cfg_read_t* val); - -int32_t lsm6ds3_sh_slv2_cfg_read(stmdev_ctx_t* ctx, lsm6ds3_sh_cfg_read_t* val); - -int32_t lsm6ds3_sh_slv3_cfg_read(stmdev_ctx_t* ctx, lsm6ds3_sh_cfg_read_t* val); - -int32_t lsm6ds3_sh_end_op_flag_get(stmdev_ctx_t* ctx, uint8_t* val); - -typedef enum { - LSM6DS3_USE_SLOPE = 0, - LSM6DS3_USE_HPF = 1, -} lsm6ds3_slope_fds_t; -int32_t lsm6ds3_xl_hp_path_internal_set(stmdev_ctx_t* ctx, lsm6ds3_slope_fds_t val); - -int32_t lsm6ds3_xl_hp_path_internal_get(stmdev_ctx_t* ctx, lsm6ds3_slope_fds_t* val); - -/** - *@} - * - */ - -#ifdef __cplusplus -} -#endif - -#endif /* LSM6DS3_REGS_H */ - -/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/inc/platforms/platform.h b/inc/platforms/platform.h new file mode 100644 index 0000000..5846430 --- /dev/null +++ b/inc/platforms/platform.h @@ -0,0 +1,88 @@ +/** + * @file platform.hpp + * + * @brief This file is a declaration of the Platform class + * + * @author Bruno Machado + * @author Pedro Machado + * + * @date 08/2023 + */ + +#ifndef __PLATFORM_HPP__ +#define __PLATFORM_HPP__ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "lsm6ds_pinout.h" + +#include "i2c.h" +#include "spi.h" +#include "gpio.h" + +/***************************************** + * Public Types + *****************************************/ + +typedef int32_t (*platform_write_f)(lsm6ds_config_t* pinout_config, uint8_t reg, const uint8_t* bufp, uint16_t len); +typedef int32_t (*platform_read_f)(lsm6ds_config_t* pinout_config, uint8_t reg, uint8_t* bufp, uint16_t len); + +/***************************************** + * Public Functions Declaration + *****************************************/ + +/** + * @brief Platform write register function + * + * @param I2C_config Structure containing pinout information + * @param reg register to be written + * @param bufp buffer to be copied + * @param len length of buffer + * + * @return error code + */ +int32_t platform_write_I2C(lsm6ds_config_t* I2C_config, uint8_t reg, const uint8_t* bufp, uint16_t len); + +/** + * @brief Platform write register function + * + * @param SPI_config Structure containing pinout information + * @param reg register to be written + * @param bufp buffer to be copied + * @param len length of buffer + * + * @return error code + */ +int32_t platform_write_SPI(lsm6ds_config_t* SPI_config, uint8_t reg, const uint8_t* bufp, uint16_t len); + +/** + * @brief Platform read register function + * + * @param I2C_config Structure containing pinout information + * @param reg register to be read + * @param bufp buffer to store read value + * @param len buffer length + * + * @return error code + */ +int32_t platform_read_I2C(lsm6ds_config_t* I2C_config, uint8_t reg, uint8_t* bufp, uint16_t len); + +/** + * @brief Platform read register function + * + * @param SPI_config Structure containing pinout information + * @param reg register to be read + * @param bufp buffer to store read value + * @param len buffer length + * + * @return error code + */ +int32_t platform_read_SPI(lsm6ds_config_t* SPI_config, uint8_t reg, uint8_t* bufp, uint16_t len); + +#ifdef __cplusplus +} +#endif + +#endif //__PLATFORM_HPP__ diff --git a/inc/proxys/lsm6ds3_proxy.hpp b/inc/proxys/lsm6ds3_proxy.hpp new file mode 100644 index 0000000..be34676 --- /dev/null +++ b/inc/proxys/lsm6ds3_proxy.hpp @@ -0,0 +1,109 @@ +/** + * @file lsm6ds3.hpp + * + * @brief This file is a declaration of the LSM6DS3 class + * + * @author Bruno Machado + * @author Pedro Machado + * + * @date 08/2023 + */ + +#ifndef __LSM6DS3_PROXY_HPP__ +#define __LSM6DS3_PROXY_HPP__ + +#include "lsm6ds_proxy.hpp" +#include "lsm6ds3_reg.h" + +class LSM6DS3_Proxy : public LSM6DS_Proxy { + public: + /** + * @brief Construct a new LSM6DS3_Proxy object + * + */ + LSM6DS3_Proxy() = default; + + /** + * @brief Destroy the LSM6DS3_Proxy object + * + */ + ~LSM6DS3_Proxy() = default; + + /** + * @brief Initialize the sensor and set acc/gyro sensitivities and data rates + * + * @param lsm6ds_settings Struct of acc/gyro sensitivities and data rates settings + * @param I2C_pinout_config Struct of sensor pinout configurations in I2C mode + * @param platform_read Platform read function + * @param platform_write Platform write function + */ + int8_t init(lsm6ds_settings_t lsm6ds_settings, lsm6ds_I2C_pinout_t I2C_pinout_config, platform_read_f platform_read, platform_write_f platform_write); + + /** + * @brief Initialize the sensor and set acc/gyro sensitivities and data rates + * + * @param lsm6ds_settings Struct of acc/gyro sensitivities and data rates settings + * @param SPI_pinout_config Struct of sensor pinout configurations in SPI mode + * @param platform_read Platform read function + * @param platform_write Platform write function + * + */ + int8_t init(lsm6ds_settings_t lsm6ds_settings, lsm6ds_SPI_pinout_t SPI_pinout_config, platform_read_f platform_read, platform_write_f platform_write); + + /** + * @brief Update sensor data if available (Use this function if not using interrupt pins) + */ + void update_data(); + + /** + * @brief Update sensor data (Use this function when using interrupt pins) + */ + void update_data_ready_interrupt(); + + /** + * @brief Update sensor data (Use this function if using fifo continuous mode with interrupt pins) + */ + void update_data_fifo_full_interrupt(); + + /** + * @brief Resets fifo data + */ + void reset_fifo(); + + /** + * @brief Get a pointer to accelerometer data array in mg + */ + float* get_acc_data_mg(); + + /** + * @brief Get a pointer to gyroscope data array in mdps + */ + float* get_gyro_data_mdps(); + + private: + /** + * @brief Base six axis sensor init + * + * @return Error code + */ + int8_t base_init(); + + /** + * @brief Interrupt mode six axis sensor init + * + * @return Error code + */ + int8_t interrupt_init(); + + /** + * @brief Fifo mode six axis sensor init + * + * @return Error code + */ + int8_t fifo_mode_init(); + + private: + stmdev_ctx_t dev_ctx; +}; + +#endif // __LSM6DS3_PROXY_HPP__ \ No newline at end of file diff --git a/inc/proxys/lsm6ds3tr-c_proxy.hpp b/inc/proxys/lsm6ds3tr-c_proxy.hpp new file mode 100644 index 0000000..1adcfc7 --- /dev/null +++ b/inc/proxys/lsm6ds3tr-c_proxy.hpp @@ -0,0 +1,110 @@ +/** + * @file lsm6ds3t-c.hpp + * + * @brief This file is a declaration of the LSM6DS3 class + * + * @author Bruno Machado + * @author Pedro Machado + * + * @date 08/2023 + */ + +#ifndef __LSM6DS3TRC_PROXY_HPP__ +#define __LSM6DS3TRC_PROXY_HPP__ + +#include "lsm6ds_proxy.hpp" +#include "lsm6ds3tr-c_reg.h" + +class LSM6DS3TRC_Proxy : public LSM6DS_Proxy { + public: + /** + * @brief Construct a new LSM6DS3TRC_Proxy object + * + */ + LSM6DS3TRC_Proxy() = default; + + /** + * @brief Destroy the LSM6DS3TRC_Proxy object + * + */ + ~LSM6DS3TRC_Proxy() = default; + + /** + * @brief Initialize the sensor and set acc/gyro sensitivities and data rates + * + * @param lsm6ds_settings Struct of acc/gyro sensitivities and data rates settings + * @param I2C_pinout_config Struct of sensor pinout configurations in I2C mode + * @param platform_read Platform read function + * @param platform_write Platform write function + */ + int8_t init(lsm6ds_settings_t lsm6ds_settings, lsm6ds_I2C_pinout_t I2C_pinout_config, platform_read_f platform_read, platform_write_f platform_write); + + /** + * @brief Initialize the sensor and set acc/gyro sensitivities and data rates + * + * @param lsm6ds_settings Struct of acc/gyro sensitivities and data rates settings + * @param SPI_pinout_config Struct of sensor pinout configurations in SPI mode + * @param platform_read Platform read function + * @param platform_write Platform write function + * + */ + int8_t init(lsm6ds_settings_t lsm6ds_settings, lsm6ds_SPI_pinout_t SPI_pinout_config, platform_read_f platform_read, platform_write_f platform_write); + + /** + * @brief Update sensor data if available (Use this function if not using interrupt pins) + */ + void update_data(); + + /** + * @brief Update sensor data (Use this function when using interrupt pins) + */ + void update_data_ready_interrupt(); + + /** + * @brief Update sensor data (Use this function if using fifo continuous mode with interrupt pins) + */ + void update_data_fifo_full_interrupt(); + + /** + * @brief Resets fifo data + */ + void reset_fifo(); + + /** + * @brief Get a pointer to accelerometer data array in mg + */ + float* get_acc_data_mg(); + + /** + * @brief Get a pointer to gyroscope data array in mdps + */ + float* get_gyro_data_mdps(); + + private: + /** + * @brief Base six axis sensor init + * + * @return Error code + */ + int8_t base_init(); + + /** + * @brief Interrupt mode six axis sensor init + * + * @return Error code + */ + int8_t interrupt_init(); + + /** + * @brief Fifo mode six axis sensor init + * + * @return Error code + */ + int8_t fifo_mode_init(); + + stmdev_ctx_t dev_ctx; + lsm6ds3tr_c_int1_route_t int_1_reg; + lsm6ds3tr_c_int2_route_t int_2_reg; +}; + +#endif // __LSM6DS3TRC_PROXY_HPP__ \ No newline at end of file diff --git a/inc/proxys/lsm6ds_proxy.hpp b/inc/proxys/lsm6ds_proxy.hpp new file mode 100644 index 0000000..98286a6 --- /dev/null +++ b/inc/proxys/lsm6ds_proxy.hpp @@ -0,0 +1,90 @@ +/** + * @file lsm6ds.hpp + * + * @brief This file is a declaration of the LSM6DS class + * + * @author Bruno Machado + * @author Pedro Machado + * + * @date 11/2019 + */ + +#ifndef __LSM6DS_PROXY_HPP__ +#define __LSM6DS_PROXY_HPP__ + +#include +#include "lsm6ds_settings_type.h" +#include "lsm6ds_common.h" +#include "lsm6ds_pinout.h" +#include "platform.h" + +class LSM6DS_Proxy { + public: + /** + * @brief Initialize the sensor and set acc/gyro sensitivities and data rates + * + * @param lsm6ds_settings Struct of acc/gyro sensitivities and data rates settings + * @param I2C_pinout_config Struct of sensor pinout configurations in I2C mode + * @param platform_read Platform read function + * @param platform_write Platform write function + */ + virtual int8_t init(lsm6ds_settings_t lsm6ds_settings, lsm6ds_I2C_pinout_t I2C_pinout_config, platform_read_f platform_read, platform_write_f platform_write) = 0; + + /** + * @brief Initialize the sensor and set acc/gyro sensitivities and data rate + + * + * @param lsm6ds_settings Struct of acc/gyro sensitivities and data rates settings + * @param SPI_pinout_config Struct of sensor pinout configurations in SPI mode + * @param platform_read Platform read function + * @param platform_write Platform write function + * + */ + virtual int8_t init(lsm6ds_settings_t lsm6ds_settings, lsm6ds_SPI_pinout_t SPI_pinout_config, platform_read_f platform_read, platform_write_f platform_write) = 0; + + /** + * @brief Update sensor data if available (Use this function if not using interrupt pins) + */ + virtual void update_data() = 0; + + /** + * @brief Update sensor data (Use this function when using interrupt pins) + */ + virtual void update_data_ready_interrupt() = 0; + + /** + * @brief Update sensor data (Use this function if using fifo continuous mode with interrupt pins) + */ + virtual void update_data_fifo_full_interrupt() = 0; + + /** + * @brief Resets fifo data + */ + virtual void reset_fifo() = 0; + + /** + * @brief Get a pointer to accelerometer data array in mg + */ + virtual float* get_acc_data_mg() = 0; + + /** + * @brief Get a pointer to gyroscope data array in mdps + */ + virtual float* get_gyro_data_mdps() = 0; + + protected: + axis3bit16_t data_raw_acceleration; + axis3bit16_t data_raw_angular_rate; + float (* acc_conversion_f)(int16_t lsm6ds_xl_fs); + float (* gyro_conversion_f)(int16_t lsm6ds_fs_g); + lsm6ds_config_t pinout_config; + lsm6ds_settings_t sensor_settings; + float acceleration_mg[3]; + float angular_rate_mdps[3]; + GPIO_TypeDef* int_gpio_port_xl; + uint8_t int_gpio_pin_xl; + GPIO_TypeDef* int_gpio_port_g; + uint8_t int_gpio_pin_g; +}; + +#endif // __LSM6DS_PROXY_HPP__ diff --git a/inc/proxys/lsm6dso_proxy.hpp b/inc/proxys/lsm6dso_proxy.hpp new file mode 100644 index 0000000..af80dde --- /dev/null +++ b/inc/proxys/lsm6dso_proxy.hpp @@ -0,0 +1,110 @@ +/** + * @file lsm6dso.hpp + * + * @brief This file is a declaration of the LSM6DS3 class + * + * @author Bruno Machado + * @author Pedro Machado + * + * @date 08/2023 + */ + +#ifndef __LSM6DSO_PROXY_HPP__ +#define __LSM6DSO_PROXY_HPP__ + +#include "lsm6ds_proxy.hpp" +#include "lsm6dso_reg.h" + +class LSM6DSO_Proxy : public LSM6DS_Proxy { + public: + /** + * @brief Construct a new LSM6DS3_Proxy object + * + */ + LSM6DSO_Proxy() = default; + + /** + * @brief Destroy the LSM6DS3_Proxy object + * + */ + ~LSM6DSO_Proxy() = default; + + /** + * @brief Initialize the sensor and set acc/gyro sensitivities and data rates + * + * @param lsm6ds_settings Struct of acc/gyro sensitivities and data rates settings + * @param I2C_pinout_config Struct of sensor pinout configurations in I2C mode + * @param platform_read Platform read function + * @param platform_write Platform write function + */ + int8_t init(lsm6ds_settings_t lsm6ds_settings, lsm6ds_I2C_pinout_t I2C_pinout_config, platform_read_f platform_read, platform_write_f platform_write); + + /** + * @brief Initialize the sensor and set acc/gyro sensitivities and data rates + * + * @param lsm6ds_settings Struct of acc/gyro sensitivities and data rates settings + * @param SPI_pinout_config Struct of sensor pinout configurations in SPI mode + * @param platform_read Platform read function + * @param platform_write Platform write function + * + */ + int8_t init(lsm6ds_settings_t lsm6ds_settings, lsm6ds_SPI_pinout_t SPI_pinout_config, platform_read_f platform_read, platform_write_f platform_write); + + /** + * @brief Update sensor data if available (Use this function if not using interrupt pins) + */ + void update_data(); + + /** + * @brief Update sensor data (Use this function when using interrupt pins) + */ + void update_data_ready_interrupt(); + + /** + * @brief Update sensor data (Use this function if using fifo continuous mode with interrupt pins) + */ + void update_data_fifo_full_interrupt(); + + /** + * @brief Resets fifo data + */ + void reset_fifo(); + + /** + * @brief Get a pointer to accelerometer data array in mg + */ + float* get_acc_data_mg(); + + /** + * @brief Get a pointer to gyroscope data array in mdps + */ + float* get_gyro_data_mdps(); + + private: + /** + * @brief Base six axis sensor init + * + * @return Error code + */ + int8_t base_init(); + + /** + * @brief Interrupt mode six axis sensor init + * + * @return Error code + */ + int8_t interrupt_init(); + + /** + * @brief Fifo mode six axis sensor init + * + * @return Error code + */ + int8_t fifo_mode_init(); + + private: + stmdev_ctx_t dev_ctx; + +}; + +#endif // __LSM6DSO_PROXY_HPP__ \ No newline at end of file diff --git a/inc/reg/lsm6ds3_reg.h b/inc/reg/lsm6ds3_reg.h new file mode 100644 index 0000000..9270922 --- /dev/null +++ b/inc/reg/lsm6ds3_reg.h @@ -0,0 +1,2475 @@ +/** + ****************************************************************************** + * @file lsm6ds3_reg.h + * @author Sensors Software Solution Team + * @brief This file contains all the functions prototypes for the + * lsm6ds3_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 LSM6DS3_REGS_H +#define LSM6DS3_REGS_H + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include +#include +#include +#include "lsm6ds_pinout.h" + +/** @addtogroup LSM6DS3 + * @{ + * + */ + +/** @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) + +/** @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 (*stmdev_write_ptr)(lsm6ds_config_t*, uint8_t, const uint8_t *, uint16_t); +typedef int32_t (*stmdev_read_ptr)(lsm6ds_config_t*, uint8_t, uint8_t *, uint16_t); +typedef void (*stmdev_mdelay_ptr)(uint32_t millisec); + +typedef struct +{ + /** Component mandatory fields **/ + stmdev_write_ptr write_reg; + stmdev_read_ptr read_reg; + /** Component optional fields **/ + stmdev_mdelay_ptr mdelay; + /** Customizable optional pointer **/ + lsm6ds_config_t *handle; +} stmdev_ctx_t; + +/** + * @} + * + */ + +#endif /* MEMS_SHARED_TYPES */ + +#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 LSM6DS3_Infos + * @{ + * + */ + +/** I2C Device Address 8 bit format if SA0=0 -> 0xD5 if SA0=1 -> 0xD7 **/ +#define LSM6DS3_I2C_ADD_L 0xD5U +#define LSM6DS3_I2C_ADD_H 0xD7U +/** Device Identification (Who am I) **/ +#define LSM6DS3_ID 0x69U + +/** + * @} + * + */ + +#define LSM6DS3_FUNC_CFG_ACCESS 0x01U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_01 : 7; + uint8_t func_cfg_en : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t func_cfg_en : 1; + uint8_t not_used_01 : 7; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_func_cfg_access_t; + +#define LSM6DS3_SENSOR_SYNC_TIME_FRAME 0x04U +typedef struct +{ + uint8_t tph : 8; +} lsm6ds3_sensor_sync_time_frame_t; + +#define LSM6DS3_FIFO_CTRL1 0x06U +typedef struct +{ + uint8_t fth : 8; +} lsm6ds3_fifo_ctrl1_t; + +#define LSM6DS3_FIFO_CTRL2 0x07U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fth : 4; + uint8_t not_used_01 : 2; + uint8_t timer_pedo_fifo_drdy : 1; + uint8_t timer_pedo_fifo_en : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t timer_pedo_fifo_en : 1; + uint8_t timer_pedo_fifo_drdy : 1; + uint8_t not_used_01 : 2; + uint8_t fth : 4; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_fifo_ctrl2_t; + +#define LSM6DS3_FIFO_CTRL3 0x08U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t dec_fifo_xl : 3; + uint8_t dec_fifo_gyro : 3; + uint8_t not_used_01 : 2; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 2; + uint8_t dec_fifo_gyro : 3; + uint8_t dec_fifo_xl : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_fifo_ctrl3_t; + +#define LSM6DS3_FIFO_CTRL4 0x09U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t dec_ds3_fifo : 3; + uint8_t dec_ds4_fifo : 3; + uint8_t only_high_data : 1; + uint8_t not_used_01 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 1; + uint8_t only_high_data : 1; + uint8_t dec_ds4_fifo : 3; + uint8_t dec_ds3_fifo : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_fifo_ctrl4_t; + +#define LSM6DS3_FIFO_CTRL5 0x0AU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fifo_mode : 3; + uint8_t odr_fifo : 4; + uint8_t not_used_01 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 1; + uint8_t odr_fifo : 4; + uint8_t fifo_mode : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_fifo_ctrl5_t; + +#define LSM6DS3_ORIENT_CFG_G 0x0BU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t orient : 3; +uint8_t sign_g : + 3; /* SignX_G) + SignY_G + SignZ_G */ + uint8_t not_used_01 : 2; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 2; +uint8_t sign_g : + 3; /* SignX_G) + SignY_G + SignZ_G */ + uint8_t orient : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_orient_cfg_g_t; + +#define LSM6DS3_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 int1_boot : 1; + uint8_t int1_fth : 1; + uint8_t int1_fifo_ovr : 1; + uint8_t int1_full_flag : 1; + uint8_t int1_sign_mot : 1; + uint8_t int1_step_detector : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t int1_step_detector : 1; + uint8_t int1_sign_mot : 1; + uint8_t int1_full_flag : 1; + uint8_t int1_fifo_ovr : 1; + uint8_t int1_fth : 1; + uint8_t int1_boot : 1; + uint8_t int1_drdy_g : 1; + uint8_t int1_drdy_xl : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_int1_ctrl_t; + +#define LSM6DS3_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_temp : 1; + uint8_t int2_fth : 1; + uint8_t int2_fifo_ovr : 1; + uint8_t int2_full_flag : 1; + uint8_t int2_step_count_ov : 1; + uint8_t int2_step_delta : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t int2_step_delta : 1; + uint8_t int2_step_count_ov : 1; + uint8_t int2_full_flag : 1; + uint8_t int2_fifo_ovr : 1; + uint8_t int2_fth : 1; + uint8_t int2_drdy_temp : 1; + uint8_t int2_drdy_g : 1; + uint8_t int2_drdy_xl : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_int2_ctrl_t; + +#define LSM6DS3_WHO_AM_I 0x0FU +#define LSM6DS3_CTRL1_XL 0x10U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t bw_xl : 2; + uint8_t fs_xl : 2; + uint8_t odr_xl : 4; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t odr_xl : 4; + uint8_t fs_xl : 2; + uint8_t bw_xl : 2; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_ctrl1_xl_t; + +#define LSM6DS3_CTRL2_G 0x11U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_01 : 1; + uint8_t fs_g : 3; /* FS_G + FS_125 */ + uint8_t odr_g : 4; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t odr_g : 4; + uint8_t fs_g : 3; /* FS_G + FS_125 */ + uint8_t not_used_01 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_ctrl2_g_t; + +#define LSM6DS3_CTRL3_C 0x12U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sw_reset : 1; + uint8_t ble : 1; + uint8_t if_inc : 1; + uint8_t sim : 1; + uint8_t pp_od : 1; + uint8_t h_lactive : 1; + 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 h_lactive : 1; + uint8_t pp_od : 1; + uint8_t sim : 1; + uint8_t if_inc : 1; + uint8_t ble : 1; + uint8_t sw_reset : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_ctrl3_c_t; + +#define LSM6DS3_CTRL4_C 0x13U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t stop_on_fth : 1; + uint8_t not_used_01 : 1; + uint8_t i2c_disable : 1; + uint8_t drdy_mask : 1; + uint8_t fifo_temp_en : 1; + uint8_t int2_on_int1 : 1; + uint8_t sleep_g : 1; + uint8_t xl_bw_scal_odr : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t xl_bw_scal_odr : 1; + uint8_t sleep_g : 1; + uint8_t int2_on_int1 : 1; + uint8_t fifo_temp_en : 1; + uint8_t drdy_mask : 1; + uint8_t i2c_disable : 1; + uint8_t not_used_01 : 1; + uint8_t stop_on_fth : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_ctrl4_c_t; + +#define LSM6DS3_CTRL5_C 0x14U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t st_xl : 2; + uint8_t st_g : 2; + uint8_t not_used_01 : 1; + uint8_t rounding : 3; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t rounding : 3; + uint8_t not_used_01 : 1; + uint8_t st_g : 2; + uint8_t st_xl : 2; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_ctrl5_c_t; + +#define LSM6DS3_CTRL6_C 0x15U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_01 : 4; + uint8_t xl_hm_mode : 1; +uint8_t den_mode : + 3; /* trig_en + lvl1_en + lvl2_en */ +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN +uint8_t den_mode : + 3; /* trig_en + lvl1_en + lvl2_en */ + uint8_t xl_hm_mode : 1; + uint8_t not_used_01 : 4; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_ctrl6_c_t; + +#define LSM6DS3_CTRL7_G 0x16U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_01 : 2; + uint8_t rounding_status : 1; + uint8_t hpcf_g : 2; + uint8_t hp_g_rst : 1; + uint8_t hp_g_en : 1; + uint8_t g_hm_mode : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t g_hm_mode : 1; + uint8_t hp_g_en : 1; + uint8_t hp_g_rst : 1; + uint8_t hpcf_g : 2; + uint8_t rounding_status : 1; + uint8_t not_used_01 : 2; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_ctrl7_g_t; + +#define LSM6DS3_CTRL8_XL 0x17U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t low_pass_on_6d : 1; + uint8_t not_used_01 : 1; + uint8_t hp_slope_xl_en : 1; + uint8_t not_used_02 : 2; + uint8_t hpcf_xl : 2; + uint8_t lpf2_xl_en : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t lpf2_xl_en : 1; + uint8_t hpcf_xl : 2; + uint8_t not_used_02 : 2; + uint8_t hp_slope_xl_en : 1; + uint8_t not_used_01 : 1; + uint8_t low_pass_on_6d : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_ctrl8_xl_t; + +#define LSM6DS3_CTRL9_XL 0x18U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_01 : 2; + uint8_t soft_en : 1; + uint8_t xen_xl : 1; + uint8_t yen_xl : 1; + uint8_t zen_xl : 1; + uint8_t not_used_02 : 2; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_02 : 2; + uint8_t zen_xl : 1; + uint8_t yen_xl : 1; + uint8_t xen_xl : 1; + uint8_t soft_en : 1; + uint8_t not_used_01 : 2; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_ctrl9_xl_t; + +#define LSM6DS3_CTRL10_C 0x19U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sign_motion_en : 1; + uint8_t pedo_rst_step : 1; + uint8_t func_en : 1; + uint8_t xen_g : 1; + uint8_t yen_g : 1; + uint8_t zen_g : 1; + uint8_t not_used_01 : 2; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 2; + uint8_t zen_g : 1; + uint8_t yen_g : 1; + uint8_t xen_g : 1; + uint8_t func_en : 1; + uint8_t pedo_rst_step : 1; + uint8_t sign_motion_en : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_ctrl10_c_t; + +#define LSM6DS3_MASTER_CONFIG 0x1AU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t master_on : 1; + uint8_t iron_en : 1; + uint8_t pass_through_mode : 1; + uint8_t pull_up_en : 1; + uint8_t start_config : 1; + uint8_t not_used_01 : 1; + uint8_t data_valid_sel_fifo : 1; + uint8_t drdy_on_int1 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t drdy_on_int1 : 1; + uint8_t data_valid_sel_fifo : 1; + uint8_t not_used_01 : 1; + uint8_t start_config : 1; + uint8_t pull_up_en : 1; + uint8_t pass_through_mode : 1; + uint8_t iron_en : 1; + uint8_t master_on : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_master_config_t; + +#define LSM6DS3_WAKE_UP_SRC 0x1BU +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_ia : 1; + uint8_t ff_ia : 1; + uint8_t not_used_01 : 2; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 2; + uint8_t ff_ia : 1; + uint8_t sleep_state_ia : 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 */ +} lsm6ds3_wake_up_src_t; + +#define LSM6DS3_TAP_SRC 0x1CU +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_used_01 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 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 */ +} lsm6ds3_tap_src_t; + +#define LSM6DS3_D6D_SRC 0x1DU +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_used_01 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 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 */ +} lsm6ds3_d6d_src_t; + +#define LSM6DS3_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 not_used_01 : 5; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 5; + uint8_t tda : 1; + uint8_t gda : 1; + uint8_t xlda : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_status_reg_t; + +#define LSM6DS3_OUT_TEMP_L 0x20U +#define LSM6DS3_OUT_TEMP_H 0x21U +#define LSM6DS3_OUTX_L_G 0x22U +#define LSM6DS3_OUTX_H_G 0x23U +#define LSM6DS3_OUTY_L_G 0x24U +#define LSM6DS3_OUTY_H_G 0x25U +#define LSM6DS3_OUTZ_L_G 0x26U +#define LSM6DS3_OUTZ_H_G 0x27U +#define LSM6DS3_OUTX_L_XL 0x28U +#define LSM6DS3_OUTX_H_XL 0x29U +#define LSM6DS3_OUTY_L_XL 0x2AU +#define LSM6DS3_OUTY_H_XL 0x2BU +#define LSM6DS3_OUTZ_L_XL 0x2CU +#define LSM6DS3_OUTZ_H_XL 0x2DU +#define LSM6DS3_SENSORHUB1_REG 0x2EU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t shub1_0 : 1; + uint8_t shub1_1 : 1; + uint8_t shub1_2 : 1; + uint8_t shub1_3 : 1; + uint8_t shub1_4 : 1; + uint8_t shub1_5 : 1; + uint8_t shub1_6 : 1; + uint8_t shub1_7 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t shub1_7 : 1; + uint8_t shub1_6 : 1; + uint8_t shub1_5 : 1; + uint8_t shub1_4 : 1; + uint8_t shub1_3 : 1; + uint8_t shub1_2 : 1; + uint8_t shub1_1 : 1; + uint8_t shub1_0 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_sensorhub1_reg_t; + +#define LSM6DS3_SENSORHUB2_REG 0x2FU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t shub2_0 : 1; + uint8_t shub2_1 : 1; + uint8_t shub2_2 : 1; + uint8_t shub2_3 : 1; + uint8_t shub2_4 : 1; + uint8_t shub2_5 : 1; + uint8_t shub2_6 : 1; + uint8_t shub2_7 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t shub2_7 : 1; + uint8_t shub2_6 : 1; + uint8_t shub2_5 : 1; + uint8_t shub2_4 : 1; + uint8_t shub2_3 : 1; + uint8_t shub2_2 : 1; + uint8_t shub2_1 : 1; + uint8_t shub2_0 : 1; +#endif /* DRV_BYTE_ORDER */ + +} lsm6ds3_sensorhub2_reg_t; + +#define LSM6DS3_SENSORHUB3_REG 0x30U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t shub3_0 : 1; + uint8_t shub3_1 : 1; + uint8_t shub3_2 : 1; + uint8_t shub3_3 : 1; + uint8_t shub3_4 : 1; + uint8_t shub3_5 : 1; + uint8_t shub3_6 : 1; + uint8_t shub3_7 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t shub3_7 : 1; + uint8_t shub3_6 : 1; + uint8_t shub3_5 : 1; + uint8_t shub3_4 : 1; + uint8_t shub3_3 : 1; + uint8_t shub3_2 : 1; + uint8_t shub3_1 : 1; + uint8_t shub3_0 : 1; +#endif /* DRV_BYTE_ORDER */ + +} lsm6ds3_sensorhub3_reg_t; + +#define LSM6DS3_SENSORHUB4_REG 0x31U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t shub4_0 : 1; + uint8_t shub4_1 : 1; + uint8_t shub4_2 : 1; + uint8_t shub4_3 : 1; + uint8_t shub4_4 : 1; + uint8_t shub4_5 : 1; + uint8_t shub4_6 : 1; + uint8_t shub4_7 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t shub4_7 : 1; + uint8_t shub4_6 : 1; + uint8_t shub4_5 : 1; + uint8_t shub4_4 : 1; + uint8_t shub4_3 : 1; + uint8_t shub4_2 : 1; + uint8_t shub4_1 : 1; + uint8_t shub4_0 : 1; +#endif /* DRV_BYTE_ORDER */ + +} lsm6ds3_sensorhub4_reg_t; + +#define LSM6DS3_SENSORHUB5_REG 0x32U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t shub5_0 : 1; + uint8_t shub5_1 : 1; + uint8_t shub5_2 : 1; + uint8_t shub5_3 : 1; + uint8_t shub5_4 : 1; + uint8_t shub5_5 : 1; + uint8_t shub5_6 : 1; + uint8_t shub5_7 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t shub5_7 : 1; + uint8_t shub5_6 : 1; + uint8_t shub5_5 : 1; + uint8_t shub5_4 : 1; + uint8_t shub5_3 : 1; + uint8_t shub5_2 : 1; + uint8_t shub5_1 : 1; + uint8_t shub5_0 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_sensorhub5_reg_t; + +#define LSM6DS3_SENSORHUB6_REG 0x33U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t shub6_0 : 1; + uint8_t shub6_1 : 1; + uint8_t shub6_2 : 1; + uint8_t shub6_3 : 1; + uint8_t shub6_4 : 1; + uint8_t shub6_5 : 1; + uint8_t shub6_6 : 1; + uint8_t shub6_7 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t shub6_7 : 1; + uint8_t shub6_6 : 1; + uint8_t shub6_5 : 1; + uint8_t shub6_4 : 1; + uint8_t shub6_3 : 1; + uint8_t shub6_2 : 1; + uint8_t shub6_1 : 1; + uint8_t shub6_0 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_sensorhub6_reg_t; + +#define LSM6DS3_SENSORHUB7_REG 0x34U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t shub7_0 : 1; + uint8_t shub7_1 : 1; + uint8_t shub7_2 : 1; + uint8_t shub7_3 : 1; + uint8_t shub7_4 : 1; + uint8_t shub7_5 : 1; + uint8_t shub7_6 : 1; + uint8_t shub7_7 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t shub7_7 : 1; + uint8_t shub7_6 : 1; + uint8_t shub7_5 : 1; + uint8_t shub7_4 : 1; + uint8_t shub7_3 : 1; + uint8_t shub7_2 : 1; + uint8_t shub7_1 : 1; + uint8_t shub7_0 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_sensorhub7_reg_t; + +#define LSM6DS3_SENSORHUB8_REG 0x35U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t shub8_0 : 1; + uint8_t shub8_1 : 1; + uint8_t shub8_2 : 1; + uint8_t shub8_3 : 1; + uint8_t shub8_4 : 1; + uint8_t shub8_5 : 1; + uint8_t shub8_6 : 1; + uint8_t shub8_7 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t shub8_7 : 1; + uint8_t shub8_6 : 1; + uint8_t shub8_5 : 1; + uint8_t shub8_4 : 1; + uint8_t shub8_3 : 1; + uint8_t shub8_2 : 1; + uint8_t shub8_1 : 1; + uint8_t shub8_0 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_sensorhub8_reg_t; + +#define LSM6DS3_SENSORHUB9_REG 0x36U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t shub9_0 : 1; + uint8_t shub9_1 : 1; + uint8_t shub9_2 : 1; + uint8_t shub9_3 : 1; + uint8_t shub9_4 : 1; + uint8_t shub9_5 : 1; + uint8_t shub9_6 : 1; + uint8_t shub9_7 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t shub9_7 : 1; + uint8_t shub9_6 : 1; + uint8_t shub9_5 : 1; + uint8_t shub9_4 : 1; + uint8_t shub9_3 : 1; + uint8_t shub9_2 : 1; + uint8_t shub9_1 : 1; + uint8_t shub9_0 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_sensorhub9_reg_t; + +#define LSM6DS3_SENSORHUB10_REG 0x37U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t shub10_0 : 1; + uint8_t shub10_1 : 1; + uint8_t shub10_2 : 1; + uint8_t shub10_3 : 1; + uint8_t shub10_4 : 1; + uint8_t shub10_5 : 1; + uint8_t shub10_6 : 1; + uint8_t shub10_7 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t shub10_7 : 1; + uint8_t shub10_6 : 1; + uint8_t shub10_5 : 1; + uint8_t shub10_4 : 1; + uint8_t shub10_3 : 1; + uint8_t shub10_2 : 1; + uint8_t shub10_1 : 1; + uint8_t shub10_0 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_sensorhub10_reg_t; + +#define LSM6DS3_SENSORHUB11_REG 0x38U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t shub11_0 : 1; + uint8_t shub11_1 : 1; + uint8_t shub11_2 : 1; + uint8_t shub11_3 : 1; + uint8_t shub11_4 : 1; + uint8_t shub11_5 : 1; + uint8_t shub11_6 : 1; + uint8_t shub11_7 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t shub11_7 : 1; + uint8_t shub11_6 : 1; + uint8_t shub11_5 : 1; + uint8_t shub11_4 : 1; + uint8_t shub11_3 : 1; + uint8_t shub11_2 : 1; + uint8_t shub11_1 : 1; + uint8_t shub11_0 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_sensorhub11_reg_t; + +#define LSM6DS3_SENSORHUB12_REG 0x39U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t shub12_0 : 1; + uint8_t shub12_1 : 1; + uint8_t shub12_2 : 1; + uint8_t shub12_3 : 1; + uint8_t shub12_4 : 1; + uint8_t shub12_5 : 1; + uint8_t shub12_6 : 1; + uint8_t shub12_7 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t shub12_7 : 1; + uint8_t shub12_6 : 1; + uint8_t shub12_5 : 1; + uint8_t shub12_4 : 1; + uint8_t shub12_3 : 1; + uint8_t shub12_2 : 1; + uint8_t shub12_1 : 1; + uint8_t shub12_0 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_sensorhub12_reg_t; + +#define LSM6DS3_FIFO_STATUS1 0x3AU +typedef struct +{ + uint8_t diff_fifo : 8; +} lsm6ds3_fifo_status1_t; + +#define LSM6DS3_FIFO_STATUS2 0x3BU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t diff_fifo : 4; + uint8_t fifo_empty : 1; + uint8_t fifo_full : 1; + uint8_t fifo_over_run : 1; + uint8_t fth : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fth : 1; + uint8_t fifo_over_run : 1; + uint8_t fifo_full : 1; + uint8_t fifo_empty : 1; + uint8_t diff_fifo : 4; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_fifo_status2_t; + +#define LSM6DS3_FIFO_STATUS3 0x3CU +typedef struct +{ + uint8_t fifo_pattern : 8; +} lsm6ds3_fifo_status3_t; + +#define LSM6DS3_FIFO_STATUS4 0x3DU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fifo_pattern : 2; + uint8_t not_used_01 : 6; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 6; + uint8_t fifo_pattern : 2; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_fifo_status4_t; + +#define LSM6DS3_FIFO_DATA_OUT_L 0x3EU +#define LSM6DS3_FIFO_DATA_OUT_H 0x3FU +#define LSM6DS3_TIMESTAMP0_REG 0x40U +#define LSM6DS3_TIMESTAMP1_REG 0x41U +#define LSM6DS3_TIMESTAMP2_REG 0x42U +#define LSM6DS3_STEP_TIMESTAMP_L 0x49U +#define LSM6DS3_STEP_TIMESTAMP_H 0x4AU +#define LSM6DS3_STEP_COUNTER_L 0x4BU +#define LSM6DS3_STEP_COUNTER_H 0x4CU +#define LSM6DS3_SENSORHUB13_REG 0x4DU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t shub13_0 : 1; + uint8_t shub13_1 : 1; + uint8_t shub13_2 : 1; + uint8_t shub13_3 : 1; + uint8_t shub13_4 : 1; + uint8_t shub13_5 : 1; + uint8_t shub13_6 : 1; + uint8_t shub13_7 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t shub13_7 : 1; + uint8_t shub13_6 : 1; + uint8_t shub13_5 : 1; + uint8_t shub13_4 : 1; + uint8_t shub13_3 : 1; + uint8_t shub13_2 : 1; + uint8_t shub13_1 : 1; + uint8_t shub13_0 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_sensorhub13_reg_t; + +#define LSM6DS3_SENSORHUB14_REG 0x4EU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t shub14_0 : 1; + uint8_t shub14_1 : 1; + uint8_t shub14_2 : 1; + uint8_t shub14_3 : 1; + uint8_t shub14_4 : 1; + uint8_t shub14_5 : 1; + uint8_t shub14_6 : 1; + uint8_t shub14_7 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t shub14_7 : 1; + uint8_t shub14_6 : 1; + uint8_t shub14_5 : 1; + uint8_t shub14_4 : 1; + uint8_t shub14_3 : 1; + uint8_t shub14_2 : 1; + uint8_t shub14_1 : 1; + uint8_t shub14_0 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_sensorhub14_reg_t; + +#define LSM6DS3_SENSORHUB15_REG 0x4FU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t shub15_0 : 1; + uint8_t shub15_1 : 1; + uint8_t shub15_2 : 1; + uint8_t shub15_3 : 1; + uint8_t shub15_4 : 1; + uint8_t shub15_5 : 1; + uint8_t shub15_6 : 1; + uint8_t shub15_7 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t shub15_7 : 1; + uint8_t shub15_6 : 1; + uint8_t shub15_5 : 1; + uint8_t shub15_4 : 1; + uint8_t shub15_3 : 1; + uint8_t shub15_2 : 1; + uint8_t shub15_1 : 1; + uint8_t shub15_0 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_sensorhub15_reg_t; + +#define LSM6DS3_SENSORHUB16_REG 0x50U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t shub16_0 : 1; + uint8_t shub16_1 : 1; + uint8_t shub16_2 : 1; + uint8_t shub16_3 : 1; + uint8_t shub16_4 : 1; + uint8_t shub16_5 : 1; + uint8_t shub16_6 : 1; + uint8_t shub16_7 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t shub16_7 : 1; + uint8_t shub16_6 : 1; + uint8_t shub16_5 : 1; + uint8_t shub16_4 : 1; + uint8_t shub16_3 : 1; + uint8_t shub16_2 : 1; + uint8_t shub16_1 : 1; + uint8_t shub16_0 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_sensorhub16_reg_t; + +#define LSM6DS3_SENSORHUB17_REG 0x51U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t shub17_0 : 1; + uint8_t shub17_1 : 1; + uint8_t shub17_2 : 1; + uint8_t shub17_3 : 1; + uint8_t shub17_4 : 1; + uint8_t shub17_5 : 1; + uint8_t shub17_6 : 1; + uint8_t shub17_7 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t shub17_7 : 1; + uint8_t shub17_6 : 1; + uint8_t shub17_5 : 1; + uint8_t shub17_4 : 1; + uint8_t shub17_3 : 1; + uint8_t shub17_2 : 1; + uint8_t shub17_1 : 1; + uint8_t shub17_0 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_sensorhub17_reg_t; + +#define LSM6DS3_SENSORHUB18_REG 0x52U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t shub18_0 : 1; + uint8_t shub18_1 : 1; + uint8_t shub18_2 : 1; + uint8_t shub18_3 : 1; + uint8_t shub18_4 : 1; + uint8_t shub18_5 : 1; + uint8_t shub18_6 : 1; + uint8_t shub18_7 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t shub18_7 : 1; + uint8_t shub18_6 : 1; + uint8_t shub18_5 : 1; + uint8_t shub18_4 : 1; + uint8_t shub18_3 : 1; + uint8_t shub18_2 : 1; + uint8_t shub18_1 : 1; + uint8_t shub18_0 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_sensorhub18_reg_t; + +#define LSM6DS3_FUNC_SRC 0x53U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sensor_hub_end_op : 1; + uint8_t si_end_op : 1; + uint8_t not_used_01 : 1; + uint8_t step_overflow : 1; + uint8_t step_detected : 1; + uint8_t tilt_ia : 1; + uint8_t sign_motion_ia : 1; + uint8_t step_count_delta_ia : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t step_count_delta_ia : 1; + uint8_t sign_motion_ia : 1; + uint8_t tilt_ia : 1; + uint8_t step_detected : 1; + uint8_t step_overflow : 1; + uint8_t not_used_01 : 1; + uint8_t si_end_op : 1; + uint8_t sensor_hub_end_op : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_func_src_t; + +#define LSM6DS3_TAP_CFG 0x58U +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 tilt_en : 1; + uint8_t pedo_en : 1; + uint8_t timer_en : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t timer_en : 1; + uint8_t pedo_en : 1; + uint8_t tilt_en : 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 */ +} lsm6ds3_tap_cfg_t; + +#define LSM6DS3_TAP_THS_6D 0x59U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t tap_ths : 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 : 5; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_tap_ths_6d_t; + +#define LSM6DS3_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 */ +} lsm6ds3_int_dur2_t; + +#define LSM6DS3_WAKE_UP_THS 0x5BU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t wk_ths : 6; + uint8_t inactivity : 1; + uint8_t single_double_tap : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t single_double_tap : 1; + uint8_t inactivity : 1; + uint8_t wk_ths : 6; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_wake_up_ths_t; + +#define LSM6DS3_WAKE_UP_DUR 0x5CU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sleep_dur : 4; + uint8_t timer_hr : 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 timer_hr : 1; + uint8_t sleep_dur : 4; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_wake_up_dur_t; + +#define LSM6DS3_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 */ +} lsm6ds3_free_fall_t; + +#define LSM6DS3_MD1_CFG 0x5EU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t int1_timer : 1; + uint8_t int1_tilt : 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_inact_state : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t int1_inact_state : 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_tilt : 1; + uint8_t int1_timer : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_md1_cfg_t; + +#define LSM6DS3_MD2_CFG 0x5FU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t int2_iron : 1; + uint8_t int2_tilt : 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_inact_state : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t int2_inact_state : 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_tilt : 1; + uint8_t int2_iron : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_md2_cfg_t; + +#define LSM6DS3_OUT_MAG_RAW_X_L 0x66U +#define LSM6DS3_OUT_MAG_RAW_X_H 0x67U +#define LSM6DS3_OUT_MAG_RAW_Y_L 0x68U +#define LSM6DS3_OUT_MAG_RAW_Y_H 0x69U +#define LSM6DS3_OUT_MAG_RAW_Z_L 0x6AU +#define LSM6DS3_OUT_MAG_RAW_Z_H 0x6BU +#define LSM6DS3_SLV0_ADD 0x02U +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 */ +} lsm6ds3_slv0_add_t; + +#define LSM6DS3_SLV0_SUBADD 0x03U +typedef struct +{ + uint8_t slave0_reg : 8; +} lsm6ds3_slv0_subadd_t; + +#define LSM6DS3_SLAVE0_CONFIG 0x04U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t slave0_numop : 3; + uint8_t src_mode : 1; + uint8_t aux_sens_on : 2; + uint8_t slave0_rate : 2; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t slave0_rate : 2; + uint8_t aux_sens_on : 2; + uint8_t src_mode : 1; + uint8_t slave0_numop : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_slave0_config_t; + +#define LSM6DS3_SLV1_ADD 0x05U +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 */ +} lsm6ds3_slv1_add_t; + +#define LSM6DS3_SLV1_SUBADD 0x06U +typedef struct +{ + uint8_t slave1_reg : 8; +} lsm6ds3_slv1_subadd_t; + +#define LSM6DS3_SLAVE1_CONFIG 0x07U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t slave1_numop : 3; + uint8_t not_used_01 : 3; + uint8_t slave1_rate : 2; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t slave1_rate : 2; + uint8_t not_used_01 : 3; + uint8_t slave1_numop : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_slave1_config_t; + +#define LSM6DS3_SLV2_ADD 0x08U +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 */ +} lsm6ds3_slv2_add_t; + +#define LSM6DS3_SLV2_SUBADD 0x09U +typedef struct +{ + uint8_t slave2_reg : 8; +} lsm6ds3_slv2_subadd_t; + +#define LSM6DS3_SLAVE2_CONFIG 0x0AU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t slave2_numop : 3; + uint8_t not_used_01 : 3; + uint8_t slave2_rate : 2; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t slave2_rate : 2; + uint8_t not_used_01 : 3; + uint8_t slave2_numop : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_slave2_config_t; + +#define LSM6DS3_SLV3_ADD 0x0BU +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 */ +} lsm6ds3_slv3_add_t; + +#define LSM6DS3_SLV3_SUBADD 0x0CU +typedef struct +{ + uint8_t slave3_reg : 8; +} lsm6ds3_slv3_subadd_t; + +#define LSM6DS3_SLAVE3_CONFIG 0x0DU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t slave3_numop : 3; + uint8_t not_used_01 : 3; + uint8_t slave3_rate : 2; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t slave3_rate : 2; + uint8_t not_used_01 : 3; + uint8_t slave3_numop : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_slave3_config_t; + +#define LSM6DS3_DATAWRITE_SRC_MODE_SUB_SLV0 0x0EU +typedef struct +{ + uint8_t slave_dataw : 8; +} lsm6ds3_datawrite_src_mode_sub_slv0_t; + +#define LSM6DS3_PEDO_THS_REG 0x0FU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ths_min : 5; + uint8_t not_used_01 : 2; + uint8_t pedo_4g : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t pedo_4g : 1; + uint8_t not_used_01 : 2; + uint8_t ths_min : 5; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_pedo_ths_reg_t; + +#define LSM6DS3_SM_THS 0x13U +typedef struct +{ + uint8_t sm_ths : 8; +} lsm6ds3_sm_ths_t; + +#define LSM6DS3_PEDO_DEB_REG 0x14U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t deb_step : 3; + uint8_t deb_time : 5; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t deb_time : 5; + uint8_t deb_step : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3_pedo_deb_reg_t; + +#define LSM6DS3_STEP_COUNT_DELTA 0x15U +typedef struct +{ + uint8_t sc_delta : 8; +} lsm6ds3_step_count_delta_t; + +#define LSM6DS3_MAG_SI_XX 0x24U +#define LSM6DS3_MAG_SI_XY 0x25U +#define LSM6DS3_MAG_SI_XZ 0x26U +#define LSM6DS3_MAG_SI_YX 0x27U +#define LSM6DS3_MAG_SI_YY 0x28U +#define LSM6DS3_MAG_SI_YZ 0x29U +#define LSM6DS3_MAG_SI_ZX 0x2AU +#define LSM6DS3_MAG_SI_ZY 0x2BU +#define LSM6DS3_MAG_SI_ZZ 0x2CU +#define LSM6DS3_MAG_OFFX_L 0x2DU +#define LSM6DS3_MAG_OFFX_H 0x2EU +#define LSM6DS3_MAG_OFFY_L 0x2FU +#define LSM6DS3_MAG_OFFY_H 0x30U +#define LSM6DS3_MAG_OFFZ_L 0x31U +#define LSM6DS3_MAG_OFFZ_H 0x32U + +/** + * @defgroup LSM6DS3_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 +{ + lsm6ds3_func_cfg_access_t func_cfg_access; + lsm6ds3_sensor_sync_time_frame_t sensor_sync_time_frame; + lsm6ds3_fifo_ctrl1_t fifo_ctrl1; + lsm6ds3_fifo_ctrl2_t fifo_ctrl2; + lsm6ds3_fifo_ctrl3_t fifo_ctrl3; + lsm6ds3_fifo_ctrl4_t fifo_ctrl4; + lsm6ds3_fifo_ctrl5_t fifo_ctrl5; + lsm6ds3_orient_cfg_g_t orient_cfg_g; + lsm6ds3_int1_ctrl_t int1_ctrl; + lsm6ds3_int2_ctrl_t int2_ctrl; + lsm6ds3_ctrl1_xl_t ctrl1_xl; + lsm6ds3_ctrl2_g_t ctrl2_g; + lsm6ds3_ctrl3_c_t ctrl3_c; + lsm6ds3_ctrl4_c_t ctrl4_c; + lsm6ds3_ctrl5_c_t ctrl5_c; + lsm6ds3_ctrl6_c_t ctrl6_c; + lsm6ds3_ctrl7_g_t ctrl7_g; + lsm6ds3_ctrl8_xl_t ctrl8_xl; + lsm6ds3_ctrl9_xl_t ctrl9_xl; + lsm6ds3_ctrl10_c_t ctrl10_c; + lsm6ds3_master_config_t master_config; + lsm6ds3_wake_up_src_t wake_up_src; + lsm6ds3_tap_src_t tap_src; + lsm6ds3_d6d_src_t d6d_src; + lsm6ds3_status_reg_t status_reg; + lsm6ds3_sensorhub1_reg_t sensorhub1_reg; + lsm6ds3_sensorhub2_reg_t sensorhub2_reg; + lsm6ds3_sensorhub3_reg_t sensorhub3_reg; + lsm6ds3_sensorhub4_reg_t sensorhub4_reg; + lsm6ds3_sensorhub5_reg_t sensorhub5_reg; + lsm6ds3_sensorhub6_reg_t sensorhub6_reg; + lsm6ds3_sensorhub7_reg_t sensorhub7_reg; + lsm6ds3_sensorhub8_reg_t sensorhub8_reg; + lsm6ds3_sensorhub9_reg_t sensorhub9_reg; + lsm6ds3_sensorhub10_reg_t sensorhub10_reg; + lsm6ds3_sensorhub11_reg_t sensorhub11_reg; + lsm6ds3_sensorhub12_reg_t sensorhub12_reg; + lsm6ds3_fifo_status1_t fifo_status1; + lsm6ds3_fifo_status2_t fifo_status2; + lsm6ds3_fifo_status3_t fifo_status3; + lsm6ds3_fifo_status4_t fifo_status4; + lsm6ds3_sensorhub13_reg_t sensorhub13_reg; + lsm6ds3_sensorhub14_reg_t sensorhub14_reg; + lsm6ds3_sensorhub15_reg_t sensorhub15_reg; + lsm6ds3_sensorhub16_reg_t sensorhub16_reg; + lsm6ds3_sensorhub17_reg_t sensorhub17_reg; + lsm6ds3_sensorhub18_reg_t sensorhub18_reg; + lsm6ds3_func_src_t func_src; + lsm6ds3_tap_cfg_t tap_cfg; + lsm6ds3_tap_ths_6d_t tap_ths_6d; + lsm6ds3_int_dur2_t int_dur2; + lsm6ds3_wake_up_ths_t wake_up_ths; + lsm6ds3_wake_up_dur_t wake_up_dur; + lsm6ds3_free_fall_t free_fall; + lsm6ds3_md1_cfg_t md1_cfg; + lsm6ds3_md2_cfg_t md2_cfg; + lsm6ds3_slv0_add_t slv0_add; + lsm6ds3_slv0_subadd_t slv0_subadd; + lsm6ds3_slave0_config_t slave0_config; + lsm6ds3_slv1_add_t slv1_add; + lsm6ds3_slv1_subadd_t slv1_subadd; + lsm6ds3_slave1_config_t slave1_config; + lsm6ds3_slv2_add_t slv2_add; + lsm6ds3_slv2_subadd_t slv2_subadd; + lsm6ds3_slave2_config_t slave2_config; + lsm6ds3_slv3_add_t slv3_add; + lsm6ds3_slv3_subadd_t slv3_subadd; + lsm6ds3_slave3_config_t slave3_config; + lsm6ds3_datawrite_src_mode_sub_slv0_t + datawrite_src_mode_sub_slv0; + lsm6ds3_pedo_ths_reg_t pedo_ths_reg; + lsm6ds3_sm_ths_t sm_ths; + lsm6ds3_pedo_deb_reg_t pedo_deb_reg; + lsm6ds3_step_count_delta_t step_count_delta; + bitwise_t bitwise; + uint8_t byte; +} lsm6ds3_reg_t; + +/** + * @} + * + */ + +#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 lsm6ds3_read_reg(stmdev_ctx_t *ctx, uint8_t reg, + uint8_t *data, + uint16_t len); +int32_t lsm6ds3_write_reg(stmdev_ctx_t *ctx, uint8_t reg, + uint8_t *data, + uint16_t len); + +float_t lsm6ds3_from_fs2g_to_mg(int16_t lsb); +float_t lsm6ds3_from_fs4g_to_mg(int16_t lsb); +float_t lsm6ds3_from_fs8g_to_mg(int16_t lsb); +float_t lsm6ds3_from_fs16g_to_mg(int16_t lsb); + +float_t lsm6ds3_from_fs125dps_to_mdps(int16_t lsb); +float_t lsm6ds3_from_fs250dps_to_mdps(int16_t lsb); +float_t lsm6ds3_from_fs500dps_to_mdps(int16_t lsb); +float_t lsm6ds3_from_fs1000dps_to_mdps(int16_t lsb); +float_t lsm6ds3_from_fs2000dps_to_mdps(int16_t lsb); + +float_t lsm6ds3_from_lsb_to_celsius(int16_t lsb); + +typedef enum +{ + LSM6DS3_GY_ORIENT_XYZ = 0, + LSM6DS3_GY_ORIENT_XZY = 1, + LSM6DS3_GY_ORIENT_YXZ = 2, + LSM6DS3_GY_ORIENT_YZX = 3, + LSM6DS3_GY_ORIENT_ZXY = 4, + LSM6DS3_GY_ORIENT_ZYX = 5, +} lsm6ds3_gy_orient_t; +int32_t lsm6ds3_gy_data_orient_set(stmdev_ctx_t *ctx, + lsm6ds3_gy_orient_t val); +int32_t lsm6ds3_gy_data_orient_get(stmdev_ctx_t *ctx, + lsm6ds3_gy_orient_t *val); + +typedef enum +{ + LSM6DS3_GY_SIGN_PPP = 0, + LSM6DS3_GY_SIGN_PPN = 1, + LSM6DS3_GY_SIGN_PNP = 2, + LSM6DS3_GY_SIGN_NPP = 4, + LSM6DS3_GY_SIGN_NNP = 6, + LSM6DS3_GY_SIGN_NPN = 5, + LSM6DS3_GY_SIGN_PNN = 3, + LSM6DS3_GY_SIGN_NNN = 7, +} lsm6ds3_gy_sgn_t; +int32_t lsm6ds3_gy_data_sign_set(stmdev_ctx_t *ctx, + lsm6ds3_gy_sgn_t val); +int32_t lsm6ds3_gy_data_sign_get(stmdev_ctx_t *ctx, + lsm6ds3_gy_sgn_t *val); + +typedef enum +{ + LSM6DS3_2g = 0, + LSM6DS3_16g = 1, + LSM6DS3_4g = 2, + LSM6DS3_8g = 3, +} lsm6ds3_xl_fs_t; +int32_t lsm6ds3_xl_full_scale_set(stmdev_ctx_t *ctx, + lsm6ds3_xl_fs_t val); +int32_t lsm6ds3_xl_full_scale_get(stmdev_ctx_t *ctx, + lsm6ds3_xl_fs_t *val); + +typedef enum +{ + LSM6DS3_XL_ODR_OFF = 0, + LSM6DS3_XL_ODR_12Hz5 = 1, + LSM6DS3_XL_ODR_26Hz = 2, + LSM6DS3_XL_ODR_52Hz = 3, + LSM6DS3_XL_ODR_104Hz = 4, + LSM6DS3_XL_ODR_208Hz = 5, + LSM6DS3_XL_ODR_416Hz = 6, + LSM6DS3_XL_ODR_833Hz = 7, + LSM6DS3_XL_ODR_1k66Hz = 8, + LSM6DS3_XL_ODR_3k33Hz = 9, + LSM6DS3_XL_ODR_6k66Hz = 10, +} lsm6ds3_odr_xl_t; +int32_t lsm6ds3_xl_data_rate_set(stmdev_ctx_t *ctx, + lsm6ds3_odr_xl_t val); +int32_t lsm6ds3_xl_data_rate_get(stmdev_ctx_t *ctx, + lsm6ds3_odr_xl_t *val); + +typedef enum +{ + LSM6DS3_250dps = 0, + LSM6DS3_125dps = 1, + LSM6DS3_500dps = 2, + LSM6DS3_1000dps = 4, + LSM6DS3_2000dps = 6, +} lsm6ds3_fs_g_t; +int32_t lsm6ds3_gy_full_scale_set(stmdev_ctx_t *ctx, + lsm6ds3_fs_g_t val); +int32_t lsm6ds3_gy_full_scale_get(stmdev_ctx_t *ctx, + lsm6ds3_fs_g_t *val); + +typedef enum +{ + LSM6DS3_GY_ODR_OFF = 0, + LSM6DS3_GY_ODR_12Hz5 = 1, + LSM6DS3_GY_ODR_26Hz = 2, + LSM6DS3_GY_ODR_52Hz = 3, + LSM6DS3_GY_ODR_104Hz = 4, + LSM6DS3_GY_ODR_208Hz = 5, + LSM6DS3_GY_ODR_416Hz = 6, + LSM6DS3_GY_ODR_833Hz = 7, + LSM6DS3_GY_ODR_1k66Hz = 8, +} lsm6ds3_odr_g_t; +int32_t lsm6ds3_gy_data_rate_set(stmdev_ctx_t *ctx, + lsm6ds3_odr_g_t val); +int32_t lsm6ds3_gy_data_rate_get(stmdev_ctx_t *ctx, + lsm6ds3_odr_g_t *val); + +int32_t lsm6ds3_block_data_update_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_block_data_update_get(stmdev_ctx_t *ctx, + uint8_t *val); + +typedef enum +{ + LSM6DS3_XL_HIGH_PERFORMANCE = 0, + LSM6DS3_XL_NORMAL = 1, +} lsm6ds3_xl_hm_mode_t; +int32_t lsm6ds3_xl_power_mode_set(stmdev_ctx_t *ctx, + lsm6ds3_xl_hm_mode_t val); +int32_t lsm6ds3_xl_power_mode_get(stmdev_ctx_t *ctx, + lsm6ds3_xl_hm_mode_t *val); + +typedef enum +{ + LSM6DS3_STAT_RND_DISABLE = 0, + LSM6DS3_STAT_RND_ENABLE = 1, +} lsm6ds3_rnd_stat_t; +int32_t lsm6ds3_rounding_on_status_set(stmdev_ctx_t *ctx, + lsm6ds3_rnd_stat_t val); +int32_t lsm6ds3_rounding_on_status_get(stmdev_ctx_t *ctx, + lsm6ds3_rnd_stat_t *val); + +typedef enum +{ + LSM6DS3_GY_HIGH_PERFORMANCE = 0, + LSM6DS3_GY_NORMAL = 1, +} lsm6ds3_g_hm_mode_t; +int32_t lsm6ds3_gy_power_mode_set(stmdev_ctx_t *ctx, + lsm6ds3_g_hm_mode_t val); +int32_t lsm6ds3_gy_power_mode_get(stmdev_ctx_t *ctx, + lsm6ds3_g_hm_mode_t *val); + +int32_t lsm6ds3_xl_axis_x_data_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_xl_axis_x_data_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3_xl_axis_y_data_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_xl_axis_y_data_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3_xl_axis_z_data_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_xl_axis_z_data_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3_gy_axis_x_data_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_gy_axis_x_data_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3_gy_axis_y_data_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_gy_axis_y_data_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3_gy_axis_z_data_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_gy_axis_z_data_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef struct +{ + lsm6ds3_wake_up_src_t wake_up_src; + lsm6ds3_tap_src_t tap_src; + lsm6ds3_d6d_src_t d6d_src; + lsm6ds3_func_src_t func_src; +} lsm6ds3_all_src_t; +int32_t lsm6ds3_all_sources_get(stmdev_ctx_t *ctx, + lsm6ds3_all_src_t *val); + +int32_t lsm6ds3_status_reg_get(stmdev_ctx_t *ctx, + lsm6ds3_status_reg_t *val); + +int32_t lsm6ds3_xl_flag_data_ready_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6ds3_gy_flag_data_ready_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6ds3_temp_flag_data_ready_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6ds3_timestamp_raw_get(stmdev_ctx_t *ctx, uint32_t *val); + +int32_t lsm6ds3_timestamp_rst_set(stmdev_ctx_t *ctx); + +int32_t lsm6ds3_timestamp_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_timestamp_get(stmdev_ctx_t *ctx, uint8_t *val); +typedef enum +{ + LSM6DS3_LSB_6ms4 = 0, + LSM6DS3_LSB_25us = 1, +} lsm6ds3_ts_res_t; +int32_t lsm6ds3_timestamp_res_set(stmdev_ctx_t *ctx, + lsm6ds3_ts_res_t val); +int32_t lsm6ds3_timestamp_res_get(stmdev_ctx_t *ctx, + lsm6ds3_ts_res_t *val); + +typedef enum +{ + LSM6DS3_ROUND_DISABLE = 0, + LSM6DS3_ROUND_XL = 1, + LSM6DS3_ROUND_GY = 2, + LSM6DS3_ROUND_GY_XL = 3, + LSM6DS3_ROUND_SH1_TO_SH6 = 4, + LSM6DS3_ROUND_XL_SH1_TO_SH6 = 5, + LSM6DS3_ROUND_GY_XL_SH1_TO_SH12 = 6, + LSM6DS3_ROUND_GY_XL_SH1_TO_SH6 = 7, +} lsm6ds3_rounding_t; +int32_t lsm6ds3_rounding_mode_set(stmdev_ctx_t *ctx, + lsm6ds3_rounding_t val); +int32_t lsm6ds3_rounding_mode_get(stmdev_ctx_t *ctx, + lsm6ds3_rounding_t *val); + +int32_t lsm6ds3_temperature_raw_get(stmdev_ctx_t *ctx, int16_t *val); + +int32_t lsm6ds3_angular_rate_raw_get(stmdev_ctx_t *ctx, + int16_t *val); + +int32_t lsm6ds3_acceleration_raw_get(stmdev_ctx_t *ctx, + int16_t *val); + +int32_t lsm6ds3_fifo_raw_data_get(stmdev_ctx_t *ctx, uint8_t *buffer, + uint8_t len); + +int32_t lsm6ds3_number_of_steps_get(stmdev_ctx_t *ctx, uint16_t *val); + +int32_t lsm6ds3_mag_calibrated_raw_get(stmdev_ctx_t *ctx, + int16_t *val); + +typedef enum +{ + LSM6DS3_USER_BANK = 0, + LSM6DS3_EMBEDDED_FUNC_BANK = 1, +} lsm6ds3_func_cfg_en_t; +int32_t lsm6ds3_mem_bank_set(stmdev_ctx_t *ctx, + lsm6ds3_func_cfg_en_t val); +int32_t lsm6ds3_mem_bank_get(stmdev_ctx_t *ctx, + lsm6ds3_func_cfg_en_t *val); + +int32_t lsm6ds3_device_id_get(stmdev_ctx_t *ctx, uint8_t *buff); + +int32_t lsm6ds3_reset_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_reset_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + LSM6DS3_LSB_AT_LOW_ADD = 0, + LSM6DS3_MSB_AT_LOW_ADD = 1, +} lsm6ds3_ble_t; +int32_t lsm6ds3_data_format_set(stmdev_ctx_t *ctx, lsm6ds3_ble_t val); +int32_t lsm6ds3_data_format_get(stmdev_ctx_t *ctx, + lsm6ds3_ble_t *val); + +int32_t lsm6ds3_auto_increment_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_auto_increment_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3_boot_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_boot_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + LSM6DS3_XL_ST_DISABLE = 0, + LSM6DS3_XL_ST_POSITIVE = 1, + LSM6DS3_XL_ST_NEGATIVE = 2, +} lsm6ds3_st_xl_t; +int32_t lsm6ds3_xl_self_test_set(stmdev_ctx_t *ctx, + lsm6ds3_st_xl_t val); +int32_t lsm6ds3_xl_self_test_get(stmdev_ctx_t *ctx, + lsm6ds3_st_xl_t *val); + +typedef enum +{ + LSM6DS3_GY_ST_DISABLE = 0, + LSM6DS3_GY_ST_POSITIVE = 1, + LSM6DS3_GY_ST_NEGATIVE = 3, +} lsm6ds3_st_g_t; +int32_t lsm6ds3_gy_self_test_set(stmdev_ctx_t *ctx, + lsm6ds3_st_g_t val); +int32_t lsm6ds3_gy_self_test_get(stmdev_ctx_t *ctx, + lsm6ds3_st_g_t *val); + +int32_t lsm6ds3_filter_settling_mask_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6ds3_filter_settling_mask_get(stmdev_ctx_t *ctx, + uint8_t *val); + +typedef enum +{ + LSM6DS3_HP_CUT_OFF_8mHz1 = 0, + LSM6DS3_HP_CUT_OFF_32mHz4 = 1, + LSM6DS3_HP_CUT_OFF_2Hz07 = 2, + LSM6DS3_HP_CUT_OFF_16Hz32 = 3, +} lsm6ds3_hpcf_g_t; +int32_t lsm6ds3_gy_hp_bandwidth_set(stmdev_ctx_t *ctx, + lsm6ds3_hpcf_g_t val); +int32_t lsm6ds3_gy_hp_bandwidth_get(stmdev_ctx_t *ctx, + lsm6ds3_hpcf_g_t *val); + +int32_t lsm6ds3_gy_hp_reset_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_gy_hp_reset_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + LSM6DS3_XL_HP_ODR_DIV_4 = 0, + LSM6DS3_XL_HP_ODR_DIV_100 = 1, + LSM6DS3_XL_HP_ODR_DIV_9 = 2, + LSM6DS3_XL_HP_ODR_DIV_400 = 3, +} lsm6ds3_hp_bw_t; +int32_t lsm6ds3_xl_hp_bandwidth_set(stmdev_ctx_t *ctx, + lsm6ds3_hp_bw_t val); +int32_t lsm6ds3_xl_hp_bandwidth_get(stmdev_ctx_t *ctx, + lsm6ds3_hp_bw_t *val); + +typedef enum +{ + LSM6DS3_XL_LP_ODR_DIV_50 = 0, + LSM6DS3_XL_LP_ODR_DIV_100 = 1, + LSM6DS3_XL_LP_ODR_DIV_9 = 2, + LSM6DS3_XL_LP_ODR_DIV_400 = 3, +} lsm6ds3_lp_bw_t; +int32_t lsm6ds3_xl_lp2_bandwidth_set(stmdev_ctx_t *ctx, + lsm6ds3_lp_bw_t val); +int32_t lsm6ds3_xl_lp2_bandwidth_get(stmdev_ctx_t *ctx, + lsm6ds3_lp_bw_t *val); + +typedef enum +{ + LSM6DS3_ANTI_ALIASING_400Hz = 0, + LSM6DS3_ANTI_ALIASING_200Hz = 1, + LSM6DS3_ANTI_ALIASING_100Hz = 2, + LSM6DS3_ANTI_ALIASING_50Hz = 3, +} lsm6ds3_bw_xl_t; +int32_t lsm6ds3_xl_filter_analog_set(stmdev_ctx_t *ctx, + lsm6ds3_bw_xl_t val); +int32_t lsm6ds3_xl_filter_analog_get(stmdev_ctx_t *ctx, + lsm6ds3_bw_xl_t *val); + +typedef enum +{ + LSM6DS3_SPI_4_WIRE = 0, + LSM6DS3_SPI_3_WIRE = 1, +} lsm6ds3_sim_t; +int32_t lsm6ds3_spi_mode_set(stmdev_ctx_t *ctx, lsm6ds3_sim_t val); +int32_t lsm6ds3_spi_mode_get(stmdev_ctx_t *ctx, lsm6ds3_sim_t *val); + +typedef enum +{ + LSM6DS3_I2C_ENABLE = 0, + LSM6DS3_I2C_DISABLE = 1, +} lsm6ds3_i2c_dis_t; +int32_t lsm6ds3_i2c_interface_set(stmdev_ctx_t *ctx, + lsm6ds3_i2c_dis_t val); +int32_t lsm6ds3_i2c_interface_get(stmdev_ctx_t *ctx, + lsm6ds3_i2c_dis_t *val); + +typedef struct +{ + uint8_t int1_drdy_xl : 1; + uint8_t int1_drdy_g : 1; + uint8_t int1_boot : 1; + uint8_t int1_fth : 1; + uint8_t int1_fifo_ovr : 1; + uint8_t int1_full_flag : 1; + uint8_t int1_sign_mot : 1; + uint8_t int1_step_detector : 1; + uint8_t int1_timer : 1; + uint8_t int1_tilt : 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_inact_state : 1; + uint8_t drdy_on_int1 : 1; +} lsm6ds3_int1_route_t; +int32_t lsm6ds3_pin_int1_route_set(stmdev_ctx_t *ctx, + lsm6ds3_int1_route_t *val); +int32_t lsm6ds3_pin_int1_route_get(stmdev_ctx_t *ctx, + lsm6ds3_int1_route_t *val); + +typedef struct +{ + uint8_t int2_drdy_xl : 1; + uint8_t int2_drdy_g : 1; + uint8_t int2_drdy_temp : 1; + uint8_t int2_fth : 1; + uint8_t int2_fifo_ovr : 1; + uint8_t int2_full_flag : 1; + uint8_t int2_step_count_ov : 1; + uint8_t int2_step_delta : 1; + uint8_t int2_iron : 1; + uint8_t int2_tilt : 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_inact_state : 1; + uint8_t start_config : 1; +} lsm6ds3_int2_route_t; +int32_t lsm6ds3_pin_int2_route_set(stmdev_ctx_t *ctx, + lsm6ds3_int2_route_t *val); +int32_t lsm6ds3_pin_int2_route_get(stmdev_ctx_t *ctx, + lsm6ds3_int2_route_t *val); + +typedef enum +{ + LSM6DS3_PUSH_PULL = 0, + LSM6DS3_OPEN_DRAIN = 1, +} lsm6ds3_pp_od_t; +int32_t lsm6ds3_pin_mode_set(stmdev_ctx_t *ctx, lsm6ds3_pp_od_t val); +int32_t lsm6ds3_pin_mode_get(stmdev_ctx_t *ctx, lsm6ds3_pp_od_t *val); + +typedef enum +{ + LSM6DS3_ACTIVE_HIGH = 0, + LSM6DS3_ACTIVE_LOW = 1, +} lsm6ds3_pin_pol_t; +int32_t lsm6ds3_pin_polarity_set(stmdev_ctx_t *ctx, + lsm6ds3_pin_pol_t val); +int32_t lsm6ds3_pin_polarity_get(stmdev_ctx_t *ctx, + lsm6ds3_pin_pol_t *val); + +int32_t lsm6ds3_all_on_int1_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_all_on_int1_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + LSM6DS3_INT_PULSED = 0, + LSM6DS3_INT_LATCHED = 1, +} lsm6ds3_lir_t; +int32_t lsm6ds3_int_notification_set(stmdev_ctx_t *ctx, + lsm6ds3_lir_t val); +int32_t lsm6ds3_int_notification_get(stmdev_ctx_t *ctx, + lsm6ds3_lir_t *val); + +int32_t lsm6ds3_wkup_src_get(stmdev_ctx_t *ctx, + lsm6ds3_wake_up_src_t *val); + +int32_t lsm6ds3_wkup_threshold_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_wkup_threshold_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3_wkup_dur_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_wkup_dur_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3_gy_sleep_mode_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_gy_sleep_mode_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3_act_mode_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_act_mode_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3_act_sleep_dur_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_act_sleep_dur_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3_tap_src_get(stmdev_ctx_t *ctx, + lsm6ds3_tap_src_t *val); + +int32_t lsm6ds3_tap_detection_on_z_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6ds3_tap_detection_on_z_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6ds3_tap_detection_on_y_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6ds3_tap_detection_on_y_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6ds3_tap_detection_on_x_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6ds3_tap_detection_on_x_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6ds3_tap_threshold_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_tap_threshold_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3_tap_shock_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_tap_shock_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3_tap_quiet_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_tap_quiet_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3_tap_dur_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_tap_dur_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + LSM6DS3_ONLY_DOUBLE = 1, + LSM6DS3_SINGLE_DOUBLE = 0, +} lsm6ds3_tap_md_t; +int32_t lsm6ds3_tap_mode_set(stmdev_ctx_t *ctx, lsm6ds3_tap_md_t val); +int32_t lsm6ds3_tap_mode_get(stmdev_ctx_t *ctx, + lsm6ds3_tap_md_t *val); + +typedef enum +{ + LSM6DS3_ODR_DIV_2_FEED = 0, + LSM6DS3_LPF2_FEED = 1, +} lsm6ds3_low_pass_on_6d_t; +int32_t lsm6ds3_6d_feed_data_set(stmdev_ctx_t *ctx, + lsm6ds3_low_pass_on_6d_t val); +int32_t lsm6ds3_6d_feed_data_get(stmdev_ctx_t *ctx, + lsm6ds3_low_pass_on_6d_t *val); + +int32_t lsm6ds3_6d_src_get(stmdev_ctx_t *ctx, lsm6ds3_d6d_src_t *val); + +typedef enum +{ + LSM6DS3_DEG_80 = 0, + LSM6DS3_DEG_70 = 1, + LSM6DS3_DEG_60 = 2, + LSM6DS3_DEG_50 = 3, +} lsm6ds3_sixd_ths_t; +int32_t lsm6ds3_6d_threshold_set(stmdev_ctx_t *ctx, + lsm6ds3_sixd_ths_t val); +int32_t lsm6ds3_6d_threshold_get(stmdev_ctx_t *ctx, + lsm6ds3_sixd_ths_t *val); + +int32_t lsm6ds3_4d_mode_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_4d_mode_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + LSM6DS3_156_mg = 0, + LSM6DS3_219_mg = 1, + LSM6DS3_250_mg = 2, + LSM6DS3_312_mg = 3, + LSM6DS3_344_mg = 4, + LSM6DS3_406_mg = 5, + LSM6DS3_469_mg = 6, + LSM6DS3_500_mg = 7, +} lsm6ds3_ff_ths_t; +int32_t lsm6ds3_ff_threshold_set(stmdev_ctx_t *ctx, + lsm6ds3_ff_ths_t val); +int32_t lsm6ds3_ff_threshold_get(stmdev_ctx_t *ctx, + lsm6ds3_ff_ths_t *val); + +int32_t lsm6ds3_ff_dur_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_ff_dur_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3_fifo_watermark_set(stmdev_ctx_t *ctx, uint16_t val); +int32_t lsm6ds3_fifo_watermark_get(stmdev_ctx_t *ctx, uint16_t *val); + +typedef enum +{ + LSM6DS3_TRG_XL_GY_DRDY = 0, + LSM6DS3_TRG_STEP_DETECT = 1, +} lsm6ds3_tmr_ped_fifo_drdy_t; +int32_t lsm6ds3_fifo_write_trigger_set(stmdev_ctx_t *ctx, + lsm6ds3_tmr_ped_fifo_drdy_t val); +int32_t lsm6ds3_fifo_write_trigger_get(stmdev_ctx_t *ctx, + lsm6ds3_tmr_ped_fifo_drdy_t *val); + +int32_t lsm6ds3_fifo_pedo_batch_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_fifo_pedo_batch_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + LSM6DS3_FIFO_XL_DISABLE = 0, + LSM6DS3_FIFO_XL_NO_DEC = 1, + LSM6DS3_FIFO_XL_DEC_2 = 2, + LSM6DS3_FIFO_XL_DEC_3 = 3, + LSM6DS3_FIFO_XL_DEC_4 = 4, + LSM6DS3_FIFO_XL_DEC_8 = 5, + LSM6DS3_FIFO_XL_DEC_16 = 6, + LSM6DS3_FIFO_XL_DEC_32 = 7, +} lsm6ds3_dec_fifo_xl_t; +int32_t lsm6ds3_fifo_xl_batch_set(stmdev_ctx_t *ctx, + lsm6ds3_dec_fifo_xl_t val); +int32_t lsm6ds3_fifo_xl_batch_get(stmdev_ctx_t *ctx, + lsm6ds3_dec_fifo_xl_t *val); + +typedef enum +{ + LSM6DS3_FIFO_GY_DISABLE = 0, + LSM6DS3_FIFO_GY_NO_DEC = 1, + LSM6DS3_FIFO_GY_DEC_2 = 2, + LSM6DS3_FIFO_GY_DEC_3 = 3, + LSM6DS3_FIFO_GY_DEC_4 = 4, + LSM6DS3_FIFO_GY_DEC_8 = 5, + LSM6DS3_FIFO_GY_DEC_16 = 6, + LSM6DS3_FIFO_GY_DEC_32 = 7, +} lsm6ds3_dec_fifo_gyro_t; +int32_t lsm6ds3_fifo_gy_batch_set(stmdev_ctx_t *ctx, + lsm6ds3_dec_fifo_gyro_t val); +int32_t lsm6ds3_fifo_gy_batch_get(stmdev_ctx_t *ctx, + lsm6ds3_dec_fifo_gyro_t *val); + +typedef enum +{ + LSM6DS3_FIFO_DS3_DISABLE = 0, + LSM6DS3_FIFO_DS3_NO_DEC = 1, + LSM6DS3_FIFO_DS3_DEC_2 = 2, + LSM6DS3_FIFO_DS3_DEC_3 = 3, + LSM6DS3_FIFO_DS3_DEC_4 = 4, + LSM6DS3_FIFO_DS3_DEC_8 = 5, + LSM6DS3_FIFO_DS3_DEC_16 = 6, + LSM6DS3_FIFO_DS3_DEC_32 = 7, +} lsm6ds3_dec_ds3_fifo_t; +int32_t lsm6ds3_fifo_dataset_3_batch_set(stmdev_ctx_t *ctx, + lsm6ds3_dec_ds3_fifo_t val); +int32_t lsm6ds3_fifo_dataset_3_batch_get(stmdev_ctx_t *ctx, + lsm6ds3_dec_ds3_fifo_t *val); + +typedef enum +{ + LSM6DS3_FIFO_DS4_DISABLE = 0, + LSM6DS3_FIFO_DS4_NO_DEC = 1, + LSM6DS3_FIFO_DS4_DEC_2 = 2, + LSM6DS3_FIFO_DS4_DEC_3 = 3, + LSM6DS3_FIFO_DS4_DEC_4 = 4, + LSM6DS3_FIFO_DS4_DEC_8 = 5, + LSM6DS3_FIFO_DS4_DEC_16 = 6, + LSM6DS3_FIFO_DS4_DEC_32 = 7, +} lsm6ds3_dec_ds4_fifo_t; +int32_t lsm6ds3_fifo_dataset_4_batch_set(stmdev_ctx_t *ctx, + lsm6ds3_dec_ds4_fifo_t val); +int32_t lsm6ds3_fifo_dataset_4_batch_get(stmdev_ctx_t *ctx, + lsm6ds3_dec_ds4_fifo_t *val); + +int32_t lsm6ds3_fifo_xl_gy_8bit_format_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6ds3_fifo_xl_gy_8bit_format_get(stmdev_ctx_t *ctx, + uint8_t *val); + +typedef enum +{ + LSM6DS3_BYPASS_MODE = 0, + LSM6DS3_FIFO_MODE = 1, + LSM6DS3_STREAM_TO_FIFO_MODE = 3, + LSM6DS3_BYPASS_TO_STREAM_MODE = 4, + LSM6DS3_STREAM_MODE = 6, +} lsm6ds3_fifo_md_t; +int32_t lsm6ds3_fifo_mode_set(stmdev_ctx_t *ctx, + lsm6ds3_fifo_md_t val); +int32_t lsm6ds3_fifo_mode_get(stmdev_ctx_t *ctx, + lsm6ds3_fifo_md_t *val); + +typedef enum +{ + LSM6DS3_FIFO_DISABLE = 0, + LSM6DS3_FIFO_12Hz5 = 1, + LSM6DS3_FIFO_26Hz = 2, + LSM6DS3_FIFO_52Hz = 3, + LSM6DS3_FIFO_104Hz = 4, + LSM6DS3_FIFO_208Hz = 5, + LSM6DS3_FIFO_416Hz = 6, + LSM6DS3_FIFO_833Hz = 7, + LSM6DS3_FIFO_1k66Hz = 8, + LSM6DS3_FIFO_3k33Hz = 9, + LSM6DS3_FIFO_6k66Hz = 10, +} lsm6ds3_odr_fifo_t; +int32_t lsm6ds3_fifo_data_rate_set(stmdev_ctx_t *ctx, + lsm6ds3_odr_fifo_t val); +int32_t lsm6ds3_fifo_data_rate_get(stmdev_ctx_t *ctx, + lsm6ds3_odr_fifo_t *val); + +int32_t lsm6ds3_fifo_stop_on_wtm_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_fifo_stop_on_wtm_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3_fifo_temp_batch_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_fifo_temp_batch_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3_fifo_data_level_get(stmdev_ctx_t *ctx, uint16_t *val); + +int32_t lsm6ds3_fifo_full_flag_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3_fifo_ovr_flag_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3_fifo_wtm_flag_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3_fifo_pattern_get(stmdev_ctx_t *ctx, uint16_t *val); + +typedef enum +{ + LSM6DS3_DEN_DISABLE = 0, + LSM6DS3_LEVEL_FIFO = 6, + LSM6DS3_LEVEL_LETCHED = 3, + LSM6DS3_LEVEL_TRIGGER = 2, + LSM6DS3_EDGE_TRIGGER = 4, +} lsm6ds3_den_mode_t; +int32_t lsm6ds3_den_mode_set(stmdev_ctx_t *ctx, + lsm6ds3_den_mode_t val); +int32_t lsm6ds3_den_mode_get(stmdev_ctx_t *ctx, + lsm6ds3_den_mode_t *val); + +int32_t lsm6ds3_pedo_step_reset_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_pedo_step_reset_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3_pedo_timestamp_raw_get(stmdev_ctx_t *ctx, + uint16_t *val); + +int32_t lsm6ds3_pedo_step_detect_flag_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6ds3_pedo_sens_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_pedo_sens_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3_pedo_threshold_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_pedo_threshold_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + LSM6DS3_PEDO_AT_2g = 0, + LSM6DS3_PEDO_AT_4g = 1, +} lsm6ds3_pedo_fs_t; +int32_t lsm6ds3_pedo_full_scale_set(stmdev_ctx_t *ctx, + lsm6ds3_pedo_fs_t val); +int32_t lsm6ds3_pedo_full_scale_get(stmdev_ctx_t *ctx, + lsm6ds3_pedo_fs_t *val); + +int32_t lsm6ds3_pedo_debounce_steps_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6ds3_pedo_debounce_steps_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6ds3_pedo_timeout_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_pedo_timeout_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3_motion_sens_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_motion_sens_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3_motion_event_flag_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6ds3_motion_threshold_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_motion_threshold_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3_sc_delta_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_sc_delta_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3_tilt_event_flag_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3_tilt_sens_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_tilt_sens_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3_mag_soft_iron_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_mag_soft_iron_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3_mag_hard_iron_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_mag_hard_iron_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3_mag_soft_iron_end_op_flag_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6ds3_mag_soft_iron_coeff_set(stmdev_ctx_t *ctx, + uint8_t *buff); +int32_t lsm6ds3_mag_soft_iron_coeff_get(stmdev_ctx_t *ctx, + uint8_t *buff); + +int32_t lsm6ds3_mag_offset_set(stmdev_ctx_t *ctx, int16_t *val); +int32_t lsm6ds3_mag_offset_get(stmdev_ctx_t *ctx, int16_t *val); + +int32_t lsm6ds3_sh_sync_sens_frame_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6ds3_sh_sync_sens_frame_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6ds3_sh_master_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_sh_master_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3_sh_pass_through_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3_sh_pass_through_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + LSM6DS3_EXT_PULL_UP = 0, + LSM6DS3_INTERNAL_PULL_UP = 1, +} lsm6ds3_sh_pin_md_t; +int32_t lsm6ds3_sh_pin_mode_set(stmdev_ctx_t *ctx, + lsm6ds3_sh_pin_md_t val); +int32_t lsm6ds3_sh_pin_mode_get(stmdev_ctx_t *ctx, + lsm6ds3_sh_pin_md_t *val); + +typedef enum +{ + LSM6DS3_XL_GY_DRDY = 0, + LSM6DS3_EXT_ON_INT2_PIN = 1, +} lsm6ds3_start_cfg_t; +int32_t lsm6ds3_sh_syncro_mode_set(stmdev_ctx_t *ctx, + lsm6ds3_start_cfg_t val); +int32_t lsm6ds3_sh_syncro_mode_get(stmdev_ctx_t *ctx, + lsm6ds3_start_cfg_t *val); + +typedef struct +{ + lsm6ds3_sensorhub1_reg_t sh_byte_1; + lsm6ds3_sensorhub2_reg_t sh_byte_2; + lsm6ds3_sensorhub3_reg_t sh_byte_3; + lsm6ds3_sensorhub4_reg_t sh_byte_4; + lsm6ds3_sensorhub5_reg_t sh_byte_5; + lsm6ds3_sensorhub6_reg_t sh_byte_6; + lsm6ds3_sensorhub7_reg_t sh_byte_7; + lsm6ds3_sensorhub8_reg_t sh_byte_8; + lsm6ds3_sensorhub9_reg_t sh_byte_9; + lsm6ds3_sensorhub10_reg_t sh_byte_10; + lsm6ds3_sensorhub11_reg_t sh_byte_11; + lsm6ds3_sensorhub12_reg_t sh_byte_12; + lsm6ds3_sensorhub13_reg_t sh_byte_13; + lsm6ds3_sensorhub14_reg_t sh_byte_14; + lsm6ds3_sensorhub15_reg_t sh_byte_15; + lsm6ds3_sensorhub16_reg_t sh_byte_16; + lsm6ds3_sensorhub17_reg_t sh_byte_17; + lsm6ds3_sensorhub18_reg_t sh_byte_18; +} lsm6ds3_sh_read_t; +int32_t lsm6ds3_sh_read_data_raw_get(stmdev_ctx_t *ctx, + lsm6ds3_sh_read_t *buff); + +typedef enum +{ + LSM6DS3_SLV_0 = 0, + LSM6DS3_SLV_0_1 = 1, + LSM6DS3_SLV_0_1_2 = 2, + LSM6DS3_SLV_0_1_2_3 = 3, +} lsm6ds3_aux_sens_on_t; +int32_t lsm6ds3_sh_num_of_dev_connected_set(stmdev_ctx_t *ctx, + lsm6ds3_aux_sens_on_t val); +int32_t lsm6ds3_sh_num_of_dev_connected_get(stmdev_ctx_t *ctx, + lsm6ds3_aux_sens_on_t *val); + +typedef struct +{ + uint8_t slv0_add; + uint8_t slv0_subadd; + uint8_t slv0_data; +} lsm6ds3_sh_cfg_write_t; +int32_t lsm6ds3_sh_cfg_write(stmdev_ctx_t *ctx, + lsm6ds3_sh_cfg_write_t *val); + +typedef struct +{ + uint8_t slv_add; + uint8_t slv_subadd; + uint8_t slv_len; +} lsm6ds3_sh_cfg_read_t; +int32_t lsm6ds3_sh_slv0_cfg_read(stmdev_ctx_t *ctx, + lsm6ds3_sh_cfg_read_t *val); +int32_t lsm6ds3_sh_slv1_cfg_read(stmdev_ctx_t *ctx, + lsm6ds3_sh_cfg_read_t *val); +int32_t lsm6ds3_sh_slv2_cfg_read(stmdev_ctx_t *ctx, + lsm6ds3_sh_cfg_read_t *val); +int32_t lsm6ds3_sh_slv3_cfg_read(stmdev_ctx_t *ctx, + lsm6ds3_sh_cfg_read_t *val); + +int32_t lsm6ds3_sh_end_op_flag_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + LSM6DS3_USE_SLOPE = 0, + LSM6DS3_USE_HPF = 1, +} lsm6ds3_slope_fds_t; +int32_t lsm6ds3_xl_hp_path_internal_set(stmdev_ctx_t *ctx, + lsm6ds3_slope_fds_t val); +int32_t lsm6ds3_xl_hp_path_internal_get(stmdev_ctx_t *ctx, + lsm6ds3_slope_fds_t *val); + +/** + *@} + * + */ + +#ifdef __cplusplus +} +#endif + +#endif /* LSM6DS3_REGS_H */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ \ No newline at end of file diff --git a/inc/reg/lsm6ds3tr-c_reg.h b/inc/reg/lsm6ds3tr-c_reg.h new file mode 100644 index 0000000..cef84cd --- /dev/null +++ b/inc/reg/lsm6ds3tr-c_reg.h @@ -0,0 +1,2811 @@ +/** + ****************************************************************************** + * @file lsm6ds3tr_c_reg.h + * @author Sensors Software Solution Team + * @brief This file contains all the functions prototypes for the + * lsm6ds3tr_c_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 LSM6DS3TR_C_DRIVER_H +#define LSM6DS3TR_C_DRIVER_H + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include +#include +#include +#include "lsm6ds_pinout.h" + +/** @addtogroup LSM6DS3TR_C + * @{ + * + */ + +/** @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) + +/** @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 (*stmdev_write_ptr)(lsm6ds_config_t*, uint8_t, const uint8_t *, uint16_t); +typedef int32_t (*stmdev_read_ptr)(lsm6ds_config_t*, uint8_t, uint8_t *, uint16_t); +typedef void (*stmdev_mdelay_ptr)(uint32_t millisec); + +typedef struct +{ + /** Component mandatory fields **/ + stmdev_write_ptr write_reg; + stmdev_read_ptr read_reg; + /** Component optional fields **/ + stmdev_mdelay_ptr mdelay; + /** Customizable optional pointer **/ + lsm6ds_config_t *handle; +} stmdev_ctx_t; + +/** + * @} + * + */ + +#endif /* MEMS_SHARED_TYPES */ + +#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 LSM6DS3TR_C_Infos + * @{ + * + */ + +/** I2C Device Address 8 bit format if SA0=0 -> D5 if SA0=1 -> D7 **/ +#define LSM6DS3TR_C_I2C_ADD_L 0xD5U +#define LSM6DS3TR_C_I2C_ADD_H 0xD7U + +/** Device Identification (Who am I) **/ +#define LSM6DS3TR_C_ID 0x6AU + +/** + * @} + * + */ + +#define LSM6DS3TR_C_FUNC_CFG_ACCESS 0x01U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_01 : 5; +uint8_t func_cfg_en : + 3; /* func_cfg_en + func_cfg_en_b */ +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN +uint8_t func_cfg_en : + 3; /* func_cfg_en + func_cfg_en_b */ + uint8_t not_used_01 : 5; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_func_cfg_access_t; + +#define LSM6DS3TR_C_SENSOR_SYNC_TIME_FRAME 0x04U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t tph : 4; + uint8_t not_used_01 : 4; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 4; + uint8_t tph : 4; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_sensor_sync_time_frame_t; + +#define LSM6DS3TR_C_SENSOR_SYNC_RES_RATIO 0x05U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t rr : 2; + uint8_t not_used_01 : 6; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 6; + uint8_t rr : 2; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_sensor_sync_res_ratio_t; + +#define LSM6DS3TR_C_FIFO_CTRL1 0x06U +typedef struct +{ + uint8_t fth : 8; /* + FIFO_CTRL2(fth) */ +} lsm6ds3tr_c_fifo_ctrl1_t; + +#define LSM6DS3TR_C_FIFO_CTRL2 0x07U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fth : 3; /* + FIFO_CTRL1(fth) */ + uint8_t fifo_temp_en : 1; + uint8_t not_used_01 : 2; + uint8_t timer_pedo_fifo_drdy : 1; + uint8_t timer_pedo_fifo_en : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t timer_pedo_fifo_en : 1; + uint8_t timer_pedo_fifo_drdy : 1; + uint8_t not_used_01 : 2; + uint8_t fifo_temp_en : 1; + uint8_t fth : 3; /* + FIFO_CTRL1(fth) */ +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_fifo_ctrl2_t; + +#define LSM6DS3TR_C_FIFO_CTRL3 0x08U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t dec_fifo_xl : 3; + uint8_t dec_fifo_gyro : 3; + uint8_t not_used_01 : 2; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 2; + uint8_t dec_fifo_gyro : 3; + uint8_t dec_fifo_xl : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_fifo_ctrl3_t; + +#define LSM6DS3TR_C_FIFO_CTRL4 0x09U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t dec_ds3_fifo : 3; + uint8_t dec_ds4_fifo : 3; + uint8_t only_high_data : 1; + uint8_t stop_on_fth : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t stop_on_fth : 1; + uint8_t only_high_data : 1; + uint8_t dec_ds4_fifo : 3; + uint8_t dec_ds3_fifo : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_fifo_ctrl4_t; + +#define LSM6DS3TR_C_FIFO_CTRL5 0x0AU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fifo_mode : 3; + uint8_t odr_fifo : 4; + uint8_t not_used_01 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 1; + uint8_t odr_fifo : 4; + uint8_t fifo_mode : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_fifo_ctrl5_t; + +#define LSM6DS3TR_C_DRDY_PULSE_CFG_G 0x0BU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t int2_wrist_tilt : 1; + uint8_t not_used_01 : 6; + uint8_t drdy_pulsed : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t drdy_pulsed : 1; + uint8_t not_used_01 : 6; + uint8_t int2_wrist_tilt : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_drdy_pulse_cfg_g_t; + +#define LSM6DS3TR_C_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 int1_boot : 1; + uint8_t int1_fth : 1; + uint8_t int1_fifo_ovr : 1; + uint8_t int1_full_flag : 1; + uint8_t int1_sign_mot : 1; + uint8_t int1_step_detector : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t int1_step_detector : 1; + uint8_t int1_sign_mot : 1; + uint8_t int1_full_flag : 1; + uint8_t int1_fifo_ovr : 1; + uint8_t int1_fth : 1; + uint8_t int1_boot : 1; + uint8_t int1_drdy_g : 1; + uint8_t int1_drdy_xl : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_int1_ctrl_t; + +#define LSM6DS3TR_C_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_temp : 1; + uint8_t int2_fth : 1; + uint8_t int2_fifo_ovr : 1; + uint8_t int2_full_flag : 1; + uint8_t int2_step_count_ov : 1; + uint8_t int2_step_delta : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t int2_step_delta : 1; + uint8_t int2_step_count_ov : 1; + uint8_t int2_full_flag : 1; + uint8_t int2_fifo_ovr : 1; + uint8_t int2_fth : 1; + uint8_t int2_drdy_temp : 1; + uint8_t int2_drdy_g : 1; + uint8_t int2_drdy_xl : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_int2_ctrl_t; + +#define LSM6DS3TR_C_WHO_AM_I 0x0FU +#define LSM6DS3TR_C_CTRL1_XL 0x10U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t bw0_xl : 1; + uint8_t lpf1_bw_sel : 1; + uint8_t fs_xl : 2; + uint8_t odr_xl : 4; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t odr_xl : 4; + uint8_t fs_xl : 2; + uint8_t lpf1_bw_sel : 1; + uint8_t bw0_xl : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_ctrl1_xl_t; + +#define LSM6DS3TR_C_CTRL2_G 0x11U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_01 : 1; + uint8_t fs_g : 3; /* fs_g + fs_125 */ + uint8_t odr_g : 4; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t odr_g : 4; + uint8_t fs_g : 3; /* fs_g + fs_125 */ + uint8_t not_used_01 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_ctrl2_g_t; + +#define LSM6DS3TR_C_CTRL3_C 0x12U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sw_reset : 1; + uint8_t ble : 1; + uint8_t if_inc : 1; + uint8_t sim : 1; + uint8_t pp_od : 1; + uint8_t h_lactive : 1; + 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 h_lactive : 1; + uint8_t pp_od : 1; + uint8_t sim : 1; + uint8_t if_inc : 1; + uint8_t ble : 1; + uint8_t sw_reset : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_ctrl3_c_t; + +#define LSM6DS3TR_C_CTRL4_C 0x13U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_01 : 1; + uint8_t lpf1_sel_g : 1; + uint8_t i2c_disable : 1; + uint8_t drdy_mask : 1; + uint8_t den_drdy_int1 : 1; + uint8_t int2_on_int1 : 1; + uint8_t sleep : 1; + uint8_t den_xl_en : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t den_xl_en : 1; + uint8_t sleep : 1; + uint8_t int2_on_int1 : 1; + uint8_t den_drdy_int1 : 1; + uint8_t drdy_mask : 1; + uint8_t i2c_disable : 1; + uint8_t lpf1_sel_g : 1; + uint8_t not_used_01 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_ctrl4_c_t; + +#define LSM6DS3TR_C_CTRL5_C 0x14U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t st_xl : 2; + uint8_t st_g : 2; + uint8_t den_lh : 1; + uint8_t rounding : 3; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t rounding : 3; + uint8_t den_lh : 1; + uint8_t st_g : 2; + uint8_t st_xl : 2; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_ctrl5_c_t; + +#define LSM6DS3TR_C_CTRL6_C 0x15U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ftype : 2; + uint8_t not_used_01 : 1; + uint8_t usr_off_w : 1; + uint8_t xl_hm_mode : 1; +uint8_t den_mode : + 3; /* trig_en + lvl_en + lvl2_en */ +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN +uint8_t den_mode : + 3; /* trig_en + lvl_en + lvl2_en */ + uint8_t xl_hm_mode : 1; + uint8_t usr_off_w : 1; + uint8_t not_used_01 : 1; + uint8_t ftype : 2; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_ctrl6_c_t; + +#define LSM6DS3TR_C_CTRL7_G 0x16U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_01 : 2; + uint8_t rounding_status : 1; + uint8_t not_used_02 : 1; + uint8_t hpm_g : 2; + uint8_t hp_en_g : 1; + uint8_t g_hm_mode : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t g_hm_mode : 1; + uint8_t hp_en_g : 1; + uint8_t hpm_g : 2; + uint8_t not_used_02 : 1; + uint8_t rounding_status : 1; + uint8_t not_used_01 : 2; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_ctrl7_g_t; + +#define LSM6DS3TR_C_CTRL8_XL 0x17U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t low_pass_on_6d : 1; + uint8_t not_used_01 : 1; + uint8_t hp_slope_xl_en : 1; + uint8_t input_composite : 1; + uint8_t hp_ref_mode : 1; + uint8_t hpcf_xl : 2; + uint8_t lpf2_xl_en : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t lpf2_xl_en : 1; + uint8_t hpcf_xl : 2; + uint8_t hp_ref_mode : 1; + uint8_t input_composite : 1; + uint8_t hp_slope_xl_en : 1; + uint8_t not_used_01 : 1; + uint8_t low_pass_on_6d : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_ctrl8_xl_t; + +#define LSM6DS3TR_C_CTRL9_XL 0x18U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_01 : 2; + uint8_t soft_en : 1; + uint8_t not_used_02 : 1; + uint8_t den_xl_g : 1; + uint8_t den_z : 1; + uint8_t den_y : 1; + uint8_t den_x : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t den_x : 1; + uint8_t den_y : 1; + uint8_t den_z : 1; + uint8_t den_xl_g : 1; + uint8_t not_used_02 : 1; + uint8_t soft_en : 1; + uint8_t not_used_01 : 2; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_ctrl9_xl_t; + +#define LSM6DS3TR_C_CTRL10_C 0x19U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sign_motion_en : 1; + uint8_t pedo_rst_step : 1; + uint8_t func_en : 1; + uint8_t tilt_en : 1; + uint8_t pedo_en : 1; + uint8_t timer_en : 1; + uint8_t not_used_01 : 1; + uint8_t wrist_tilt_en : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t wrist_tilt_en : 1; + uint8_t not_used_01 : 1; + uint8_t timer_en : 1; + uint8_t pedo_en : 1; + uint8_t tilt_en : 1; + uint8_t func_en : 1; + uint8_t pedo_rst_step : 1; + uint8_t sign_motion_en : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_ctrl10_c_t; + +#define LSM6DS3TR_C_MASTER_CONFIG 0x1AU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t master_on : 1; + uint8_t iron_en : 1; + uint8_t pass_through_mode : 1; + uint8_t pull_up_en : 1; + uint8_t start_config : 1; + uint8_t not_used_01 : 1; + uint8_t data_valid_sel_fifo : 1; + uint8_t drdy_on_int1 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t drdy_on_int1 : 1; + uint8_t data_valid_sel_fifo : 1; + uint8_t not_used_01 : 1; + uint8_t start_config : 1; + uint8_t pull_up_en : 1; + uint8_t pass_through_mode : 1; + uint8_t iron_en : 1; + uint8_t master_on : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_master_config_t; + +#define LSM6DS3TR_C_WAKE_UP_SRC 0x1BU +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_ia : 1; + uint8_t ff_ia : 1; + uint8_t not_used_01 : 2; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 2; + uint8_t ff_ia : 1; + uint8_t sleep_state_ia : 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 */ +} lsm6ds3tr_c_wake_up_src_t; + +#define LSM6DS3TR_C_TAP_SRC 0x1CU +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_used_01 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 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 */ +} lsm6ds3tr_c_tap_src_t; + +#define LSM6DS3TR_C_D6D_SRC 0x1DU +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 den_drdy : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t den_drdy : 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 */ +} lsm6ds3tr_c_d6d_src_t; + +#define LSM6DS3TR_C_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 not_used_01 : 5; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 5; + uint8_t tda : 1; + uint8_t gda : 1; + uint8_t xlda : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_status_reg_t; + +#define LSM6DS3TR_C_OUT_TEMP_L 0x20U +#define LSM6DS3TR_C_OUT_TEMP_H 0x21U +#define LSM6DS3TR_C_OUTX_L_G 0x22U +#define LSM6DS3TR_C_OUTX_H_G 0x23U +#define LSM6DS3TR_C_OUTY_L_G 0x24U +#define LSM6DS3TR_C_OUTY_H_G 0x25U +#define LSM6DS3TR_C_OUTZ_L_G 0x26U +#define LSM6DS3TR_C_OUTZ_H_G 0x27U +#define LSM6DS3TR_C_OUTX_L_XL 0x28U +#define LSM6DS3TR_C_OUTX_H_XL 0x29U +#define LSM6DS3TR_C_OUTY_L_XL 0x2AU +#define LSM6DS3TR_C_OUTY_H_XL 0x2BU +#define LSM6DS3TR_C_OUTZ_L_XL 0x2CU +#define LSM6DS3TR_C_OUTZ_H_XL 0x2DU +#define LSM6DS3TR_C_SENSORHUB1_REG 0x2EU +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 */ +} lsm6ds3tr_c_sensorhub1_reg_t; + +#define LSM6DS3TR_C_SENSORHUB2_REG 0x2FU +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 */ +} lsm6ds3tr_c_sensorhub2_reg_t; + +#define LSM6DS3TR_C_SENSORHUB3_REG 0x30U +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 */ +} lsm6ds3tr_c_sensorhub3_reg_t; + +#define LSM6DS3TR_C_SENSORHUB4_REG 0x31U +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 */ +} lsm6ds3tr_c_sensorhub4_reg_t; + +#define LSM6DS3TR_C_SENSORHUB5_REG 0x32U +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 */ +} lsm6ds3tr_c_sensorhub5_reg_t; + +#define LSM6DS3TR_C_SENSORHUB6_REG 0x33U +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 */ +} lsm6ds3tr_c_sensorhub6_reg_t; + +#define LSM6DS3TR_C_SENSORHUB7_REG 0x34U +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 */ +} lsm6ds3tr_c_sensorhub7_reg_t; + +#define LSM6DS3TR_C_SENSORHUB8_REG 0x35U +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 */ +} lsm6ds3tr_c_sensorhub8_reg_t; + +#define LSM6DS3TR_C_SENSORHUB9_REG 0x36U +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 */ +} lsm6ds3tr_c_sensorhub9_reg_t; + +#define LSM6DS3TR_C_SENSORHUB10_REG 0x37U +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 */ +} lsm6ds3tr_c_sensorhub10_reg_t; + +#define LSM6DS3TR_C_SENSORHUB11_REG 0x38U +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 */ +} lsm6ds3tr_c_sensorhub11_reg_t; + +#define LSM6DS3TR_C_SENSORHUB12_REG 0x39U +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 */ +} lsm6ds3tr_c_sensorhub12_reg_t; + +#define LSM6DS3TR_C_FIFO_STATUS1 0x3AU +typedef struct +{ + uint8_t diff_fifo : 8; /* + FIFO_STATUS2(diff_fifo) */ +} lsm6ds3tr_c_fifo_status1_t; + +#define LSM6DS3TR_C_FIFO_STATUS2 0x3BU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t diff_fifo : 3; /* + FIFO_STATUS1(diff_fifo) */ + uint8_t not_used_01 : 1; + uint8_t fifo_empty : 1; + uint8_t fifo_full_smart : 1; + uint8_t over_run : 1; + uint8_t waterm : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t waterm : 1; + uint8_t over_run : 1; + uint8_t fifo_full_smart : 1; + uint8_t fifo_empty : 1; + uint8_t not_used_01 : 1; + uint8_t diff_fifo : 3; /* + FIFO_STATUS1(diff_fifo) */ +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_fifo_status2_t; + +#define LSM6DS3TR_C_FIFO_STATUS3 0x3CU +typedef struct +{ +uint8_t fifo_pattern : + 8; /* + FIFO_STATUS4(fifo_pattern) */ +} lsm6ds3tr_c_fifo_status3_t; + +#define LSM6DS3TR_C_FIFO_STATUS4 0x3DU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN +uint8_t fifo_pattern : + 2; /* + FIFO_STATUS3(fifo_pattern) */ + uint8_t not_used_01 : 6; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 6; +uint8_t fifo_pattern : + 2; /* + FIFO_STATUS3(fifo_pattern) */ +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_fifo_status4_t; + +#define LSM6DS3TR_C_FIFO_DATA_OUT_L 0x3EU +#define LSM6DS3TR_C_FIFO_DATA_OUT_H 0x3FU +#define LSM6DS3TR_C_TIMESTAMP0_REG 0x40U +#define LSM6DS3TR_C_TIMESTAMP1_REG 0x41U +#define LSM6DS3TR_C_TIMESTAMP2_REG 0x42U +#define LSM6DS3TR_C_STEP_TIMESTAMP_L 0x49U +#define LSM6DS3TR_C_STEP_TIMESTAMP_H 0x4AU +#define LSM6DS3TR_C_STEP_COUNTER_L 0x4BU +#define LSM6DS3TR_C_STEP_COUNTER_H 0x4CU + +#define LSM6DS3TR_C_SENSORHUB13_REG 0x4DU +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 */ +} lsm6ds3tr_c_sensorhub13_reg_t; + +#define LSM6DS3TR_C_SENSORHUB14_REG 0x4EU +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 */ +} lsm6ds3tr_c_sensorhub14_reg_t; + +#define LSM6DS3TR_C_SENSORHUB15_REG 0x4FU +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 */ +} lsm6ds3tr_c_sensorhub15_reg_t; + +#define LSM6DS3TR_C_SENSORHUB16_REG 0x50U +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 */ +} lsm6ds3tr_c_sensorhub16_reg_t; + +#define LSM6DS3TR_C_SENSORHUB17_REG 0x51U +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 */ +} lsm6ds3tr_c_sensorhub17_reg_t; + +#define LSM6DS3TR_C_SENSORHUB18_REG 0x52U +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 */ +} lsm6ds3tr_c_sensorhub18_reg_t; + +#define LSM6DS3TR_C_FUNC_SRC1 0x53U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sensorhub_end_op : 1; + uint8_t si_end_op : 1; + uint8_t hi_fail : 1; + uint8_t step_overflow : 1; + uint8_t step_detected : 1; + uint8_t tilt_ia : 1; + uint8_t sign_motion_ia : 1; + uint8_t step_count_delta_ia : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t step_count_delta_ia : 1; + uint8_t sign_motion_ia : 1; + uint8_t tilt_ia : 1; + uint8_t step_detected : 1; + uint8_t step_overflow : 1; + uint8_t hi_fail : 1; + uint8_t si_end_op : 1; + uint8_t sensorhub_end_op : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_func_src1_t; + +#define LSM6DS3TR_C_FUNC_SRC2 0x54U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t wrist_tilt_ia : 1; + uint8_t not_used_01 : 2; + uint8_t slave0_nack : 1; + uint8_t slave1_nack : 1; + uint8_t slave2_nack : 1; + uint8_t slave3_nack : 1; + uint8_t not_used_02 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_02 : 1; + uint8_t slave3_nack : 1; + uint8_t slave2_nack : 1; + uint8_t slave1_nack : 1; + uint8_t slave0_nack : 1; + uint8_t not_used_01 : 2; + uint8_t wrist_tilt_ia : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_func_src2_t; + +#define LSM6DS3TR_C_WRIST_TILT_IA 0x55U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_01 : 2; + uint8_t wrist_tilt_ia_zneg : 1; + uint8_t wrist_tilt_ia_zpos : 1; + uint8_t wrist_tilt_ia_yneg : 1; + uint8_t wrist_tilt_ia_ypos : 1; + uint8_t wrist_tilt_ia_xneg : 1; + uint8_t wrist_tilt_ia_xpos : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t wrist_tilt_ia_xpos : 1; + uint8_t wrist_tilt_ia_xneg : 1; + uint8_t wrist_tilt_ia_ypos : 1; + uint8_t wrist_tilt_ia_yneg : 1; + uint8_t wrist_tilt_ia_zpos : 1; + uint8_t wrist_tilt_ia_zneg : 1; + uint8_t not_used_01 : 2; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_wrist_tilt_ia_t; + +#define LSM6DS3TR_C_TAP_CFG 0x58U +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 inact_en : 2; + uint8_t interrupts_enable : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t interrupts_enable : 1; + uint8_t inact_en : 2; + 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 */ +} lsm6ds3tr_c_tap_cfg_t; + +#define LSM6DS3TR_C_TAP_THS_6D 0x59U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t tap_ths : 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 : 5; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_tap_ths_6d_t; + +#define LSM6DS3TR_C_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 */ +} lsm6ds3tr_c_int_dur2_t; + +#define LSM6DS3TR_C_WAKE_UP_THS 0x5BU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t wk_ths : 6; + uint8_t not_used_01 : 1; + uint8_t single_double_tap : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t single_double_tap : 1; + uint8_t not_used_01 : 1; + uint8_t wk_ths : 6; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_wake_up_ths_t; + +#define LSM6DS3TR_C_WAKE_UP_DUR 0x5CU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sleep_dur : 4; + uint8_t timer_hr : 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 timer_hr : 1; + uint8_t sleep_dur : 4; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_wake_up_dur_t; + +#define LSM6DS3TR_C_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 */ +} lsm6ds3tr_c_free_fall_t; + +#define LSM6DS3TR_C_MD1_CFG 0x5EU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t int1_timer : 1; + uint8_t int1_tilt : 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_inact_state : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t int1_inact_state : 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_tilt : 1; + uint8_t int1_timer : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_md1_cfg_t; + +#define LSM6DS3TR_C_MD2_CFG 0x5FU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t int2_iron : 1; + uint8_t int2_tilt : 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_inact_state : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t int2_inact_state : 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_tilt : 1; + uint8_t int2_iron : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_md2_cfg_t; + +#define LSM6DS3TR_C_MASTER_CMD_CODE 0x60U +typedef struct +{ + uint8_t master_cmd_code : 8; +} lsm6ds3tr_c_master_cmd_code_t; + +#define LSM6DS3TR_C_SENS_SYNC_SPI_ERROR_CODE 0x61U +typedef struct +{ + uint8_t error_code : 8; +} lsm6ds3tr_c_sens_sync_spi_error_code_t; + +#define LSM6DS3TR_C_OUT_MAG_RAW_X_L 0x66U +#define LSM6DS3TR_C_OUT_MAG_RAW_X_H 0x67U +#define LSM6DS3TR_C_OUT_MAG_RAW_Y_L 0x68U +#define LSM6DS3TR_C_OUT_MAG_RAW_Y_H 0x69U +#define LSM6DS3TR_C_OUT_MAG_RAW_Z_L 0x6AU +#define LSM6DS3TR_C_OUT_MAG_RAW_Z_H 0x6BU +#define LSM6DS3TR_C_X_OFS_USR 0x73U +#define LSM6DS3TR_C_Y_OFS_USR 0x74U +#define LSM6DS3TR_C_Z_OFS_USR 0x75U +#define LSM6DS3TR_C_SLV0_ADD 0x02U +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 */ +} lsm6ds3tr_c_slv0_add_t; + +#define LSM6DS3TR_C_SLV0_SUBADD 0x03U +typedef struct +{ + uint8_t slave0_reg : 8; +} lsm6ds3tr_c_slv0_subadd_t; + +#define LSM6DS3TR_C_SLAVE0_CONFIG 0x04U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t slave0_numop : 3; + uint8_t src_mode : 1; + uint8_t aux_sens_on : 2; + uint8_t slave0_rate : 2; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t slave0_rate : 2; + uint8_t aux_sens_on : 2; + uint8_t src_mode : 1; + uint8_t slave0_numop : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_slave0_config_t; + +#define LSM6DS3TR_C_SLV1_ADD 0x05U +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 */ +} lsm6ds3tr_c_slv1_add_t; + +#define LSM6DS3TR_C_SLV1_SUBADD 0x06U +typedef struct +{ + uint8_t slave1_reg : 8; +} lsm6ds3tr_c_slv1_subadd_t; + +#define LSM6DS3TR_C_SLAVE1_CONFIG 0x07U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t slave1_numop : 3; + uint8_t not_used_01 : 2; + uint8_t write_once : 1; + uint8_t slave1_rate : 2; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t slave1_rate : 2; + uint8_t write_once : 1; + uint8_t not_used_01 : 2; + uint8_t slave1_numop : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_slave1_config_t; + +#define LSM6DS3TR_C_SLV2_ADD 0x08U +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 */ +} lsm6ds3tr_c_slv2_add_t; + +#define LSM6DS3TR_C_SLV2_SUBADD 0x09U +typedef struct +{ + uint8_t slave2_reg : 8; +} lsm6ds3tr_c_slv2_subadd_t; + +#define LSM6DS3TR_C_SLAVE2_CONFIG 0x0AU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t slave2_numop : 3; + uint8_t not_used_01 : 3; + uint8_t slave2_rate : 2; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t slave2_rate : 2; + uint8_t not_used_01 : 3; + uint8_t slave2_numop : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_slave2_config_t; + +#define LSM6DS3TR_C_SLV3_ADD 0x0BU +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 */ +} lsm6ds3tr_c_slv3_add_t; + +#define LSM6DS3TR_C_SLV3_SUBADD 0x0CU +typedef struct +{ + uint8_t slave3_reg : 8; +} lsm6ds3tr_c_slv3_subadd_t; + +#define LSM6DS3TR_C_SLAVE3_CONFIG 0x0DU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t slave3_numop : 3; + uint8_t not_used_01 : 3; + uint8_t slave3_rate : 2; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t slave3_rate : 2; + uint8_t not_used_01 : 3; + uint8_t slave3_numop : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_slave3_config_t; + +#define LSM6DS3TR_C_DATAWRITE_SRC_MODE_SUB_SLV0 0x0EU +typedef struct +{ + uint8_t slave_dataw : 8; +} lsm6ds3tr_c_datawrite_src_mode_sub_slv0_t; + +#define LSM6DS3TR_C_CONFIG_PEDO_THS_MIN 0x0FU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ths_min : 5; + uint8_t not_used_01 : 2; + uint8_t pedo_fs : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t pedo_fs : 1; + uint8_t not_used_01 : 2; + uint8_t ths_min : 5; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_config_pedo_ths_min_t; + +#define LSM6DS3TR_C_SM_THS 0x13U +#define LSM6DS3TR_C_PEDO_DEB_REG 0x14U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t deb_step : 3; + uint8_t deb_time : 5; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t deb_time : 5; + uint8_t deb_step : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_pedo_deb_reg_t; + +#define LSM6DS3TR_C_STEP_COUNT_DELTA 0x15U +#define LSM6DS3TR_C_MAG_SI_XX 0x24U +#define LSM6DS3TR_C_MAG_SI_XY 0x25U +#define LSM6DS3TR_C_MAG_SI_XZ 0x26U +#define LSM6DS3TR_C_MAG_SI_YX 0x27U +#define LSM6DS3TR_C_MAG_SI_YY 0x28U +#define LSM6DS3TR_C_MAG_SI_YZ 0x29U +#define LSM6DS3TR_C_MAG_SI_ZX 0x2AU +#define LSM6DS3TR_C_MAG_SI_ZY 0x2BU +#define LSM6DS3TR_C_MAG_SI_ZZ 0x2CU +#define LSM6DS3TR_C_MAG_OFFX_L 0x2DU +#define LSM6DS3TR_C_MAG_OFFX_H 0x2EU +#define LSM6DS3TR_C_MAG_OFFY_L 0x2FU +#define LSM6DS3TR_C_MAG_OFFY_H 0x30U +#define LSM6DS3TR_C_MAG_OFFZ_L 0x31U +#define LSM6DS3TR_C_MAG_OFFZ_H 0x32U +#define LSM6DS3TR_C_A_WRIST_TILT_LAT 0x50U +#define LSM6DS3TR_C_A_WRIST_TILT_THS 0x54U +#define LSM6DS3TR_C_A_WRIST_TILT_MASK 0x59U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_01 : 2; + uint8_t wrist_tilt_mask_zneg : 1; + uint8_t wrist_tilt_mask_zpos : 1; + uint8_t wrist_tilt_mask_yneg : 1; + uint8_t wrist_tilt_mask_ypos : 1; + uint8_t wrist_tilt_mask_xneg : 1; + uint8_t wrist_tilt_mask_xpos : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t wrist_tilt_mask_xpos : 1; + uint8_t wrist_tilt_mask_xneg : 1; + uint8_t wrist_tilt_mask_ypos : 1; + uint8_t wrist_tilt_mask_yneg : 1; + uint8_t wrist_tilt_mask_zpos : 1; + uint8_t wrist_tilt_mask_zneg : 1; + uint8_t not_used_01 : 2; +#endif /* DRV_BYTE_ORDER */ +} lsm6ds3tr_c_a_wrist_tilt_mask_t; + +/** + * @defgroup LSM6DS3TR_C_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 +{ + lsm6ds3tr_c_func_cfg_access_t func_cfg_access; + lsm6ds3tr_c_sensor_sync_time_frame_t sensor_sync_time_frame; + lsm6ds3tr_c_sensor_sync_res_ratio_t sensor_sync_res_ratio; + lsm6ds3tr_c_fifo_ctrl1_t fifo_ctrl1; + lsm6ds3tr_c_fifo_ctrl2_t fifo_ctrl2; + lsm6ds3tr_c_fifo_ctrl3_t fifo_ctrl3; + lsm6ds3tr_c_fifo_ctrl4_t fifo_ctrl4; + lsm6ds3tr_c_fifo_ctrl5_t fifo_ctrl5; + lsm6ds3tr_c_drdy_pulse_cfg_g_t drdy_pulse_cfg_g; + lsm6ds3tr_c_int1_ctrl_t int1_ctrl; + lsm6ds3tr_c_int2_ctrl_t int2_ctrl; + lsm6ds3tr_c_ctrl1_xl_t ctrl1_xl; + lsm6ds3tr_c_ctrl2_g_t ctrl2_g; + lsm6ds3tr_c_ctrl3_c_t ctrl3_c; + lsm6ds3tr_c_ctrl4_c_t ctrl4_c; + lsm6ds3tr_c_ctrl5_c_t ctrl5_c; + lsm6ds3tr_c_ctrl6_c_t ctrl6_c; + lsm6ds3tr_c_ctrl7_g_t ctrl7_g; + lsm6ds3tr_c_ctrl8_xl_t ctrl8_xl; + lsm6ds3tr_c_ctrl9_xl_t ctrl9_xl; + lsm6ds3tr_c_ctrl10_c_t ctrl10_c; + lsm6ds3tr_c_master_config_t master_config; + lsm6ds3tr_c_wake_up_src_t wake_up_src; + lsm6ds3tr_c_tap_src_t tap_src; + lsm6ds3tr_c_d6d_src_t d6d_src; + lsm6ds3tr_c_status_reg_t status_reg; + lsm6ds3tr_c_sensorhub1_reg_t sensorhub1_reg; + lsm6ds3tr_c_sensorhub2_reg_t sensorhub2_reg; + lsm6ds3tr_c_sensorhub3_reg_t sensorhub3_reg; + lsm6ds3tr_c_sensorhub4_reg_t sensorhub4_reg; + lsm6ds3tr_c_sensorhub5_reg_t sensorhub5_reg; + lsm6ds3tr_c_sensorhub6_reg_t sensorhub6_reg; + lsm6ds3tr_c_sensorhub7_reg_t sensorhub7_reg; + lsm6ds3tr_c_sensorhub8_reg_t sensorhub8_reg; + lsm6ds3tr_c_sensorhub9_reg_t sensorhub9_reg; + lsm6ds3tr_c_sensorhub10_reg_t sensorhub10_reg; + lsm6ds3tr_c_sensorhub11_reg_t sensorhub11_reg; + lsm6ds3tr_c_sensorhub12_reg_t sensorhub12_reg; + lsm6ds3tr_c_fifo_status1_t fifo_status1; + lsm6ds3tr_c_fifo_status2_t fifo_status2; + lsm6ds3tr_c_fifo_status3_t fifo_status3; + lsm6ds3tr_c_fifo_status4_t fifo_status4; + lsm6ds3tr_c_sensorhub13_reg_t sensorhub13_reg; + lsm6ds3tr_c_sensorhub14_reg_t sensorhub14_reg; + lsm6ds3tr_c_sensorhub15_reg_t sensorhub15_reg; + lsm6ds3tr_c_sensorhub16_reg_t sensorhub16_reg; + lsm6ds3tr_c_sensorhub17_reg_t sensorhub17_reg; + lsm6ds3tr_c_sensorhub18_reg_t sensorhub18_reg; + lsm6ds3tr_c_func_src1_t func_src1; + lsm6ds3tr_c_func_src2_t func_src2; + lsm6ds3tr_c_wrist_tilt_ia_t wrist_tilt_ia; + lsm6ds3tr_c_tap_cfg_t tap_cfg; + lsm6ds3tr_c_tap_ths_6d_t tap_ths_6d; + lsm6ds3tr_c_int_dur2_t int_dur2; + lsm6ds3tr_c_wake_up_ths_t wake_up_ths; + lsm6ds3tr_c_wake_up_dur_t wake_up_dur; + lsm6ds3tr_c_free_fall_t free_fall; + lsm6ds3tr_c_md1_cfg_t md1_cfg; + lsm6ds3tr_c_md2_cfg_t md2_cfg; + lsm6ds3tr_c_master_cmd_code_t master_cmd_code; + lsm6ds3tr_c_sens_sync_spi_error_code_t + sens_sync_spi_error_code; + lsm6ds3tr_c_slv0_add_t slv0_add; + lsm6ds3tr_c_slv0_subadd_t slv0_subadd; + lsm6ds3tr_c_slave0_config_t slave0_config; + lsm6ds3tr_c_slv1_add_t slv1_add; + lsm6ds3tr_c_slv1_subadd_t slv1_subadd; + lsm6ds3tr_c_slave1_config_t slave1_config; + lsm6ds3tr_c_slv2_add_t slv2_add; + lsm6ds3tr_c_slv2_subadd_t slv2_subadd; + lsm6ds3tr_c_slave2_config_t slave2_config; + lsm6ds3tr_c_slv3_add_t slv3_add; + lsm6ds3tr_c_slv3_subadd_t slv3_subadd; + lsm6ds3tr_c_slave3_config_t slave3_config; + lsm6ds3tr_c_datawrite_src_mode_sub_slv0_t + datawrite_src_mode_sub_slv0; + lsm6ds3tr_c_config_pedo_ths_min_t config_pedo_ths_min; + lsm6ds3tr_c_pedo_deb_reg_t pedo_deb_reg; + lsm6ds3tr_c_a_wrist_tilt_mask_t a_wrist_tilt_mask; + bitwise_t bitwise; + uint8_t byte; +} lsm6ds3tr_c_reg_t; + +/** + * @} + * + */ + +#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 lsm6ds3tr_c_read_reg(stmdev_ctx_t *ctx, uint8_t reg, + uint8_t *data, + uint16_t len); +int32_t lsm6ds3tr_c_write_reg(stmdev_ctx_t *ctx, uint8_t reg, + uint8_t *data, + uint16_t len); + +float_t lsm6ds3tr_c_from_fs2g_to_mg(int16_t lsb); +float_t lsm6ds3tr_c_from_fs4g_to_mg(int16_t lsb); +float_t lsm6ds3tr_c_from_fs8g_to_mg(int16_t lsb); +float_t lsm6ds3tr_c_from_fs16g_to_mg(int16_t lsb); + +float_t lsm6ds3tr_c_from_fs125dps_to_mdps(int16_t lsb); +float_t lsm6ds3tr_c_from_fs250dps_to_mdps(int16_t lsb); +float_t lsm6ds3tr_c_from_fs500dps_to_mdps(int16_t lsb); +float_t lsm6ds3tr_c_from_fs1000dps_to_mdps(int16_t lsb); +float_t lsm6ds3tr_c_from_fs2000dps_to_mdps(int16_t lsb); + +float_t lsm6ds3tr_c_from_lsb_to_celsius(int16_t lsb); + +typedef enum +{ + LSM6DS3TR_C_2g = 0, + LSM6DS3TR_C_16g = 1, + LSM6DS3TR_C_4g = 2, + LSM6DS3TR_C_8g = 3, + LSM6DS3TR_C_XL_FS_ND = 4, /* ERROR CODE */ +} lsm6ds3tr_c_fs_xl_t; +int32_t lsm6ds3tr_c_xl_full_scale_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_fs_xl_t val); +int32_t lsm6ds3tr_c_xl_full_scale_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_fs_xl_t *val); + +typedef enum +{ + LSM6DS3TR_C_XL_ODR_OFF = 0, + LSM6DS3TR_C_XL_ODR_12Hz5 = 1, + LSM6DS3TR_C_XL_ODR_26Hz = 2, + LSM6DS3TR_C_XL_ODR_52Hz = 3, + LSM6DS3TR_C_XL_ODR_104Hz = 4, + LSM6DS3TR_C_XL_ODR_208Hz = 5, + LSM6DS3TR_C_XL_ODR_416Hz = 6, + LSM6DS3TR_C_XL_ODR_833Hz = 7, + LSM6DS3TR_C_XL_ODR_1k66Hz = 8, + LSM6DS3TR_C_XL_ODR_3k33Hz = 9, + LSM6DS3TR_C_XL_ODR_6k66Hz = 10, + LSM6DS3TR_C_XL_ODR_1Hz6 = 11, + LSM6DS3TR_C_XL_ODR_ND = 12, /* ERROR CODE */ +} lsm6ds3tr_c_odr_xl_t; +int32_t lsm6ds3tr_c_xl_data_rate_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_odr_xl_t val); +int32_t lsm6ds3tr_c_xl_data_rate_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_odr_xl_t *val); + +typedef enum +{ + LSM6DS3TR_C_250dps = 0, + LSM6DS3TR_C_125dps = 1, + LSM6DS3TR_C_500dps = 2, + LSM6DS3TR_C_1000dps = 4, + LSM6DS3TR_C_2000dps = 6, + LSM6DS3TR_C_GY_FS_ND = 7, /* ERROR CODE */ +} lsm6ds3tr_c_fs_g_t; +int32_t lsm6ds3tr_c_gy_full_scale_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_fs_g_t val); +int32_t lsm6ds3tr_c_gy_full_scale_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_fs_g_t *val); + +typedef enum +{ + LSM6DS3TR_C_GY_ODR_OFF = 0, + LSM6DS3TR_C_GY_ODR_12Hz5 = 1, + LSM6DS3TR_C_GY_ODR_26Hz = 2, + LSM6DS3TR_C_GY_ODR_52Hz = 3, + LSM6DS3TR_C_GY_ODR_104Hz = 4, + LSM6DS3TR_C_GY_ODR_208Hz = 5, + LSM6DS3TR_C_GY_ODR_416Hz = 6, + LSM6DS3TR_C_GY_ODR_833Hz = 7, + LSM6DS3TR_C_GY_ODR_1k66Hz = 8, + LSM6DS3TR_C_GY_ODR_3k33Hz = 9, + LSM6DS3TR_C_GY_ODR_6k66Hz = 10, + LSM6DS3TR_C_GY_ODR_ND = 11, /* ERROR CODE */ +} lsm6ds3tr_c_odr_g_t; +int32_t lsm6ds3tr_c_gy_data_rate_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_odr_g_t val); +int32_t lsm6ds3tr_c_gy_data_rate_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_odr_g_t *val); + +int32_t lsm6ds3tr_c_block_data_update_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6ds3tr_c_block_data_update_get(stmdev_ctx_t *ctx, + uint8_t *val); + +typedef enum +{ + LSM6DS3TR_C_LSb_1mg = 0, + LSM6DS3TR_C_LSb_16mg = 1, + LSM6DS3TR_C_WEIGHT_ND = 2, +} lsm6ds3tr_c_usr_off_w_t; +int32_t lsm6ds3tr_c_xl_offset_weight_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_usr_off_w_t val); +int32_t lsm6ds3tr_c_xl_offset_weight_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_usr_off_w_t *val); + +typedef enum +{ + LSM6DS3TR_C_XL_HIGH_PERFORMANCE = 0, + LSM6DS3TR_C_XL_NORMAL = 1, + LSM6DS3TR_C_XL_PW_MODE_ND = 2, /* ERROR CODE */ +} lsm6ds3tr_c_xl_hm_mode_t; +int32_t lsm6ds3tr_c_xl_power_mode_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_xl_hm_mode_t val); +int32_t lsm6ds3tr_c_xl_power_mode_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_xl_hm_mode_t *val); + +typedef enum +{ + LSM6DS3TR_C_STAT_RND_DISABLE = 0, + LSM6DS3TR_C_STAT_RND_ENABLE = 1, + LSM6DS3TR_C_STAT_RND_ND = 2, /* ERROR CODE */ +} lsm6ds3tr_c_rounding_status_t; +int32_t lsm6ds3tr_c_rounding_on_status_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_rounding_status_t val); +int32_t lsm6ds3tr_c_rounding_on_status_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_rounding_status_t *val); + +typedef enum +{ + LSM6DS3TR_C_GY_HIGH_PERFORMANCE = 0, + LSM6DS3TR_C_GY_NORMAL = 1, + LSM6DS3TR_C_GY_PW_MODE_ND = 2, /* ERROR CODE */ +} lsm6ds3tr_c_g_hm_mode_t; +int32_t lsm6ds3tr_c_gy_power_mode_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_g_hm_mode_t val); +int32_t lsm6ds3tr_c_gy_power_mode_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_g_hm_mode_t *val); + +typedef struct +{ + lsm6ds3tr_c_wake_up_src_t wake_up_src; + lsm6ds3tr_c_tap_src_t tap_src; + lsm6ds3tr_c_d6d_src_t d6d_src; + lsm6ds3tr_c_status_reg_t status_reg; + lsm6ds3tr_c_func_src1_t func_src1; + lsm6ds3tr_c_func_src2_t func_src2; + lsm6ds3tr_c_wrist_tilt_ia_t wrist_tilt_ia; + lsm6ds3tr_c_a_wrist_tilt_mask_t a_wrist_tilt_mask; +} lsm6ds3tr_c_all_sources_t; +int32_t lsm6ds3tr_c_all_sources_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_all_sources_t *val); + +int32_t lsm6ds3tr_c_status_reg_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_status_reg_t *val); + +int32_t lsm6ds3tr_c_xl_flag_data_ready_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6ds3tr_c_gy_flag_data_ready_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6ds3tr_c_temp_flag_data_ready_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6ds3tr_c_xl_usr_offset_set(stmdev_ctx_t *ctx, + uint8_t *buff); +int32_t lsm6ds3tr_c_xl_usr_offset_get(stmdev_ctx_t *ctx, + uint8_t *buff); +int32_t lsm6ds3tr_c_timestamp_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3tr_c_timestamp_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + LSM6DS3TR_C_LSB_6ms4 = 0, + LSM6DS3TR_C_LSB_25us = 1, + LSM6DS3TR_C_TS_RES_ND = 2, /* ERROR CODE */ +} lsm6ds3tr_c_timer_hr_t; +int32_t lsm6ds3tr_c_timestamp_res_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_timer_hr_t val); +int32_t lsm6ds3tr_c_timestamp_res_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_timer_hr_t *val); + +typedef enum +{ + LSM6DS3TR_C_ROUND_DISABLE = 0, + LSM6DS3TR_C_ROUND_XL = 1, + LSM6DS3TR_C_ROUND_GY = 2, + LSM6DS3TR_C_ROUND_GY_XL = 3, + LSM6DS3TR_C_ROUND_SH1_TO_SH6 = 4, + LSM6DS3TR_C_ROUND_XL_SH1_TO_SH6 = 5, + LSM6DS3TR_C_ROUND_GY_XL_SH1_TO_SH12 = 6, + LSM6DS3TR_C_ROUND_GY_XL_SH1_TO_SH6 = 7, + LSM6DS3TR_C_ROUND_OUT_ND = 8, /* ERROR CODE */ +} lsm6ds3tr_c_rounding_t; +int32_t lsm6ds3tr_c_rounding_mode_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_rounding_t val); +int32_t lsm6ds3tr_c_rounding_mode_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_rounding_t *val); + +int32_t lsm6ds3tr_c_temperature_raw_get(stmdev_ctx_t *ctx, + int16_t *val); +int32_t lsm6ds3tr_c_angular_rate_raw_get(stmdev_ctx_t *ctx, + int16_t *val); +int32_t lsm6ds3tr_c_acceleration_raw_get(stmdev_ctx_t *ctx, + int16_t *val); + +int32_t lsm6ds3tr_c_mag_calibrated_raw_get(stmdev_ctx_t *ctx, + int16_t *val); + +int32_t lsm6ds3tr_c_fifo_raw_data_get(stmdev_ctx_t *ctx, + uint8_t *buffer); + +typedef enum +{ + LSM6DS3TR_C_USER_BANK = 0, + LSM6DS3TR_C_BANK_A = 4, + LSM6DS3TR_C_BANK_B = 5, + LSM6DS3TR_C_BANK_ND = 6, /* ERROR CODE */ +} lsm6ds3tr_c_func_cfg_en_t; +int32_t lsm6ds3tr_c_mem_bank_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_func_cfg_en_t val); +int32_t lsm6ds3tr_c_mem_bank_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_func_cfg_en_t *val); + +typedef enum +{ + LSM6DS3TR_C_DRDY_LATCHED = 0, + LSM6DS3TR_C_DRDY_PULSED = 1, + LSM6DS3TR_C_DRDY_ND = 2, /* ERROR CODE */ +} lsm6ds3tr_c_drdy_pulsed_g_t; +int32_t lsm6ds3tr_c_data_ready_mode_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_drdy_pulsed_g_t val); +int32_t lsm6ds3tr_c_data_ready_mode_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_drdy_pulsed_g_t *val); + +int32_t lsm6ds3tr_c_device_id_get(stmdev_ctx_t *ctx, uint8_t *buff); +int32_t lsm6ds3tr_c_reset_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3tr_c_reset_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + LSM6DS3TR_C_LSB_AT_LOW_ADD = 0, + LSM6DS3TR_C_MSB_AT_LOW_ADD = 1, + LSM6DS3TR_C_DATA_FMT_ND = 2, /* ERROR CODE */ +} lsm6ds3tr_c_ble_t; +int32_t lsm6ds3tr_c_data_format_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_ble_t val); +int32_t lsm6ds3tr_c_data_format_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_ble_t *val); + +int32_t lsm6ds3tr_c_auto_increment_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6ds3tr_c_auto_increment_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6ds3tr_c_boot_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3tr_c_boot_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + LSM6DS3TR_C_XL_ST_DISABLE = 0, + LSM6DS3TR_C_XL_ST_POSITIVE = 1, + LSM6DS3TR_C_XL_ST_NEGATIVE = 2, + LSM6DS3TR_C_XL_ST_ND = 3, /* ERROR CODE */ +} lsm6ds3tr_c_st_xl_t; +int32_t lsm6ds3tr_c_xl_self_test_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_st_xl_t val); +int32_t lsm6ds3tr_c_xl_self_test_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_st_xl_t *val); + +typedef enum +{ + LSM6DS3TR_C_GY_ST_DISABLE = 0, + LSM6DS3TR_C_GY_ST_POSITIVE = 1, + LSM6DS3TR_C_GY_ST_NEGATIVE = 3, + LSM6DS3TR_C_GY_ST_ND = 4, /* ERROR CODE */ +} lsm6ds3tr_c_st_g_t; +int32_t lsm6ds3tr_c_gy_self_test_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_st_g_t val); +int32_t lsm6ds3tr_c_gy_self_test_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_st_g_t *val); + +int32_t lsm6ds3tr_c_filter_settling_mask_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6ds3tr_c_filter_settling_mask_get(stmdev_ctx_t *ctx, + uint8_t *val); + +typedef enum +{ + LSM6DS3TR_C_USE_SLOPE = 0, + LSM6DS3TR_C_USE_HPF = 1, + LSM6DS3TR_C_HP_PATH_ND = 2, /* ERROR CODE */ +} lsm6ds3tr_c_slope_fds_t; +int32_t lsm6ds3tr_c_xl_hp_path_internal_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_slope_fds_t val); +int32_t lsm6ds3tr_c_xl_hp_path_internal_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_slope_fds_t *val); + +typedef enum +{ + LSM6DS3TR_C_XL_ANA_BW_1k5Hz = 0, + LSM6DS3TR_C_XL_ANA_BW_400Hz = 1, + LSM6DS3TR_C_XL_ANA_BW_ND = 2, /* ERROR CODE */ +} lsm6ds3tr_c_bw0_xl_t; +int32_t lsm6ds3tr_c_xl_filter_analog_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_bw0_xl_t val); +int32_t lsm6ds3tr_c_xl_filter_analog_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_bw0_xl_t *val); + +typedef enum +{ + LSM6DS3TR_C_XL_LP1_ODR_DIV_2 = 0, + LSM6DS3TR_C_XL_LP1_ODR_DIV_4 = 1, + LSM6DS3TR_C_XL_LP1_NA = 2, /* ERROR CODE */ +} lsm6ds3tr_c_lpf1_bw_sel_t; +int32_t lsm6ds3tr_c_xl_lp1_bandwidth_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_lpf1_bw_sel_t val); +int32_t lsm6ds3tr_c_xl_lp1_bandwidth_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_lpf1_bw_sel_t *val); + +typedef enum +{ + LSM6DS3TR_C_XL_LOW_LAT_LP_ODR_DIV_50 = 0x00, + LSM6DS3TR_C_XL_LOW_LAT_LP_ODR_DIV_100 = 0x01, + LSM6DS3TR_C_XL_LOW_LAT_LP_ODR_DIV_9 = 0x02, + LSM6DS3TR_C_XL_LOW_LAT_LP_ODR_DIV_400 = 0x03, + LSM6DS3TR_C_XL_LOW_NOISE_LP_ODR_DIV_50 = 0x10, + LSM6DS3TR_C_XL_LOW_NOISE_LP_ODR_DIV_100 = 0x11, + LSM6DS3TR_C_XL_LOW_NOISE_LP_ODR_DIV_9 = 0x12, + LSM6DS3TR_C_XL_LOW_NOISE_LP_ODR_DIV_400 = 0x13, + LSM6DS3TR_C_XL_LP_NA = 0x20, /* ERROR CODE */ +} lsm6ds3tr_c_input_composite_t; +int32_t lsm6ds3tr_c_xl_lp2_bandwidth_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_input_composite_t val); +int32_t lsm6ds3tr_c_xl_lp2_bandwidth_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_input_composite_t *val); + +int32_t lsm6ds3tr_c_xl_reference_mode_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6ds3tr_c_xl_reference_mode_get(stmdev_ctx_t *ctx, + uint8_t *val); + +typedef enum +{ + LSM6DS3TR_C_XL_HP_ODR_DIV_4 = 0x00, /* Slope filter */ + LSM6DS3TR_C_XL_HP_ODR_DIV_100 = 0x01, + LSM6DS3TR_C_XL_HP_ODR_DIV_9 = 0x02, + LSM6DS3TR_C_XL_HP_ODR_DIV_400 = 0x03, + LSM6DS3TR_C_XL_HP_NA = 0x10, /* ERROR CODE */ +} lsm6ds3tr_c_hpcf_xl_t; +int32_t lsm6ds3tr_c_xl_hp_bandwidth_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_hpcf_xl_t val); +int32_t lsm6ds3tr_c_xl_hp_bandwidth_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_hpcf_xl_t *val); + +typedef enum +{ + LSM6DS3TR_C_LP2_ONLY = 0x00, + + LSM6DS3TR_C_HP_16mHz_LP2 = 0x80, + LSM6DS3TR_C_HP_65mHz_LP2 = 0x90, + LSM6DS3TR_C_HP_260mHz_LP2 = 0xA0, + LSM6DS3TR_C_HP_1Hz04_LP2 = 0xB0, + + LSM6DS3TR_C_HP_DISABLE_LP1_LIGHT = 0x0A, + LSM6DS3TR_C_HP_DISABLE_LP1_NORMAL = 0x09, + LSM6DS3TR_C_HP_DISABLE_LP_STRONG = 0x08, + LSM6DS3TR_C_HP_DISABLE_LP1_AGGRESSIVE = 0x0B, + + LSM6DS3TR_C_HP_16mHz_LP1_LIGHT = 0x8A, + LSM6DS3TR_C_HP_65mHz_LP1_NORMAL = 0x99, + LSM6DS3TR_C_HP_260mHz_LP1_STRONG = 0xA8, + LSM6DS3TR_C_HP_1Hz04_LP1_AGGRESSIVE = 0xBB, + + LSM6DS3TR_C_HP_GY_BAND_NA = 0xFF, /* ERROR CODE */ +} lsm6ds3tr_c_lpf1_sel_g_t; +int32_t lsm6ds3tr_c_gy_band_pass_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_lpf1_sel_g_t val); +int32_t lsm6ds3tr_c_gy_band_pass_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_lpf1_sel_g_t *val); + +typedef enum +{ + LSM6DS3TR_C_SPI_4_WIRE = 0, + LSM6DS3TR_C_SPI_3_WIRE = 1, + LSM6DS3TR_C_SPI_MODE_ND = 2, /* ERROR CODE */ +} lsm6ds3tr_c_sim_t; +int32_t lsm6ds3tr_c_spi_mode_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_sim_t val); +int32_t lsm6ds3tr_c_spi_mode_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_sim_t *val); + +typedef enum +{ + LSM6DS3TR_C_I2C_ENABLE = 0, + LSM6DS3TR_C_I2C_DISABLE = 1, + LSM6DS3TR_C_I2C_MODE_ND = 2, /* ERROR CODE */ +} lsm6ds3tr_c_i2c_disable_t; +int32_t lsm6ds3tr_c_i2c_interface_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_i2c_disable_t val); +int32_t lsm6ds3tr_c_i2c_interface_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_i2c_disable_t *val); + +typedef struct +{ + uint8_t int1_drdy_xl : 1; + uint8_t int1_drdy_g : 1; + uint8_t int1_boot : 1; + uint8_t int1_fth : 1; + uint8_t int1_fifo_ovr : 1; + uint8_t int1_full_flag : 1; + uint8_t int1_sign_mot : 1; + uint8_t int1_step_detector : 1; + uint8_t int1_timer : 1; + uint8_t int1_tilt : 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_inact_state : 1; + uint8_t den_drdy_int1 : 1; + uint8_t drdy_on_int1 : 1; +} lsm6ds3tr_c_int1_route_t; +int32_t lsm6ds3tr_c_pin_int1_route_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_int1_route_t val); +int32_t lsm6ds3tr_c_pin_int1_route_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_int1_route_t *val); + +typedef struct +{ + uint8_t int2_drdy_xl : 1; + uint8_t int2_drdy_g : 1; + uint8_t int2_drdy_temp : 1; + uint8_t int2_fth : 1; + uint8_t int2_fifo_ovr : 1; + uint8_t int2_full_flag : 1; + uint8_t int2_step_count_ov : 1; + uint8_t int2_step_delta : 1; + uint8_t int2_iron : 1; + uint8_t int2_tilt : 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_inact_state : 1; + uint8_t int2_wrist_tilt : 1; +} lsm6ds3tr_c_int2_route_t; +int32_t lsm6ds3tr_c_pin_int2_route_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_int2_route_t val); +int32_t lsm6ds3tr_c_pin_int2_route_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_int2_route_t *val); + +typedef enum +{ + LSM6DS3TR_C_PUSH_PULL = 0, + LSM6DS3TR_C_OPEN_DRAIN = 1, + LSM6DS3TR_C_PIN_MODE_ND = 2, /* ERROR CODE */ +} lsm6ds3tr_c_pp_od_t; +int32_t lsm6ds3tr_c_pin_mode_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_pp_od_t val); +int32_t lsm6ds3tr_c_pin_mode_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_pp_od_t *val); + +typedef enum +{ + LSM6DS3TR_C_ACTIVE_HIGH = 0, + LSM6DS3TR_C_ACTIVE_LOW = 1, + LSM6DS3TR_C_POLARITY_ND = 2, /* ERROR CODE */ +} lsm6ds3tr_c_h_lactive_t; +int32_t lsm6ds3tr_c_pin_polarity_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_h_lactive_t val); +int32_t lsm6ds3tr_c_pin_polarity_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_h_lactive_t *val); + +int32_t lsm6ds3tr_c_all_on_int1_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3tr_c_all_on_int1_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + LSM6DS3TR_C_INT_PULSED = 0, + LSM6DS3TR_C_INT_LATCHED = 1, + LSM6DS3TR_C_INT_MODE = 2, /* ERROR CODE */ +} lsm6ds3tr_c_lir_t; +int32_t lsm6ds3tr_c_int_notification_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_lir_t val); +int32_t lsm6ds3tr_c_int_notification_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_lir_t *val); + +int32_t lsm6ds3tr_c_wkup_threshold_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6ds3tr_c_wkup_threshold_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6ds3tr_c_wkup_dur_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3tr_c_wkup_dur_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3tr_c_gy_sleep_mode_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3tr_c_gy_sleep_mode_get(stmdev_ctx_t *ctx, + uint8_t *val); + +typedef enum +{ + LSM6DS3TR_C_PROPERTY_DISABLE = 0, + LSM6DS3TR_C_XL_12Hz5_GY_NOT_AFFECTED = 1, + LSM6DS3TR_C_XL_12Hz5_GY_SLEEP = 2, + LSM6DS3TR_C_XL_12Hz5_GY_PD = 3, + LSM6DS3TR_C_ACT_MODE_ND = 4, /* ERROR CODE */ +} lsm6ds3tr_c_inact_en_t; +int32_t lsm6ds3tr_c_act_mode_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_inact_en_t val); +int32_t lsm6ds3tr_c_act_mode_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_inact_en_t *val); + +int32_t lsm6ds3tr_c_act_sleep_dur_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3tr_c_act_sleep_dur_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6ds3tr_c_tap_src_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_tap_src_t *val); + +int32_t lsm6ds3tr_c_tap_detection_on_z_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6ds3tr_c_tap_detection_on_z_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6ds3tr_c_tap_detection_on_y_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6ds3tr_c_tap_detection_on_y_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6ds3tr_c_tap_detection_on_x_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6ds3tr_c_tap_detection_on_x_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6ds3tr_c_tap_threshold_x_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6ds3tr_c_tap_threshold_x_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6ds3tr_c_tap_shock_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3tr_c_tap_shock_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3tr_c_tap_quiet_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3tr_c_tap_quiet_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3tr_c_tap_dur_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3tr_c_tap_dur_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + LSM6DS3TR_C_ONLY_SINGLE = 0, + LSM6DS3TR_C_BOTH_SINGLE_DOUBLE = 1, + LSM6DS3TR_C_TAP_MODE_ND = 2, /* ERROR CODE */ +} lsm6ds3tr_c_single_double_tap_t; +int32_t lsm6ds3tr_c_tap_mode_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_single_double_tap_t val); +int32_t lsm6ds3tr_c_tap_mode_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_single_double_tap_t *val); + +typedef enum +{ + LSM6DS3TR_C_ODR_DIV_2_FEED = 0, + LSM6DS3TR_C_LPF2_FEED = 1, + LSM6DS3TR_C_6D_FEED_ND = 2, /* ERROR CODE */ +} lsm6ds3tr_c_low_pass_on_6d_t; +int32_t lsm6ds3tr_c_6d_feed_data_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_low_pass_on_6d_t val); +int32_t lsm6ds3tr_c_6d_feed_data_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_low_pass_on_6d_t *val); + +typedef enum +{ + LSM6DS3TR_C_DEG_80 = 0, + LSM6DS3TR_C_DEG_70 = 1, + LSM6DS3TR_C_DEG_60 = 2, + LSM6DS3TR_C_DEG_50 = 3, + LSM6DS3TR_C_6D_TH_ND = 4, /* ERROR CODE */ +} lsm6ds3tr_c_sixd_ths_t; +int32_t lsm6ds3tr_c_6d_threshold_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_sixd_ths_t val); +int32_t lsm6ds3tr_c_6d_threshold_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_sixd_ths_t *val); + +int32_t lsm6ds3tr_c_4d_mode_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3tr_c_4d_mode_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3tr_c_ff_dur_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3tr_c_ff_dur_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + LSM6DS3TR_C_FF_TSH_156mg = 0, + LSM6DS3TR_C_FF_TSH_219mg = 1, + LSM6DS3TR_C_FF_TSH_250mg = 2, + LSM6DS3TR_C_FF_TSH_312mg = 3, + LSM6DS3TR_C_FF_TSH_344mg = 4, + LSM6DS3TR_C_FF_TSH_406mg = 5, + LSM6DS3TR_C_FF_TSH_469mg = 6, + LSM6DS3TR_C_FF_TSH_500mg = 7, + LSM6DS3TR_C_FF_TSH_ND = 8, /* ERROR CODE */ +} lsm6ds3tr_c_ff_ths_t; +int32_t lsm6ds3tr_c_ff_threshold_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_ff_ths_t val); +int32_t lsm6ds3tr_c_ff_threshold_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_ff_ths_t *val); + +int32_t lsm6ds3tr_c_fifo_watermark_set(stmdev_ctx_t *ctx, + uint16_t val); +int32_t lsm6ds3tr_c_fifo_watermark_get(stmdev_ctx_t *ctx, + uint16_t *val); + +int32_t lsm6ds3tr_c_fifo_data_level_get(stmdev_ctx_t *ctx, + uint16_t *val); + +int32_t lsm6ds3tr_c_fifo_wtm_flag_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6ds3tr_c_fifo_pattern_get(stmdev_ctx_t *ctx, + uint16_t *val); + +int32_t lsm6ds3tr_c_fifo_temp_batch_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6ds3tr_c_fifo_temp_batch_get(stmdev_ctx_t *ctx, + uint8_t *val); + +typedef enum +{ + LSM6DS3TR_C_TRG_XL_GY_DRDY = 0, + LSM6DS3TR_C_TRG_STEP_DETECT = 1, + LSM6DS3TR_C_TRG_SH_DRDY = 2, + LSM6DS3TR_C_TRG_SH_ND = 3, /* ERROR CODE */ +} lsm6ds3tr_c_trigger_fifo_t; +int32_t lsm6ds3tr_c_fifo_write_trigger_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_trigger_fifo_t val); +int32_t lsm6ds3tr_c_fifo_write_trigger_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_trigger_fifo_t *val); + +int32_t lsm6ds3tr_c_fifo_pedo_and_timestamp_batch_set( + stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6ds3tr_c_fifo_pedo_and_timestamp_batch_get( + stmdev_ctx_t *ctx, + uint8_t *val); + +typedef enum +{ + LSM6DS3TR_C_FIFO_XL_DISABLE = 0, + LSM6DS3TR_C_FIFO_XL_NO_DEC = 1, + LSM6DS3TR_C_FIFO_XL_DEC_2 = 2, + LSM6DS3TR_C_FIFO_XL_DEC_3 = 3, + LSM6DS3TR_C_FIFO_XL_DEC_4 = 4, + LSM6DS3TR_C_FIFO_XL_DEC_8 = 5, + LSM6DS3TR_C_FIFO_XL_DEC_16 = 6, + LSM6DS3TR_C_FIFO_XL_DEC_32 = 7, + LSM6DS3TR_C_FIFO_XL_DEC_ND = 8, /* ERROR CODE */ +} lsm6ds3tr_c_dec_fifo_xl_t; +int32_t lsm6ds3tr_c_fifo_xl_batch_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_dec_fifo_xl_t val); +int32_t lsm6ds3tr_c_fifo_xl_batch_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_dec_fifo_xl_t *val); + +typedef enum +{ + LSM6DS3TR_C_FIFO_GY_DISABLE = 0, + LSM6DS3TR_C_FIFO_GY_NO_DEC = 1, + LSM6DS3TR_C_FIFO_GY_DEC_2 = 2, + LSM6DS3TR_C_FIFO_GY_DEC_3 = 3, + LSM6DS3TR_C_FIFO_GY_DEC_4 = 4, + LSM6DS3TR_C_FIFO_GY_DEC_8 = 5, + LSM6DS3TR_C_FIFO_GY_DEC_16 = 6, + LSM6DS3TR_C_FIFO_GY_DEC_32 = 7, + LSM6DS3TR_C_FIFO_GY_DEC_ND = 8, /* ERROR CODE */ +} lsm6ds3tr_c_dec_fifo_gyro_t; +int32_t lsm6ds3tr_c_fifo_gy_batch_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_dec_fifo_gyro_t val); +int32_t lsm6ds3tr_c_fifo_gy_batch_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_dec_fifo_gyro_t *val); + +typedef enum +{ + LSM6DS3TR_C_FIFO_DS3_DISABLE = 0, + LSM6DS3TR_C_FIFO_DS3_NO_DEC = 1, + LSM6DS3TR_C_FIFO_DS3_DEC_2 = 2, + LSM6DS3TR_C_FIFO_DS3_DEC_3 = 3, + LSM6DS3TR_C_FIFO_DS3_DEC_4 = 4, + LSM6DS3TR_C_FIFO_DS3_DEC_8 = 5, + LSM6DS3TR_C_FIFO_DS3_DEC_16 = 6, + LSM6DS3TR_C_FIFO_DS3_DEC_32 = 7, + LSM6DS3TR_C_FIFO_DS3_DEC_ND = 8, /* ERROR CODE */ +} lsm6ds3tr_c_dec_ds3_fifo_t; +int32_t lsm6ds3tr_c_fifo_dataset_3_batch_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_dec_ds3_fifo_t val); +int32_t lsm6ds3tr_c_fifo_dataset_3_batch_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_dec_ds3_fifo_t *val); + +typedef enum +{ + LSM6DS3TR_C_FIFO_DS4_DISABLE = 0, + LSM6DS3TR_C_FIFO_DS4_NO_DEC = 1, + LSM6DS3TR_C_FIFO_DS4_DEC_2 = 2, + LSM6DS3TR_C_FIFO_DS4_DEC_3 = 3, + LSM6DS3TR_C_FIFO_DS4_DEC_4 = 4, + LSM6DS3TR_C_FIFO_DS4_DEC_8 = 5, + LSM6DS3TR_C_FIFO_DS4_DEC_16 = 6, + LSM6DS3TR_C_FIFO_DS4_DEC_32 = 7, + LSM6DS3TR_C_FIFO_DS4_DEC_ND = 8, /* ERROR CODE */ +} lsm6ds3tr_c_dec_ds4_fifo_t; +int32_t lsm6ds3tr_c_fifo_dataset_4_batch_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_dec_ds4_fifo_t val); +int32_t lsm6ds3tr_c_fifo_dataset_4_batch_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_dec_ds4_fifo_t *val); + +int32_t lsm6ds3tr_c_fifo_xl_gy_8bit_format_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6ds3tr_c_fifo_xl_gy_8bit_format_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6ds3tr_c_fifo_stop_on_wtm_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6ds3tr_c_fifo_stop_on_wtm_get(stmdev_ctx_t *ctx, + uint8_t *val); + +typedef enum +{ + LSM6DS3TR_C_BYPASS_MODE = 0, + LSM6DS3TR_C_FIFO_MODE = 1, + LSM6DS3TR_C_STREAM_TO_FIFO_MODE = 3, + LSM6DS3TR_C_BYPASS_TO_STREAM_MODE = 4, + LSM6DS3TR_C_STREAM_MODE = 6, + LSM6DS3TR_C_FIFO_MODE_ND = 8, /* ERROR CODE */ +} lsm6ds3tr_c_fifo_mode_t; +int32_t lsm6ds3tr_c_fifo_mode_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_fifo_mode_t val); +int32_t lsm6ds3tr_c_fifo_mode_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_fifo_mode_t *val); + +typedef enum +{ + LSM6DS3TR_C_FIFO_DISABLE = 0, + LSM6DS3TR_C_FIFO_12Hz5 = 1, + LSM6DS3TR_C_FIFO_26Hz = 2, + LSM6DS3TR_C_FIFO_52Hz = 3, + LSM6DS3TR_C_FIFO_104Hz = 4, + LSM6DS3TR_C_FIFO_208Hz = 5, + LSM6DS3TR_C_FIFO_416Hz = 6, + LSM6DS3TR_C_FIFO_833Hz = 7, + LSM6DS3TR_C_FIFO_1k66Hz = 8, + LSM6DS3TR_C_FIFO_3k33Hz = 9, + LSM6DS3TR_C_FIFO_6k66Hz = 10, + LSM6DS3TR_C_FIFO_RATE_ND = 11, /* ERROR CODE */ +} lsm6ds3tr_c_odr_fifo_t; +int32_t lsm6ds3tr_c_fifo_data_rate_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_odr_fifo_t val); +int32_t lsm6ds3tr_c_fifo_data_rate_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_odr_fifo_t *val); + +typedef enum +{ + LSM6DS3TR_C_DEN_ACT_LOW = 0, + LSM6DS3TR_C_DEN_ACT_HIGH = 1, + LSM6DS3TR_C_DEN_POL_ND = 2, /* ERROR CODE */ +} lsm6ds3tr_c_den_lh_t; +int32_t lsm6ds3tr_c_den_polarity_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_den_lh_t val); +int32_t lsm6ds3tr_c_den_polarity_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_den_lh_t *val); + +typedef enum +{ + LSM6DS3TR_C_DEN_DISABLE = 0, + LSM6DS3TR_C_LEVEL_FIFO = 6, + LSM6DS3TR_C_LEVEL_LETCHED = 3, + LSM6DS3TR_C_LEVEL_TRIGGER = 2, + LSM6DS3TR_C_EDGE_TRIGGER = 4, + LSM6DS3TR_C_DEN_MODE_ND = 5, /* ERROR CODE */ +} lsm6ds3tr_c_den_mode_t; +int32_t lsm6ds3tr_c_den_mode_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_den_mode_t val); +int32_t lsm6ds3tr_c_den_mode_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_den_mode_t *val); + +typedef enum +{ + LSM6DS3TR_C_STAMP_IN_GY_DATA = 0, + LSM6DS3TR_C_STAMP_IN_XL_DATA = 1, + LSM6DS3TR_C_STAMP_IN_GY_XL_DATA = 2, + LSM6DS3TR_C_DEN_STAMP_ND = 3, /* ERROR CODE */ +} lsm6ds3tr_c_den_xl_en_t; +int32_t lsm6ds3tr_c_den_enable_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_den_xl_en_t val); +int32_t lsm6ds3tr_c_den_enable_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_den_xl_en_t *val); + +int32_t lsm6ds3tr_c_den_mark_axis_z_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6ds3tr_c_den_mark_axis_z_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6ds3tr_c_den_mark_axis_y_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6ds3tr_c_den_mark_axis_y_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6ds3tr_c_den_mark_axis_x_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6ds3tr_c_den_mark_axis_x_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6ds3tr_c_pedo_step_reset_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6ds3tr_c_pedo_step_reset_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6ds3tr_c_pedo_sens_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3tr_c_pedo_sens_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3tr_c_pedo_threshold_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6ds3tr_c_pedo_threshold_get(stmdev_ctx_t *ctx, + uint8_t *val); + +typedef enum +{ + LSM6DS3TR_C_PEDO_AT_2g = 0, + LSM6DS3TR_C_PEDO_AT_4g = 1, + LSM6DS3TR_C_PEDO_FS_ND = 2, /* ERROR CODE */ +} lsm6ds3tr_c_pedo_fs_t; +int32_t lsm6ds3tr_c_pedo_full_scale_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_pedo_fs_t val); +int32_t lsm6ds3tr_c_pedo_full_scale_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_pedo_fs_t *val); + +int32_t lsm6ds3tr_c_pedo_debounce_steps_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6ds3tr_c_pedo_debounce_steps_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6ds3tr_c_pedo_timeout_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3tr_c_pedo_timeout_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3tr_c_pedo_steps_period_set(stmdev_ctx_t *ctx, + uint8_t *buff); +int32_t lsm6ds3tr_c_pedo_steps_period_get(stmdev_ctx_t *ctx, + uint8_t *buff); + +int32_t lsm6ds3tr_c_motion_sens_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3tr_c_motion_sens_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3tr_c_motion_threshold_set(stmdev_ctx_t *ctx, + uint8_t *buff); +int32_t lsm6ds3tr_c_motion_threshold_get(stmdev_ctx_t *ctx, + uint8_t *buff); + +int32_t lsm6ds3tr_c_tilt_sens_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3tr_c_tilt_sens_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3tr_c_wrist_tilt_sens_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6ds3tr_c_wrist_tilt_sens_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6ds3tr_c_tilt_latency_set(stmdev_ctx_t *ctx, + uint8_t *buff); +int32_t lsm6ds3tr_c_tilt_latency_get(stmdev_ctx_t *ctx, + uint8_t *buff); + +int32_t lsm6ds3tr_c_tilt_threshold_set(stmdev_ctx_t *ctx, + uint8_t *buff); +int32_t lsm6ds3tr_c_tilt_threshold_get(stmdev_ctx_t *ctx, + uint8_t *buff); + +int32_t lsm6ds3tr_c_tilt_src_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_a_wrist_tilt_mask_t *val); +int32_t lsm6ds3tr_c_tilt_src_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_a_wrist_tilt_mask_t *val); + +int32_t lsm6ds3tr_c_mag_soft_iron_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3tr_c_mag_soft_iron_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6ds3tr_c_mag_hard_iron_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3tr_c_mag_hard_iron_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6ds3tr_c_mag_soft_iron_mat_set(stmdev_ctx_t *ctx, + uint8_t *buff); +int32_t lsm6ds3tr_c_mag_soft_iron_mat_get(stmdev_ctx_t *ctx, + uint8_t *buff); + +int32_t lsm6ds3tr_c_mag_offset_set(stmdev_ctx_t *ctx, int16_t *val); +int32_t lsm6ds3tr_c_mag_offset_get(stmdev_ctx_t *ctx, int16_t *val); + +int32_t lsm6ds3tr_c_func_en_set(stmdev_ctx_t *ctx, uint8_t val); + +int32_t lsm6ds3tr_c_sh_sync_sens_frame_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6ds3tr_c_sh_sync_sens_frame_get(stmdev_ctx_t *ctx, + uint8_t *val); + +typedef enum +{ + LSM6DS3TR_C_RES_RATIO_2_11 = 0, + LSM6DS3TR_C_RES_RATIO_2_12 = 1, + LSM6DS3TR_C_RES_RATIO_2_13 = 2, + LSM6DS3TR_C_RES_RATIO_2_14 = 3, + LSM6DS3TR_C_RES_RATIO_ND = 4, /* ERROR CODE */ +} lsm6ds3tr_c_rr_t; +int32_t lsm6ds3tr_c_sh_sync_sens_ratio_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_rr_t val); +int32_t lsm6ds3tr_c_sh_sync_sens_ratio_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_rr_t *val); + +int32_t lsm6ds3tr_c_sh_master_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6ds3tr_c_sh_master_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6ds3tr_c_sh_pass_through_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6ds3tr_c_sh_pass_through_get(stmdev_ctx_t *ctx, + uint8_t *val); + +typedef enum +{ + LSM6DS3TR_C_EXT_PULL_UP = 0, + LSM6DS3TR_C_INTERNAL_PULL_UP = 1, + LSM6DS3TR_C_SH_PIN_MODE = 2, /* ERROR CODE */ +} lsm6ds3tr_c_pull_up_en_t; +int32_t lsm6ds3tr_c_sh_pin_mode_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_pull_up_en_t val); +int32_t lsm6ds3tr_c_sh_pin_mode_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_pull_up_en_t *val); + +typedef enum +{ + LSM6DS3TR_C_XL_GY_DRDY = 0, + LSM6DS3TR_C_EXT_ON_INT2_PIN = 1, + LSM6DS3TR_C_SH_SYNCRO_ND = 2, /* ERROR CODE */ +} lsm6ds3tr_c_start_config_t; +int32_t lsm6ds3tr_c_sh_syncro_mode_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_start_config_t val); +int32_t lsm6ds3tr_c_sh_syncro_mode_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_start_config_t *val); + +int32_t lsm6ds3tr_c_sh_drdy_on_int1_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6ds3tr_c_sh_drdy_on_int1_get(stmdev_ctx_t *ctx, + uint8_t *val); + +typedef struct +{ + lsm6ds3tr_c_sensorhub1_reg_t sh_byte_1; + lsm6ds3tr_c_sensorhub2_reg_t sh_byte_2; + lsm6ds3tr_c_sensorhub3_reg_t sh_byte_3; + lsm6ds3tr_c_sensorhub4_reg_t sh_byte_4; + lsm6ds3tr_c_sensorhub5_reg_t sh_byte_5; + lsm6ds3tr_c_sensorhub6_reg_t sh_byte_6; + lsm6ds3tr_c_sensorhub7_reg_t sh_byte_7; + lsm6ds3tr_c_sensorhub8_reg_t sh_byte_8; + lsm6ds3tr_c_sensorhub9_reg_t sh_byte_9; + lsm6ds3tr_c_sensorhub10_reg_t sh_byte_10; + lsm6ds3tr_c_sensorhub11_reg_t sh_byte_11; + lsm6ds3tr_c_sensorhub12_reg_t sh_byte_12; + lsm6ds3tr_c_sensorhub13_reg_t sh_byte_13; + lsm6ds3tr_c_sensorhub14_reg_t sh_byte_14; + lsm6ds3tr_c_sensorhub15_reg_t sh_byte_15; + lsm6ds3tr_c_sensorhub16_reg_t sh_byte_16; + lsm6ds3tr_c_sensorhub17_reg_t sh_byte_17; + lsm6ds3tr_c_sensorhub18_reg_t sh_byte_18; +} lsm6ds3tr_c_emb_sh_read_t; +int32_t lsm6ds3tr_c_sh_read_data_raw_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_emb_sh_read_t *val); + +int32_t lsm6ds3tr_c_sh_cmd_sens_sync_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6ds3tr_c_sh_cmd_sens_sync_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6ds3tr_c_sh_spi_sync_error_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6ds3tr_c_sh_spi_sync_error_get(stmdev_ctx_t *ctx, + uint8_t *val); + +typedef enum +{ + LSM6DS3TR_C_SLV_0 = 0, + LSM6DS3TR_C_SLV_0_1 = 1, + LSM6DS3TR_C_SLV_0_1_2 = 2, + LSM6DS3TR_C_SLV_0_1_2_3 = 3, + LSM6DS3TR_C_SLV_EN_ND = 4, /* ERROR CODE */ +} lsm6ds3tr_c_aux_sens_on_t; +int32_t lsm6ds3tr_c_sh_num_of_dev_connected_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_aux_sens_on_t val); +int32_t lsm6ds3tr_c_sh_num_of_dev_connected_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_aux_sens_on_t *val); + +typedef struct +{ + uint8_t slv0_add; + uint8_t slv0_subadd; + uint8_t slv0_data; +} lsm6ds3tr_c_sh_cfg_write_t; +int32_t lsm6ds3tr_c_sh_cfg_write(stmdev_ctx_t *ctx, + lsm6ds3tr_c_sh_cfg_write_t *val); + +typedef struct +{ + uint8_t slv_add; + uint8_t slv_subadd; + uint8_t slv_len; +} lsm6ds3tr_c_sh_cfg_read_t; +int32_t lsm6ds3tr_c_sh_slv0_cfg_read(stmdev_ctx_t *ctx, + lsm6ds3tr_c_sh_cfg_read_t *val); +int32_t lsm6ds3tr_c_sh_slv1_cfg_read(stmdev_ctx_t *ctx, + lsm6ds3tr_c_sh_cfg_read_t *val); +int32_t lsm6ds3tr_c_sh_slv2_cfg_read(stmdev_ctx_t *ctx, + lsm6ds3tr_c_sh_cfg_read_t *val); +int32_t lsm6ds3tr_c_sh_slv3_cfg_read(stmdev_ctx_t *ctx, + lsm6ds3tr_c_sh_cfg_read_t *val); + +typedef enum +{ + LSM6DS3TR_C_SL0_NO_DEC = 0, + LSM6DS3TR_C_SL0_DEC_2 = 1, + LSM6DS3TR_C_SL0_DEC_4 = 2, + LSM6DS3TR_C_SL0_DEC_8 = 3, + LSM6DS3TR_C_SL0_DEC_ND = 4, /* ERROR CODE */ +} lsm6ds3tr_c_slave0_rate_t; +int32_t lsm6ds3tr_c_sh_slave_0_dec_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_slave0_rate_t val); +int32_t lsm6ds3tr_c_sh_slave_0_dec_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_slave0_rate_t *val); + +typedef enum +{ + LSM6DS3TR_C_EACH_SH_CYCLE = 0, + LSM6DS3TR_C_ONLY_FIRST_CYCLE = 1, + LSM6DS3TR_C_SH_WR_MODE_ND = 2, /* ERROR CODE */ +} lsm6ds3tr_c_write_once_t; +int32_t lsm6ds3tr_c_sh_write_mode_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_write_once_t val); +int32_t lsm6ds3tr_c_sh_write_mode_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_write_once_t *val); + +typedef enum +{ + LSM6DS3TR_C_SL1_NO_DEC = 0, + LSM6DS3TR_C_SL1_DEC_2 = 1, + LSM6DS3TR_C_SL1_DEC_4 = 2, + LSM6DS3TR_C_SL1_DEC_8 = 3, + LSM6DS3TR_C_SL1_DEC_ND = 4, /* ERROR CODE */ +} lsm6ds3tr_c_slave1_rate_t; +int32_t lsm6ds3tr_c_sh_slave_1_dec_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_slave1_rate_t val); +int32_t lsm6ds3tr_c_sh_slave_1_dec_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_slave1_rate_t *val); + +typedef enum +{ + LSM6DS3TR_C_SL2_NO_DEC = 0, + LSM6DS3TR_C_SL2_DEC_2 = 1, + LSM6DS3TR_C_SL2_DEC_4 = 2, + LSM6DS3TR_C_SL2_DEC_8 = 3, + LSM6DS3TR_C_SL2_DEC_ND = 4, /* ERROR CODE */ +} lsm6ds3tr_c_slave2_rate_t; +int32_t lsm6ds3tr_c_sh_slave_2_dec_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_slave2_rate_t val); +int32_t lsm6ds3tr_c_sh_slave_2_dec_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_slave2_rate_t *val); + +typedef enum +{ + LSM6DS3TR_C_SL3_NO_DEC = 0, + LSM6DS3TR_C_SL3_DEC_2 = 1, + LSM6DS3TR_C_SL3_DEC_4 = 2, + LSM6DS3TR_C_SL3_DEC_8 = 3, + LSM6DS3TR_C_SL3_DEC_ND = 4, /* ERROR CODE */ +} lsm6ds3tr_c_slave3_rate_t; +int32_t lsm6ds3tr_c_sh_slave_3_dec_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_slave3_rate_t val); +int32_t lsm6ds3tr_c_sh_slave_3_dec_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_slave3_rate_t *val); + +/** + * @} + * + */ + +#ifdef __cplusplus +} +#endif + +#endif /* LSM6DS3TR_C_DRIVER_H */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/inc/reg/lsm6dso_reg.h b/inc/reg/lsm6dso_reg.h new file mode 100644 index 0000000..9ac7fbe --- /dev/null +++ b/inc/reg/lsm6dso_reg.h @@ -0,0 +1,4475 @@ +/** + ****************************************************************************** + * @file lsm6dso_reg.h + * @author Sensors Software Solution Team + * @brief This file contains all the functions prototypes for the + * lsm6dso_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 LSM6DSO_REGS_H +#define LSM6DSO_REGS_H + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include +#include +#include +#include "lsm6ds_pinout.h" + +/** @addtogroup LSM6DSO + * @{ + * + */ + +/** @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) + +/** @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 (*stmdev_write_ptr)(lsm6ds_config_t*, uint8_t, const uint8_t *, uint16_t); +typedef int32_t (*stmdev_read_ptr)(lsm6ds_config_t*, uint8_t, uint8_t *, uint16_t); +typedef void (*stmdev_mdelay_ptr)(uint32_t millisec); + +typedef struct +{ + /** Component mandatory fields **/ + stmdev_write_ptr write_reg; + stmdev_read_ptr read_reg; + /** Component optional fields **/ + stmdev_mdelay_ptr mdelay; + /** Customizable optional pointer **/ + lsm6ds_config_t *handle; +} stmdev_ctx_t; + +#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 lsm6dso_read_reg(stmdev_ctx_t* ctx, uint8_t reg, + uint8_t* data, + uint16_t len); +int32_t lsm6dso_write_reg(stmdev_ctx_t* ctx, uint8_t reg, + uint8_t* data, + uint16_t len); + +/** + * @} + * + */ + +#endif /* MEMS_SHARED_TYPES */ + +#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 LSM6DSO_Infos + * @{ + * + */ + +/** I2C Device Address 8 bit format if SA0=0 -> D5 if SA0=1 -> D7 **/ +#define LSM6DSO_I2C_ADD_L 0xD5 +#define LSM6DSO_I2C_ADD_H 0xD7 + +/** Device Identification (Who am I) **/ +#define LSM6DSO_ID 0x6C + +/** + * @} + * + */ + +#define LSM6DSO_FUNC_CFG_ACCESS 0x01U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_01 : 6; +uint8_t reg_access : + 2; /* shub_reg_access + func_cfg_access */ +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN +uint8_t reg_access : + 2; /* shub_reg_access + func_cfg_access */ + uint8_t not_used_01 : 6; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_func_cfg_access_t; + +#define LSM6DSO_PIN_CTRL 0x02U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_01 : 6; + 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 not_used_01 : 6; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_pin_ctrl_t; + +#define LSM6DSO_FIFO_CTRL1 0x07U +typedef struct +{ + uint8_t wtm : 8; +} lsm6dso_fifo_ctrl1_t; + +#define LSM6DSO_FIFO_CTRL2 0x08U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t wtm : 1; + uint8_t uncoptr_rate : 2; + uint8_t not_used_01 : 1; + uint8_t odrchg_en : 1; + uint8_t not_used_02 : 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_used_02 : 1; + uint8_t odrchg_en : 1; + uint8_t not_used_01 : 1; + uint8_t uncoptr_rate : 2; + uint8_t wtm : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_fifo_ctrl2_t; + +#define LSM6DSO_FIFO_CTRL3 0x09U +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 */ +} lsm6dso_fifo_ctrl3_t; + +#define LSM6DSO_FIFO_CTRL4 0x0AU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fifo_mode : 3; + uint8_t not_used_01 : 1; + uint8_t odr_t_batch : 2; + uint8_t odr_ts_batch : 2; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t odr_ts_batch : 2; + uint8_t odr_t_batch : 2; + uint8_t not_used_01 : 1; + uint8_t fifo_mode : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_fifo_ctrl4_t; + +#define LSM6DSO_COUNTER_BDR_REG1 0x0BU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t cnt_bdr_th : 3; + uint8_t not_used_01 : 2; + uint8_t trig_counter_bdr : 1; + uint8_t rst_counter_bdr : 1; + uint8_t dataready_pulsed : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t dataready_pulsed : 1; + uint8_t rst_counter_bdr : 1; + uint8_t trig_counter_bdr : 1; + uint8_t not_used_01 : 2; + uint8_t cnt_bdr_th : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_counter_bdr_reg1_t; + +#define LSM6DSO_COUNTER_BDR_REG2 0x0CU +typedef struct +{ + uint8_t cnt_bdr_th : 8; +} lsm6dso_counter_bdr_reg2_t; + +#define LSM6DSO_INT1_CTRL 0x0D +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t int1_drdy_xl : 1; + uint8_t int1_drdy_g : 1; + uint8_t int1_boot : 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 den_drdy_flag : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t den_drdy_flag : 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 int1_boot : 1; + uint8_t int1_drdy_g : 1; + uint8_t int1_drdy_xl : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_int1_ctrl_t; + +#define LSM6DSO_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_temp : 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 not_used_01 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 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_temp : 1; + uint8_t int2_drdy_g : 1; + uint8_t int2_drdy_xl : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_int2_ctrl_t; + +#define LSM6DSO_WHO_AM_I 0x0FU +#define LSM6DSO_CTRL1_XL 0x10U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_01 : 1; + uint8_t lpf2_xl_en : 1; + uint8_t fs_xl : 2; + uint8_t odr_xl : 4; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t odr_xl : 4; + uint8_t fs_xl : 2; + uint8_t lpf2_xl_en : 1; + uint8_t not_used_01 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_ctrl1_xl_t; + +#define LSM6DSO_CTRL2_G 0x11U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_01 : 1; + uint8_t fs_g : 3; /* fs_125 + fs_g */ + uint8_t odr_g : 4; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t odr_g : 4; + uint8_t fs_g : 3; /* fs_125 + fs_g */ + uint8_t not_used_01 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_ctrl2_g_t; + +#define LSM6DSO_CTRL3_C 0x12U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sw_reset : 1; + uint8_t not_used_01 : 1; + uint8_t if_inc : 1; + uint8_t sim : 1; + uint8_t pp_od : 1; + uint8_t h_lactive : 1; + 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 h_lactive : 1; + uint8_t pp_od : 1; + uint8_t sim : 1; + uint8_t if_inc : 1; + uint8_t not_used_01 : 1; + uint8_t sw_reset : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_ctrl3_c_t; + +#define LSM6DSO_CTRL4_C 0x13U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_01 : 1; + uint8_t lpf1_sel_g : 1; + uint8_t i2c_disable : 1; + uint8_t drdy_mask : 1; + uint8_t not_used_02 : 1; + uint8_t int2_on_int1 : 1; + uint8_t sleep_g : 1; + uint8_t not_used_03 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_03 : 1; + uint8_t sleep_g : 1; + uint8_t int2_on_int1 : 1; + uint8_t not_used_02 : 1; + uint8_t drdy_mask : 1; + uint8_t i2c_disable : 1; + uint8_t lpf1_sel_g : 1; + uint8_t not_used_01 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_ctrl4_c_t; + +#define LSM6DSO_CTRL5_C 0x14U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t st_xl : 2; + uint8_t st_g : 2; + uint8_t not_used_01 : 1; + uint8_t rounding : 2; + uint8_t xl_ulp_en : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t xl_ulp_en : 1; + uint8_t rounding : 2; + uint8_t not_used_01 : 1; + uint8_t st_g : 2; + uint8_t st_xl : 2; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_ctrl5_c_t; + +#define LSM6DSO_CTRL6_C 0x15U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ftype : 3; + uint8_t usr_off_w : 1; + uint8_t xl_hm_mode : 1; +uint8_t den_mode : + 3; /* trig_en + lvl1_en + lvl2_en */ +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN +uint8_t den_mode : + 3; /* trig_en + lvl1_en + lvl2_en */ + uint8_t xl_hm_mode : 1; + uint8_t usr_off_w : 1; + uint8_t ftype : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_ctrl6_c_t; + +#define LSM6DSO_CTRL7_G 0x16U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ois_on : 1; + uint8_t usr_off_on_out : 1; + uint8_t ois_on_en : 1; + uint8_t not_used_01 : 1; + uint8_t hpm_g : 2; + uint8_t hp_en_g : 1; + uint8_t g_hm_mode : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t g_hm_mode : 1; + uint8_t hp_en_g : 1; + uint8_t hpm_g : 2; + uint8_t not_used_01 : 1; + uint8_t ois_on_en : 1; + uint8_t usr_off_on_out : 1; + uint8_t ois_on : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_ctrl7_g_t; + +#define LSM6DSO_CTRL8_XL 0x17U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t low_pass_on_6d : 1; + uint8_t xl_fs_mode : 1; + uint8_t hp_slope_xl_en : 1; + uint8_t fastsettl_mode_xl : 1; + uint8_t hp_ref_mode_xl : 1; + uint8_t hpcf_xl : 3; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t hpcf_xl : 3; + uint8_t hp_ref_mode_xl : 1; + uint8_t fastsettl_mode_xl : 1; + uint8_t hp_slope_xl_en : 1; + uint8_t xl_fs_mode : 1; + uint8_t low_pass_on_6d : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_ctrl8_xl_t; + +#define LSM6DSO_CTRL9_XL 0x18U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_01 : 1; + uint8_t i3c_disable : 1; + uint8_t den_lh : 1; + uint8_t den_xl_g : 2; /* den_xl_en + den_xl_g */ + uint8_t den_z : 1; + uint8_t den_y : 1; + uint8_t den_x : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t den_x : 1; + uint8_t den_y : 1; + uint8_t den_z : 1; + uint8_t den_xl_g : 2; /* den_xl_en + den_xl_g */ + uint8_t den_lh : 1; + uint8_t i3c_disable : 1; + uint8_t not_used_01 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_ctrl9_xl_t; + +#define LSM6DSO_CTRL10_C 0x19U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_01 : 5; + uint8_t timestamp_en : 1; + uint8_t not_used_02 : 2; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_02 : 2; + uint8_t timestamp_en : 1; + uint8_t not_used_01 : 5; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_ctrl10_c_t; + +#define LSM6DSO_ALL_INT_SRC 0x1AU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ff_ia : 1; + uint8_t wu_ia : 1; + uint8_t single_tap : 1; + uint8_t double_tap : 1; + uint8_t d6d_ia : 1; + uint8_t sleep_change_ia : 1; + uint8_t not_used_01 : 1; + uint8_t timestamp_endcount : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t timestamp_endcount : 1; + uint8_t not_used_01 : 1; + uint8_t sleep_change_ia : 1; + uint8_t d6d_ia : 1; + uint8_t double_tap : 1; + uint8_t single_tap : 1; + uint8_t wu_ia : 1; + uint8_t ff_ia : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_all_int_src_t; + +#define LSM6DSO_WAKE_UP_SRC 0x1BU +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_used_01 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 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 */ +} lsm6dso_wake_up_src_t; + +#define LSM6DSO_TAP_SRC 0x1CU +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_used_02 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_02 : 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 */ +} lsm6dso_tap_src_t; + +#define LSM6DSO_D6D_SRC 0x1DU +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 den_drdy : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t den_drdy : 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 */ +} lsm6dso_d6d_src_t; + +#define LSM6DSO_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 not_used_01 : 5; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 5; + uint8_t tda : 1; + uint8_t gda : 1; + uint8_t xlda : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_status_reg_t; + +#define LSM6DSO_STATUS_SPIAUX 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_used_01 : 5; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 5; + uint8_t gyro_settling : 1; + uint8_t gda : 1; + uint8_t xlda : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_status_spiaux_t; + +#define LSM6DSO_OUT_TEMP_L 0x20U +#define LSM6DSO_OUT_TEMP_H 0x21U +#define LSM6DSO_OUTX_L_G 0x22U +#define LSM6DSO_OUTX_H_G 0x23U +#define LSM6DSO_OUTY_L_G 0x24U +#define LSM6DSO_OUTY_H_G 0x25U +#define LSM6DSO_OUTZ_L_G 0x26U +#define LSM6DSO_OUTZ_H_G 0x27U +#define LSM6DSO_OUTX_L_A 0x28U +#define LSM6DSO_OUTX_H_A 0x29U +#define LSM6DSO_OUTY_L_A 0x2AU +#define LSM6DSO_OUTY_H_A 0x2BU +#define LSM6DSO_OUTZ_L_A 0x2CU +#define LSM6DSO_OUTZ_H_A 0x2DU +#define LSM6DSO_EMB_FUNC_STATUS_MAINPAGE 0x35U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_01 : 3; + uint8_t is_step_det : 1; + uint8_t is_tilt : 1; + uint8_t is_sigmot : 1; + uint8_t not_used_02 : 1; + uint8_t is_fsm_lc : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t is_fsm_lc : 1; + uint8_t not_used_02 : 1; + uint8_t is_sigmot : 1; + uint8_t is_tilt : 1; + uint8_t is_step_det : 1; + uint8_t not_used_01 : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_emb_func_status_mainpage_t; + +#define LSM6DSO_FSM_STATUS_A_MAINPAGE 0x36U +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 */ +} lsm6dso_fsm_status_a_mainpage_t; + +#define LSM6DSO_FSM_STATUS_B_MAINPAGE 0x37U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t is_fsm9 : 1; + uint8_t is_fsm10 : 1; + uint8_t is_fsm11 : 1; + uint8_t is_fsm12 : 1; + uint8_t is_fsm13 : 1; + uint8_t is_fsm14 : 1; + uint8_t is_fsm15 : 1; + uint8_t is_fsm16 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t is_fsm16 : 1; + uint8_t is_fsm15 : 1; + uint8_t is_fsm14 : 1; + uint8_t is_fsm13 : 1; + uint8_t is_fsm12 : 1; + uint8_t is_fsm11 : 1; + uint8_t is_fsm10 : 1; + uint8_t is_fsm9 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_fsm_status_b_mainpage_t; + +#define LSM6DSO_STATUS_MASTER_MAINPAGE 0x39U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sens_hub_endop : 1; + uint8_t not_used_01 : 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_used_01 : 2; + uint8_t sens_hub_endop : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_status_master_mainpage_t; + +#define LSM6DSO_FIFO_STATUS1 0x3AU +typedef struct +{ + uint8_t diff_fifo : 8; +} lsm6dso_fifo_status1_t; + +#define LSM6DSO_FIFO_STATUS2 0x3B +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t diff_fifo : 2; + uint8_t not_used_01 : 1; + uint8_t over_run_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 over_run_latched : 1; + uint8_t not_used_01 : 1; + uint8_t diff_fifo : 2; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_fifo_status2_t; + +#define LSM6DSO_TIMESTAMP0 0x40U +#define LSM6DSO_TIMESTAMP1 0x41U +#define LSM6DSO_TIMESTAMP2 0x42U +#define LSM6DSO_TIMESTAMP3 0x43U +#define LSM6DSO_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 sleep_status_on_int : 1; + uint8_t int_clr_on_read : 1; + uint8_t not_used_01 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 1; + uint8_t int_clr_on_read : 1; + uint8_t sleep_status_on_int : 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 */ +} lsm6dso_tap_cfg0_t; + +#define LSM6DSO_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 */ +} lsm6dso_tap_cfg1_t; + +#define LSM6DSO_TAP_CFG2 0x58U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t tap_ths_y : 5; + uint8_t inact_en : 2; + uint8_t interrupts_enable : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t interrupts_enable : 1; + uint8_t inact_en : 2; + uint8_t tap_ths_y : 5; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_tap_cfg2_t; + +#define LSM6DSO_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 */ +} lsm6dso_tap_ths_6d_t; + +#define LSM6DSO_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 */ +} lsm6dso_int_dur2_t; + +#define LSM6DSO_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 */ +} lsm6dso_wake_up_ths_t; + +#define LSM6DSO_WAKE_UP_DUR 0x5CU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sleep_dur : 4; + uint8_t wake_ths_w : 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 wake_ths_w : 1; + uint8_t sleep_dur : 4; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_wake_up_dur_t; + +#define LSM6DSO_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 */ +} lsm6dso_free_fall_t; + +#define LSM6DSO_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 */ +} lsm6dso_md1_cfg_t; + +#define LSM6DSO_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 */ +} lsm6dso_md2_cfg_t; + +#define LSM6DSO_I3C_BUS_AVB 0x62U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t pd_dis_int1 : 1; + uint8_t not_used_01 : 2; + uint8_t i3c_bus_avb_sel : 2; + uint8_t not_used_02 : 3; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_02 : 3; + uint8_t i3c_bus_avb_sel : 2; + uint8_t not_used_01 : 2; + uint8_t pd_dis_int1 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_i3c_bus_avb_t; + +#define LSM6DSO_INTERNAL_FREQ_FINE 0x63U +typedef struct +{ + uint8_t freq_fine : 8; +} lsm6dso_internal_freq_fine_t; + +#define LSM6DSO_INT_OIS 0x6FU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t st_xl_ois : 2; + uint8_t not_used_01 : 3; + uint8_t den_lh_ois : 1; + uint8_t lvl2_ois : 1; + uint8_t int2_drdy_ois : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t int2_drdy_ois : 1; + uint8_t lvl2_ois : 1; + uint8_t den_lh_ois : 1; + uint8_t not_used_01 : 3; + uint8_t st_xl_ois : 2; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_int_ois_t; + +#define LSM6DSO_CTRL1_OIS 0x70U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ois_en_spi2 : 1; + uint8_t fs_g_ois : 3; /* fs_125_ois + fs[1:0]_g_ois */ + uint8_t mode4_en : 1; + uint8_t sim_ois : 1; + uint8_t lvl1_ois : 1; + uint8_t not_used_01 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 1; + uint8_t lvl1_ois : 1; + uint8_t sim_ois : 1; + uint8_t mode4_en : 1; + uint8_t fs_g_ois : 3; /* fs_125_ois + fs[1:0]_g_ois */ + uint8_t ois_en_spi2 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_ctrl1_ois_t; + +#define LSM6DSO_CTRL2_OIS 0x71U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t hp_en_ois : 1; + uint8_t ftype_ois : 2; + uint8_t not_used_01 : 1; + uint8_t hpm_ois : 2; + uint8_t not_used_02 : 2; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_02 : 2; + uint8_t hpm_ois : 2; + uint8_t not_used_01 : 1; + uint8_t ftype_ois : 2; + uint8_t hp_en_ois : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_ctrl2_ois_t; + +#define LSM6DSO_CTRL3_OIS 0x72U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t st_ois_clampdis : 1; + uint8_t st_ois : 2; + uint8_t filter_xl_conf_ois : 3; + uint8_t fs_xl_ois : 2; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fs_xl_ois : 2; + uint8_t filter_xl_conf_ois : 3; + uint8_t st_ois : 2; + uint8_t st_ois_clampdis : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_ctrl3_ois_t; + +#define LSM6DSO_X_OFS_USR 0x73U +#define LSM6DSO_Y_OFS_USR 0x74U +#define LSM6DSO_Z_OFS_USR 0x75U +#define LSM6DSO_FIFO_DATA_OUT_TAG 0x78U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t tag_parity : 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 tag_parity : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_fifo_data_out_tag_t; + +#define LSM6DSO_FIFO_DATA_OUT_X_L 0x79U +#define LSM6DSO_FIFO_DATA_OUT_X_H 0x7AU +#define LSM6DSO_FIFO_DATA_OUT_Y_L 0x7BU +#define LSM6DSO_FIFO_DATA_OUT_Y_H 0x7CU +#define LSM6DSO_FIFO_DATA_OUT_Z_L 0x7DU +#define LSM6DSO_FIFO_DATA_OUT_Z_H 0x7EU +#define LSM6DSO_PAGE_SEL 0x02U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_01 : 4; + uint8_t page_sel : 4; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t page_sel : 4; + uint8_t not_used_01 : 4; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_page_sel_t; + +#define LSM6DSO_EMB_FUNC_EN_A 0x04U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_01 : 3; + uint8_t pedo_en : 1; + uint8_t tilt_en : 1; + uint8_t sign_motion_en : 1; + uint8_t not_used_02 : 2; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_02 : 2; + uint8_t sign_motion_en : 1; + uint8_t tilt_en : 1; + uint8_t pedo_en : 1; + uint8_t not_used_01 : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_emb_func_en_a_t; + +#define LSM6DSO_EMB_FUNC_EN_B 0x05U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm_en : 1; + uint8_t not_used_01 : 2; + uint8_t fifo_compr_en : 1; + uint8_t pedo_adv_en : 1; + uint8_t not_used_02 : 3; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_02 : 3; + uint8_t pedo_adv_en : 1; + uint8_t fifo_compr_en : 1; + uint8_t not_used_01 : 2; + uint8_t fsm_en : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_emb_func_en_b_t; + +#define LSM6DSO_PAGE_ADDRESS 0x08U +typedef struct +{ + uint8_t page_addr : 8; +} lsm6dso_page_address_t; + +#define LSM6DSO_PAGE_VALUE 0x09U +typedef struct +{ + uint8_t page_value : 8; +} lsm6dso_page_value_t; + +#define LSM6DSO_EMB_FUNC_INT1 0x0AU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_01 : 3; + uint8_t int1_step_detector : 1; + uint8_t int1_tilt : 1; + uint8_t int1_sig_mot : 1; + uint8_t not_used_02 : 1; + uint8_t int1_fsm_lc : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t int1_fsm_lc : 1; + uint8_t not_used_02 : 1; + uint8_t int1_sig_mot : 1; + uint8_t int1_tilt : 1; + uint8_t int1_step_detector : 1; + uint8_t not_used_01 : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_emb_func_int1_t; + +#define LSM6DSO_FSM_INT1_A 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 */ +} lsm6dso_fsm_int1_a_t; + +#define LSM6DSO_FSM_INT1_B 0x0CU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t int1_fsm9 : 1; + uint8_t int1_fsm10 : 1; + uint8_t int1_fsm11 : 1; + uint8_t int1_fsm12 : 1; + uint8_t int1_fsm13 : 1; + uint8_t int1_fsm14 : 1; + uint8_t int1_fsm15 : 1; + uint8_t int1_fsm16 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t int1_fsm16 : 1; + uint8_t int1_fsm15 : 1; + uint8_t int1_fsm14 : 1; + uint8_t int1_fsm13 : 1; + uint8_t int1_fsm12 : 1; + uint8_t int1_fsm11 : 1; + uint8_t int1_fsm10 : 1; + uint8_t int1_fsm9 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_fsm_int1_b_t; + +#define LSM6DSO_EMB_FUNC_INT2 0x0EU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_01 : 3; + uint8_t int2_step_detector : 1; + uint8_t int2_tilt : 1; + uint8_t int2_sig_mot : 1; + uint8_t not_used_02 : 1; + uint8_t int2_fsm_lc : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t int2_fsm_lc : 1; + uint8_t not_used_02 : 1; + uint8_t int2_sig_mot : 1; + uint8_t int2_tilt : 1; + uint8_t int2_step_detector : 1; + uint8_t not_used_01 : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_emb_func_int2_t; + +#define LSM6DSO_FSM_INT2_A 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 */ +} lsm6dso_fsm_int2_a_t; + +#define LSM6DSO_FSM_INT2_B 0x10U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t int2_fsm9 : 1; + uint8_t int2_fsm10 : 1; + uint8_t int2_fsm11 : 1; + uint8_t int2_fsm12 : 1; + uint8_t int2_fsm13 : 1; + uint8_t int2_fsm14 : 1; + uint8_t int2_fsm15 : 1; + uint8_t int2_fsm16 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t int2_fsm16 : 1; + uint8_t int2_fsm15 : 1; + uint8_t int2_fsm14 : 1; + uint8_t int2_fsm13 : 1; + uint8_t int2_fsm12 : 1; + uint8_t int2_fsm11 : 1; + uint8_t int2_fsm10 : 1; + uint8_t int2_fsm9 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_fsm_int2_b_t; + +#define LSM6DSO_EMB_FUNC_STATUS 0x12U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_01 : 3; + uint8_t is_step_det : 1; + uint8_t is_tilt : 1; + uint8_t is_sigmot : 1; + uint8_t not_used_02 : 1; + uint8_t is_fsm_lc : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t is_fsm_lc : 1; + uint8_t not_used_02 : 1; + uint8_t is_sigmot : 1; + uint8_t is_tilt : 1; + uint8_t is_step_det : 1; + uint8_t not_used_01 : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_emb_func_status_t; + +#define LSM6DSO_FSM_STATUS_A 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 */ +} lsm6dso_fsm_status_a_t; + +#define LSM6DSO_FSM_STATUS_B 0x14U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t is_fsm9 : 1; + uint8_t is_fsm10 : 1; + uint8_t is_fsm11 : 1; + uint8_t is_fsm12 : 1; + uint8_t is_fsm13 : 1; + uint8_t is_fsm14 : 1; + uint8_t is_fsm15 : 1; + uint8_t is_fsm16 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t is_fsm16 : 1; + uint8_t is_fsm15 : 1; + uint8_t is_fsm14 : 1; + uint8_t is_fsm13 : 1; + uint8_t is_fsm12 : 1; + uint8_t is_fsm11 : 1; + uint8_t is_fsm10 : 1; + uint8_t is_fsm9 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_fsm_status_b_t; + +#define LSM6DSO_PAGE_RW 0x17U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_01 : 5; + uint8_t page_rw : 2; /* page_write + page_read */ + uint8_t emb_func_lir : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t emb_func_lir : 1; + uint8_t page_rw : 2; /* page_write + page_read */ + uint8_t not_used_01 : 5; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_page_rw_t; + +#define LSM6DSO_EMB_FUNC_FIFO_CFG 0x44U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_00 : 6; + uint8_t pedo_fifo_en : 1; + uint8_t not_used_01 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 1; + uint8_t pedo_fifo_en : 1; + uint8_t not_used_00 : 6; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_emb_func_fifo_cfg_t; + +#define LSM6DSO_FSM_ENABLE_A 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 */ +} lsm6dso_fsm_enable_a_t; + +#define LSM6DSO_FSM_ENABLE_B 0x47U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm9_en : 1; + uint8_t fsm10_en : 1; + uint8_t fsm11_en : 1; + uint8_t fsm12_en : 1; + uint8_t fsm13_en : 1; + uint8_t fsm14_en : 1; + uint8_t fsm15_en : 1; + uint8_t fsm16_en : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm16_en : 1; + uint8_t fsm15_en : 1; + uint8_t fsm14_en : 1; + uint8_t fsm13_en : 1; + uint8_t fsm12_en : 1; + uint8_t fsm11_en : 1; + uint8_t fsm10_en : 1; + uint8_t fsm9_en : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_fsm_enable_b_t; + +#define LSM6DSO_FSM_LONG_COUNTER_L 0x48U +#define LSM6DSO_FSM_LONG_COUNTER_H 0x49U +#define LSM6DSO_FSM_LONG_COUNTER_CLEAR 0x4AU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN +uint8_t fsm_lc_clr : + 2; /* fsm_lc_cleared + fsm_lc_clear */ + uint8_t not_used_01 : 6; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 6; +uint8_t fsm_lc_clr : + 2; /* fsm_lc_cleared + fsm_lc_clear */ +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_fsm_long_counter_clear_t; + +#define LSM6DSO_FSM_OUTS1 0x4CU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t n_v : 1; + uint8_t p_v : 1; + uint8_t n_z : 1; + uint8_t p_z : 1; + uint8_t n_y : 1; + uint8_t p_y : 1; + uint8_t n_x : 1; + uint8_t p_x : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t p_x : 1; + uint8_t n_x : 1; + uint8_t p_y : 1; + uint8_t n_y : 1; + uint8_t p_z : 1; + uint8_t n_z : 1; + uint8_t p_v : 1; + uint8_t n_v : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_fsm_outs1_t; + +#define LSM6DSO_FSM_OUTS2 0x4DU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t n_v : 1; + uint8_t p_v : 1; + uint8_t n_z : 1; + uint8_t p_z : 1; + uint8_t n_y : 1; + uint8_t p_y : 1; + uint8_t n_x : 1; + uint8_t p_x : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t p_x : 1; + uint8_t n_x : 1; + uint8_t p_y : 1; + uint8_t n_y : 1; + uint8_t p_z : 1; + uint8_t n_z : 1; + uint8_t p_v : 1; + uint8_t n_v : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_fsm_outs2_t; + +#define LSM6DSO_FSM_OUTS3 0x4EU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t n_v : 1; + uint8_t p_v : 1; + uint8_t n_z : 1; + uint8_t p_z : 1; + uint8_t n_y : 1; + uint8_t p_y : 1; + uint8_t n_x : 1; + uint8_t p_x : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t p_x : 1; + uint8_t n_x : 1; + uint8_t p_y : 1; + uint8_t n_y : 1; + uint8_t p_z : 1; + uint8_t n_z : 1; + uint8_t p_v : 1; + uint8_t n_v : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_fsm_outs3_t; + +#define LSM6DSO_FSM_OUTS4 0x4FU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t n_v : 1; + uint8_t p_v : 1; + uint8_t n_z : 1; + uint8_t p_z : 1; + uint8_t n_y : 1; + uint8_t p_y : 1; + uint8_t n_x : 1; + uint8_t p_x : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t p_x : 1; + uint8_t n_x : 1; + uint8_t p_y : 1; + uint8_t n_y : 1; + uint8_t p_z : 1; + uint8_t n_z : 1; + uint8_t p_v : 1; + uint8_t n_v : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_fsm_outs4_t; + +#define LSM6DSO_FSM_OUTS5 0x50U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t n_v : 1; + uint8_t p_v : 1; + uint8_t n_z : 1; + uint8_t p_z : 1; + uint8_t n_y : 1; + uint8_t p_y : 1; + uint8_t n_x : 1; + uint8_t p_x : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t p_x : 1; + uint8_t n_x : 1; + uint8_t p_y : 1; + uint8_t n_y : 1; + uint8_t p_z : 1; + uint8_t n_z : 1; + uint8_t p_v : 1; + uint8_t n_v : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_fsm_outs5_t; + +#define LSM6DSO_FSM_OUTS6 0x51U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t n_v : 1; + uint8_t p_v : 1; + uint8_t n_z : 1; + uint8_t p_z : 1; + uint8_t n_y : 1; + uint8_t p_y : 1; + uint8_t n_x : 1; + uint8_t p_x : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t p_x : 1; + uint8_t n_x : 1; + uint8_t p_y : 1; + uint8_t n_y : 1; + uint8_t p_z : 1; + uint8_t n_z : 1; + uint8_t p_v : 1; + uint8_t n_v : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_fsm_outs6_t; + +#define LSM6DSO_FSM_OUTS7 0x52U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t n_v : 1; + uint8_t p_v : 1; + uint8_t n_z : 1; + uint8_t p_z : 1; + uint8_t n_y : 1; + uint8_t p_y : 1; + uint8_t n_x : 1; + uint8_t p_x : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t p_x : 1; + uint8_t n_x : 1; + uint8_t p_y : 1; + uint8_t n_y : 1; + uint8_t p_z : 1; + uint8_t n_z : 1; + uint8_t p_v : 1; + uint8_t n_v : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_fsm_outs7_t; + +#define LSM6DSO_FSM_OUTS8 0x53U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t n_v : 1; + uint8_t p_v : 1; + uint8_t n_z : 1; + uint8_t p_z : 1; + uint8_t n_y : 1; + uint8_t p_y : 1; + uint8_t n_x : 1; + uint8_t p_x : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t p_x : 1; + uint8_t n_x : 1; + uint8_t p_y : 1; + uint8_t n_y : 1; + uint8_t p_z : 1; + uint8_t n_z : 1; + uint8_t p_v : 1; + uint8_t n_v : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_fsm_outs8_t; + +#define LSM6DSO_FSM_OUTS9 0x54U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t n_v : 1; + uint8_t p_v : 1; + uint8_t n_z : 1; + uint8_t p_z : 1; + uint8_t n_y : 1; + uint8_t p_y : 1; + uint8_t n_x : 1; + uint8_t p_x : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t p_x : 1; + uint8_t n_x : 1; + uint8_t p_y : 1; + uint8_t n_y : 1; + uint8_t p_z : 1; + uint8_t n_z : 1; + uint8_t p_v : 1; + uint8_t n_v : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_fsm_outs9_t; + +#define LSM6DSO_FSM_OUTS10 0x55U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t n_v : 1; + uint8_t p_v : 1; + uint8_t n_z : 1; + uint8_t p_z : 1; + uint8_t n_y : 1; + uint8_t p_y : 1; + uint8_t n_x : 1; + uint8_t p_x : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t p_x : 1; + uint8_t n_x : 1; + uint8_t p_y : 1; + uint8_t n_y : 1; + uint8_t p_z : 1; + uint8_t n_z : 1; + uint8_t p_v : 1; + uint8_t n_v : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_fsm_outs10_t; + +#define LSM6DSO_FSM_OUTS11 0x56U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t n_v : 1; + uint8_t p_v : 1; + uint8_t n_z : 1; + uint8_t p_z : 1; + uint8_t n_y : 1; + uint8_t p_y : 1; + uint8_t n_x : 1; + uint8_t p_x : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t p_x : 1; + uint8_t n_x : 1; + uint8_t p_y : 1; + uint8_t n_y : 1; + uint8_t p_z : 1; + uint8_t n_z : 1; + uint8_t p_v : 1; + uint8_t n_v : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_fsm_outs11_t; + +#define LSM6DSO_FSM_OUTS12 0x57U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t n_v : 1; + uint8_t p_v : 1; + uint8_t n_z : 1; + uint8_t p_z : 1; + uint8_t n_y : 1; + uint8_t p_y : 1; + uint8_t n_x : 1; + uint8_t p_x : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t p_x : 1; + uint8_t n_x : 1; + uint8_t p_y : 1; + uint8_t n_y : 1; + uint8_t p_z : 1; + uint8_t n_z : 1; + uint8_t p_v : 1; + uint8_t n_v : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_fsm_outs12_t; + +#define LSM6DSO_FSM_OUTS13 0x58U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t n_v : 1; + uint8_t p_v : 1; + uint8_t n_z : 1; + uint8_t p_z : 1; + uint8_t n_y : 1; + uint8_t p_y : 1; + uint8_t n_x : 1; + uint8_t p_x : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t p_x : 1; + uint8_t n_x : 1; + uint8_t p_y : 1; + uint8_t n_y : 1; + uint8_t p_z : 1; + uint8_t n_z : 1; + uint8_t p_v : 1; + uint8_t n_v : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_fsm_outs13_t; + +#define LSM6DSO_FSM_OUTS14 0x59U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t n_v : 1; + uint8_t p_v : 1; + uint8_t n_z : 1; + uint8_t p_z : 1; + uint8_t n_y : 1; + uint8_t p_y : 1; + uint8_t n_x : 1; + uint8_t p_x : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t p_x : 1; + uint8_t n_x : 1; + uint8_t p_y : 1; + uint8_t n_y : 1; + uint8_t p_z : 1; + uint8_t n_z : 1; + uint8_t p_v : 1; + uint8_t n_v : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_fsm_outs14_t; + +#define LSM6DSO_FSM_OUTS15 0x5AU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t n_v : 1; + uint8_t p_v : 1; + uint8_t n_z : 1; + uint8_t p_z : 1; + uint8_t n_y : 1; + uint8_t p_y : 1; + uint8_t n_x : 1; + uint8_t p_x : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t p_x : 1; + uint8_t n_x : 1; + uint8_t p_y : 1; + uint8_t n_y : 1; + uint8_t p_z : 1; + uint8_t n_z : 1; + uint8_t p_v : 1; + uint8_t n_v : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_fsm_outs15_t; + +#define LSM6DSO_FSM_OUTS16 0x5BU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t n_v : 1; + uint8_t p_v : 1; + uint8_t n_z : 1; + uint8_t p_z : 1; + uint8_t n_y : 1; + uint8_t p_y : 1; + uint8_t n_x : 1; + uint8_t p_x : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t p_x : 1; + uint8_t n_x : 1; + uint8_t p_y : 1; + uint8_t n_y : 1; + uint8_t p_z : 1; + uint8_t n_z : 1; + uint8_t p_v : 1; + uint8_t n_v : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_fsm_outs16_t; + +#define LSM6DSO_EMB_FUNC_ODR_CFG_B 0x5FU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_01 : 3; + uint8_t fsm_odr : 2; + uint8_t not_used_02 : 3; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_02 : 3; + uint8_t fsm_odr : 2; + uint8_t not_used_01 : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_emb_func_odr_cfg_b_t; + +#define LSM6DSO_STEP_COUNTER_L 0x62U +#define LSM6DSO_STEP_COUNTER_H 0x63U +#define LSM6DSO_EMB_FUNC_SRC 0x64U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_01 : 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_used_02 : 1; + uint8_t pedo_rst_step : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t pedo_rst_step : 1; + uint8_t not_used_02 : 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_used_01 : 2; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_emb_func_src_t; + +#define LSM6DSO_EMB_FUNC_INIT_A 0x66U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used_01 : 3; + uint8_t step_det_init : 1; + uint8_t tilt_init : 1; + uint8_t sig_mot_init : 1; + uint8_t not_used_02 : 2; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_02 : 2; + uint8_t sig_mot_init : 1; + uint8_t tilt_init : 1; + uint8_t step_det_init : 1; + uint8_t not_used_01 : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_emb_func_init_a_t; + +#define LSM6DSO_EMB_FUNC_INIT_B 0x67U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm_init : 1; + uint8_t not_used_01 : 2; + uint8_t fifo_compr_init : 1; + uint8_t not_used_02 : 4; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_02 : 4; + uint8_t fifo_compr_init : 1; + uint8_t not_used_01 : 2; + uint8_t fsm_init : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_emb_func_init_b_t; + +#define LSM6DSO_MAG_SENSITIVITY_L 0xBAU +#define LSM6DSO_MAG_SENSITIVITY_H 0xBBU +#define LSM6DSO_MAG_OFFX_L 0xC0U +#define LSM6DSO_MAG_OFFX_H 0xC1U +#define LSM6DSO_MAG_OFFY_L 0xC2U +#define LSM6DSO_MAG_OFFY_H 0xC3U +#define LSM6DSO_MAG_OFFZ_L 0xC4U +#define LSM6DSO_MAG_OFFZ_H 0xC5U +#define LSM6DSO_MAG_SI_XX_L 0xC6U +#define LSM6DSO_MAG_SI_XX_H 0xC7U +#define LSM6DSO_MAG_SI_XY_L 0xC8U +#define LSM6DSO_MAG_SI_XY_H 0xC9U +#define LSM6DSO_MAG_SI_XZ_L 0xCAU +#define LSM6DSO_MAG_SI_XZ_H 0xCBU +#define LSM6DSO_MAG_SI_YY_L 0xCCU +#define LSM6DSO_MAG_SI_YY_H 0xCDU +#define LSM6DSO_MAG_SI_YZ_L 0xCEU +#define LSM6DSO_MAG_SI_YZ_H 0xCFU +#define LSM6DSO_MAG_SI_ZZ_L 0xD0U +#define LSM6DSO_MAG_SI_ZZ_H 0xD1U +#define LSM6DSO_MAG_CFG_A 0xD4U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t mag_z_axis : 3; + uint8_t not_used_01 : 1; + uint8_t mag_y_axis : 3; + uint8_t not_used_02 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_02 : 1; + uint8_t mag_y_axis : 3; + uint8_t not_used_01 : 1; + uint8_t mag_z_axis : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_mag_cfg_a_t; + +#define LSM6DSO_MAG_CFG_B 0xD5U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t mag_x_axis : 3; + uint8_t not_used_01 : 5; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 5; + uint8_t mag_x_axis : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_mag_cfg_b_t; + +#define LSM6DSO_FSM_LC_TIMEOUT_L 0x17AU +#define LSM6DSO_FSM_LC_TIMEOUT_H 0x17BU +#define LSM6DSO_FSM_PROGRAMS 0x17CU +#define LSM6DSO_FSM_START_ADD_L 0x17EU +#define LSM6DSO_FSM_START_ADD_H 0x17FU +#define LSM6DSO_PEDO_CMD_REG 0x183U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ad_det_en : 1; + uint8_t not_used_01 : 1; + uint8_t fp_rejection_en : 1; + uint8_t carry_count_en : 1; + uint8_t not_used_02 : 4; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_02 : 4; + uint8_t carry_count_en : 1; + uint8_t fp_rejection_en : 1; + uint8_t not_used_01 : 1; + uint8_t ad_det_en : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_pedo_cmd_reg_t; + +#define LSM6DSO_PEDO_DEB_STEPS_CONF 0x184U +#define LSM6DSO_PEDO_SC_DELTAT_L 0x1D0U +#define LSM6DSO_PEDO_SC_DELTAT_H 0x1D1U +#define LSM6DSO_SENSOR_HUB_1 0x02U +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 */ +} lsm6dso_sensor_hub_1_t; + +#define LSM6DSO_SENSOR_HUB_2 0x03U +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 */ +} lsm6dso_sensor_hub_2_t; + +#define LSM6DSO_SENSOR_HUB_3 0x04U +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 */ +} lsm6dso_sensor_hub_3_t; + +#define LSM6DSO_SENSOR_HUB_4 0x05U +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 */ +} lsm6dso_sensor_hub_4_t; + +#define LSM6DSO_SENSOR_HUB_5 0x06U +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 */ +} lsm6dso_sensor_hub_5_t; + +#define LSM6DSO_SENSOR_HUB_6 0x07U +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 */ +} lsm6dso_sensor_hub_6_t; + +#define LSM6DSO_SENSOR_HUB_7 0x08U +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 */ +} lsm6dso_sensor_hub_7_t; + +#define LSM6DSO_SENSOR_HUB_8 0x09U +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 */ +} lsm6dso_sensor_hub_8_t; + +#define LSM6DSO_SENSOR_HUB_9 0x0AU +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 */ +} lsm6dso_sensor_hub_9_t; + +#define LSM6DSO_SENSOR_HUB_10 0x0BU +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 */ +} lsm6dso_sensor_hub_10_t; + +#define LSM6DSO_SENSOR_HUB_11 0x0CU +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 */ +} lsm6dso_sensor_hub_11_t; + +#define LSM6DSO_SENSOR_HUB_12 0x0DU +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 */ +} lsm6dso_sensor_hub_12_t; + +#define LSM6DSO_SENSOR_HUB_13 0x0EU +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 */ +} lsm6dso_sensor_hub_13_t; + +#define LSM6DSO_SENSOR_HUB_14 0x0FU +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 */ +} lsm6dso_sensor_hub_14_t; + +#define LSM6DSO_SENSOR_HUB_15 0x10U +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 */ +} lsm6dso_sensor_hub_15_t; + +#define LSM6DSO_SENSOR_HUB_16 0x11U +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 */ +} lsm6dso_sensor_hub_16_t; + +#define LSM6DSO_SENSOR_HUB_17 0x12U +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 */ +} lsm6dso_sensor_hub_17_t; + +#define LSM6DSO_SENSOR_HUB_18 0x13U +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 */ +} lsm6dso_sensor_hub_18_t; + +#define LSM6DSO_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 shub_pu_en : 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 shub_pu_en : 1; + uint8_t master_on : 1; + uint8_t aux_sens_on : 2; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_master_config_t; + +#define LSM6DSO_SLV0_ADD 0x15U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t rw_0 : 1; + uint8_t slave0 : 7; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t slave0 : 7; + uint8_t rw_0 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_slv0_add_t; + +#define LSM6DSO_SLV0_SUBADD 0x16U +typedef struct +{ + uint8_t slave0_reg : 8; +} lsm6dso_slv0_subadd_t; + +#define LSM6DSO_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_used_01 : 2; + uint8_t shub_odr : 2; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t shub_odr : 2; + uint8_t not_used_01 : 2; + uint8_t batch_ext_sens_0_en : 1; + uint8_t slave0_numop : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_slv0_config_t; + +#define LSM6DSO_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 */ +} lsm6dso_slv1_add_t; + +#define LSM6DSO_SLV1_SUBADD 0x19U +typedef struct +{ + uint8_t slave1_reg : 8; +} lsm6dso_slv1_subadd_t; + +#define LSM6DSO_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_used_01 : 4; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 4; + uint8_t batch_ext_sens_1_en : 1; + uint8_t slave1_numop : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_slv1_config_t; + +#define LSM6DSO_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 */ +} lsm6dso_slv2_add_t; + +#define LSM6DSO_SLV2_SUBADD 0x1CU +typedef struct +{ + uint8_t slave2_reg : 8; +} lsm6dso_slv2_subadd_t; + +#define LSM6DSO_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_used_01 : 4; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 4; + uint8_t batch_ext_sens_2_en : 1; + uint8_t slave2_numop : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_slv2_config_t; + +#define LSM6DSO_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 */ +} lsm6dso_slv3_add_t; + +#define LSM6DSO_SLV3_SUBADD 0x1FU +typedef struct +{ + uint8_t slave3_reg : 8; +} lsm6dso_slv3_subadd_t; + +#define LSM6DSO_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_used_01 : 4; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 4; + uint8_t batch_ext_sens_3_en : 1; + uint8_t slave3_numop : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_slv3_config_t; + +#define LSM6DSO_DATAWRITE_SLV0 0x21U +typedef struct +{ + uint8_t slave0_dataw : 8; +} lsm6dso_datawrite_src_mode_sub_slv0_t; + +#define LSM6DSO_STATUS_MASTER 0x22U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sens_hub_endop : 1; + uint8_t not_used_01 : 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_used_01 : 2; + uint8_t sens_hub_endop : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dso_status_master_t; + +#define LSM6DSO_START_FSM_ADD 0x0400U + +/** + * @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 +{ + lsm6dso_func_cfg_access_t func_cfg_access; + lsm6dso_pin_ctrl_t pin_ctrl; + lsm6dso_fifo_ctrl1_t fifo_ctrl1; + lsm6dso_fifo_ctrl2_t fifo_ctrl2; + lsm6dso_fifo_ctrl3_t fifo_ctrl3; + lsm6dso_fifo_ctrl4_t fifo_ctrl4; + lsm6dso_counter_bdr_reg1_t counter_bdr_reg1; + lsm6dso_counter_bdr_reg2_t counter_bdr_reg2; + lsm6dso_int1_ctrl_t int1_ctrl; + lsm6dso_int2_ctrl_t int2_ctrl; + lsm6dso_ctrl1_xl_t ctrl1_xl; + lsm6dso_ctrl2_g_t ctrl2_g; + lsm6dso_ctrl3_c_t ctrl3_c; + lsm6dso_ctrl4_c_t ctrl4_c; + lsm6dso_ctrl5_c_t ctrl5_c; + lsm6dso_ctrl6_c_t ctrl6_c; + lsm6dso_ctrl7_g_t ctrl7_g; + lsm6dso_ctrl8_xl_t ctrl8_xl; + lsm6dso_ctrl9_xl_t ctrl9_xl; + lsm6dso_ctrl10_c_t ctrl10_c; + lsm6dso_all_int_src_t all_int_src; + lsm6dso_wake_up_src_t wake_up_src; + lsm6dso_tap_src_t tap_src; + lsm6dso_d6d_src_t d6d_src; + lsm6dso_status_reg_t status_reg; + lsm6dso_status_spiaux_t status_spiaux; + lsm6dso_fifo_status1_t fifo_status1; + lsm6dso_fifo_status2_t fifo_status2; + lsm6dso_tap_cfg0_t tap_cfg0; + lsm6dso_tap_cfg1_t tap_cfg1; + lsm6dso_tap_cfg2_t tap_cfg2; + lsm6dso_tap_ths_6d_t tap_ths_6d; + lsm6dso_int_dur2_t int_dur2; + lsm6dso_wake_up_ths_t wake_up_ths; + lsm6dso_wake_up_dur_t wake_up_dur; + lsm6dso_free_fall_t free_fall; + lsm6dso_md1_cfg_t md1_cfg; + lsm6dso_md2_cfg_t md2_cfg; + lsm6dso_i3c_bus_avb_t i3c_bus_avb; + lsm6dso_internal_freq_fine_t internal_freq_fine; + lsm6dso_int_ois_t int_ois; + lsm6dso_ctrl1_ois_t ctrl1_ois; + lsm6dso_ctrl2_ois_t ctrl2_ois; + lsm6dso_ctrl3_ois_t ctrl3_ois; + lsm6dso_fifo_data_out_tag_t fifo_data_out_tag; + lsm6dso_page_sel_t page_sel; + lsm6dso_emb_func_en_a_t emb_func_en_a; + lsm6dso_emb_func_en_b_t emb_func_en_b; + lsm6dso_page_address_t page_address; + lsm6dso_page_value_t page_value; + lsm6dso_emb_func_int1_t emb_func_int1; + lsm6dso_fsm_int1_a_t fsm_int1_a; + lsm6dso_fsm_int1_b_t fsm_int1_b; + lsm6dso_emb_func_int2_t emb_func_int2; + lsm6dso_fsm_int2_a_t fsm_int2_a; + lsm6dso_fsm_int2_b_t fsm_int2_b; + lsm6dso_emb_func_status_t emb_func_status; + lsm6dso_fsm_status_a_t fsm_status_a; + lsm6dso_fsm_status_b_t fsm_status_b; + lsm6dso_page_rw_t page_rw; + lsm6dso_emb_func_fifo_cfg_t emb_func_fifo_cfg; + lsm6dso_fsm_enable_a_t fsm_enable_a; + lsm6dso_fsm_enable_b_t fsm_enable_b; + lsm6dso_fsm_long_counter_clear_t fsm_long_counter_clear; + lsm6dso_fsm_outs1_t fsm_outs1; + lsm6dso_fsm_outs2_t fsm_outs2; + lsm6dso_fsm_outs3_t fsm_outs3; + lsm6dso_fsm_outs4_t fsm_outs4; + lsm6dso_fsm_outs5_t fsm_outs5; + lsm6dso_fsm_outs6_t fsm_outs6; + lsm6dso_fsm_outs7_t fsm_outs7; + lsm6dso_fsm_outs8_t fsm_outs8; + lsm6dso_fsm_outs9_t fsm_outs9; + lsm6dso_fsm_outs10_t fsm_outs10; + lsm6dso_fsm_outs11_t fsm_outs11; + lsm6dso_fsm_outs12_t fsm_outs12; + lsm6dso_fsm_outs13_t fsm_outs13; + lsm6dso_fsm_outs14_t fsm_outs14; + lsm6dso_fsm_outs15_t fsm_outs15; + lsm6dso_fsm_outs16_t fsm_outs16; + lsm6dso_emb_func_odr_cfg_b_t emb_func_odr_cfg_b; + lsm6dso_emb_func_src_t emb_func_src; + lsm6dso_emb_func_init_a_t emb_func_init_a; + lsm6dso_emb_func_init_b_t emb_func_init_b; + lsm6dso_mag_cfg_a_t mag_cfg_a; + lsm6dso_mag_cfg_b_t mag_cfg_b; + lsm6dso_pedo_cmd_reg_t pedo_cmd_reg; + lsm6dso_sensor_hub_1_t sensor_hub_1; + lsm6dso_sensor_hub_2_t sensor_hub_2; + lsm6dso_sensor_hub_3_t sensor_hub_3; + lsm6dso_sensor_hub_4_t sensor_hub_4; + lsm6dso_sensor_hub_5_t sensor_hub_5; + lsm6dso_sensor_hub_6_t sensor_hub_6; + lsm6dso_sensor_hub_7_t sensor_hub_7; + lsm6dso_sensor_hub_8_t sensor_hub_8; + lsm6dso_sensor_hub_9_t sensor_hub_9; + lsm6dso_sensor_hub_10_t sensor_hub_10; + lsm6dso_sensor_hub_11_t sensor_hub_11; + lsm6dso_sensor_hub_12_t sensor_hub_12; + lsm6dso_sensor_hub_13_t sensor_hub_13; + lsm6dso_sensor_hub_14_t sensor_hub_14; + lsm6dso_sensor_hub_15_t sensor_hub_15; + lsm6dso_sensor_hub_16_t sensor_hub_16; + lsm6dso_sensor_hub_17_t sensor_hub_17; + lsm6dso_sensor_hub_18_t sensor_hub_18; + lsm6dso_master_config_t master_config; + lsm6dso_slv0_add_t slv0_add; + lsm6dso_slv0_subadd_t slv0_subadd; + lsm6dso_slv0_config_t slv0_config; + lsm6dso_slv1_add_t slv1_add; + lsm6dso_slv1_subadd_t slv1_subadd; + lsm6dso_slv1_config_t slv1_config; + lsm6dso_slv2_add_t slv2_add; + lsm6dso_slv2_subadd_t slv2_subadd; + lsm6dso_slv2_config_t slv2_config; + lsm6dso_slv3_add_t slv3_add; + lsm6dso_slv3_subadd_t slv3_subadd; + lsm6dso_slv3_config_t slv3_config; + lsm6dso_datawrite_src_mode_sub_slv0_t datawrite_src_mode_sub_slv0; + lsm6dso_status_master_t status_master; + bitwise_t bitwise; + uint8_t byte; +} lsm6dso_reg_t; + +/** + * @} + * + */ + +float_t lsm6dso_from_fs2_to_mg(int16_t lsb); +float_t lsm6dso_from_fs4_to_mg(int16_t lsb); +float_t lsm6dso_from_fs8_to_mg(int16_t lsb); +float_t lsm6dso_from_fs16_to_mg(int16_t lsb); + +float_t lsm6dso_from_fs125_to_mdps(int16_t lsb); +float_t lsm6dso_from_fs500_to_mdps(int16_t lsb); +float_t lsm6dso_from_fs250_to_mdps(int16_t lsb); +float_t lsm6dso_from_fs1000_to_mdps(int16_t lsb); +float_t lsm6dso_from_fs2000_to_mdps(int16_t lsb); + +float_t lsm6dso_from_lsb_to_celsius(int16_t lsb); + +float_t lsm6dso_from_lsb_to_nsec(int16_t lsb); + +typedef enum +{ + LSM6DSO_2g = 0, + LSM6DSO_16g = 1, /* if XL_FS_MODE = '1' -> LSM6DSO_2g */ + LSM6DSO_4g = 2, + LSM6DSO_8g = 3, +} lsm6dso_fs_xl_t; +int32_t lsm6dso_xl_full_scale_set(stmdev_ctx_t *ctx, + lsm6dso_fs_xl_t val); +int32_t lsm6dso_xl_full_scale_get(stmdev_ctx_t *ctx, + lsm6dso_fs_xl_t *val); + +typedef enum +{ + LSM6DSO_XL_ODR_OFF = 0, + LSM6DSO_XL_ODR_12Hz5 = 1, + LSM6DSO_XL_ODR_26Hz = 2, + LSM6DSO_XL_ODR_52Hz = 3, + LSM6DSO_XL_ODR_104Hz = 4, + LSM6DSO_XL_ODR_208Hz = 5, + LSM6DSO_XL_ODR_417Hz = 6, + LSM6DSO_XL_ODR_833Hz = 7, + LSM6DSO_XL_ODR_1667Hz = 8, + LSM6DSO_XL_ODR_3333Hz = 9, + LSM6DSO_XL_ODR_6667Hz = 10, + LSM6DSO_XL_ODR_1Hz6 = 11, /* (low power only) */ +} lsm6dso_odr_xl_t; +int32_t lsm6dso_xl_data_rate_set(stmdev_ctx_t *ctx, + lsm6dso_odr_xl_t val); +int32_t lsm6dso_xl_data_rate_get(stmdev_ctx_t *ctx, + lsm6dso_odr_xl_t *val); + +typedef enum +{ + LSM6DSO_250dps = 0, + LSM6DSO_125dps = 1, + LSM6DSO_500dps = 2, + LSM6DSO_1000dps = 4, + LSM6DSO_2000dps = 6, +} lsm6dso_fs_g_t; +int32_t lsm6dso_gy_full_scale_set(stmdev_ctx_t *ctx, + lsm6dso_fs_g_t val); +int32_t lsm6dso_gy_full_scale_get(stmdev_ctx_t *ctx, + lsm6dso_fs_g_t *val); + +typedef enum +{ + LSM6DSO_GY_ODR_OFF = 0, + LSM6DSO_GY_ODR_12Hz5 = 1, + LSM6DSO_GY_ODR_26Hz = 2, + LSM6DSO_GY_ODR_52Hz = 3, + LSM6DSO_GY_ODR_104Hz = 4, + LSM6DSO_GY_ODR_208Hz = 5, + LSM6DSO_GY_ODR_417Hz = 6, + LSM6DSO_GY_ODR_833Hz = 7, + LSM6DSO_GY_ODR_1667Hz = 8, + LSM6DSO_GY_ODR_3333Hz = 9, + LSM6DSO_GY_ODR_6667Hz = 10, +} lsm6dso_odr_g_t; +int32_t lsm6dso_gy_data_rate_set(stmdev_ctx_t *ctx, + lsm6dso_odr_g_t val); +int32_t lsm6dso_gy_data_rate_get(stmdev_ctx_t *ctx, + lsm6dso_odr_g_t *val); + +int32_t lsm6dso_block_data_update_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dso_block_data_update_get(stmdev_ctx_t *ctx, + uint8_t *val); + +typedef enum +{ + LSM6DSO_LSb_1mg = 0, + LSM6DSO_LSb_16mg = 1, +} lsm6dso_usr_off_w_t; +int32_t lsm6dso_xl_offset_weight_set(stmdev_ctx_t *ctx, + lsm6dso_usr_off_w_t val); +int32_t lsm6dso_xl_offset_weight_get(stmdev_ctx_t *ctx, + lsm6dso_usr_off_w_t *val); + +typedef enum +{ + LSM6DSO_HIGH_PERFORMANCE_MD = 0, + LSM6DSO_LOW_NORMAL_POWER_MD = 1, + LSM6DSO_ULTRA_LOW_POWER_MD = 2, +} lsm6dso_xl_hm_mode_t; +int32_t lsm6dso_xl_power_mode_set(stmdev_ctx_t *ctx, + lsm6dso_xl_hm_mode_t val); +int32_t lsm6dso_xl_power_mode_get(stmdev_ctx_t *ctx, + lsm6dso_xl_hm_mode_t *val); + +typedef enum +{ + LSM6DSO_GY_HIGH_PERFORMANCE = 0, + LSM6DSO_GY_NORMAL = 1, +} lsm6dso_g_hm_mode_t; +int32_t lsm6dso_gy_power_mode_set(stmdev_ctx_t *ctx, + lsm6dso_g_hm_mode_t val); +int32_t lsm6dso_gy_power_mode_get(stmdev_ctx_t *ctx, + lsm6dso_g_hm_mode_t *val); + +int32_t lsm6dso_status_reg_get(stmdev_ctx_t *ctx, + lsm6dso_status_reg_t *val); + +int32_t lsm6dso_xl_flag_data_ready_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6dso_gy_flag_data_ready_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6dso_temp_flag_data_ready_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6dso_xl_usr_offset_x_set(stmdev_ctx_t *ctx, uint8_t *buff); +int32_t lsm6dso_xl_usr_offset_x_get(stmdev_ctx_t *ctx, uint8_t *buff); + +int32_t lsm6dso_xl_usr_offset_y_set(stmdev_ctx_t *ctx, uint8_t *buff); +int32_t lsm6dso_xl_usr_offset_y_get(stmdev_ctx_t *ctx, uint8_t *buff); + +int32_t lsm6dso_xl_usr_offset_z_set(stmdev_ctx_t *ctx, uint8_t *buff); +int32_t lsm6dso_xl_usr_offset_z_get(stmdev_ctx_t *ctx, uint8_t *buff); + +int32_t lsm6dso_xl_usr_offset_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dso_xl_usr_offset_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dso_timestamp_rst(stmdev_ctx_t *ctx); + +int32_t lsm6dso_timestamp_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dso_timestamp_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dso_timestamp_raw_get(stmdev_ctx_t *ctx, uint32_t *val); + +typedef enum +{ + LSM6DSO_NO_ROUND = 0, + LSM6DSO_ROUND_XL = 1, + LSM6DSO_ROUND_GY = 2, + LSM6DSO_ROUND_GY_XL = 3, +} lsm6dso_rounding_t; +int32_t lsm6dso_rounding_mode_set(stmdev_ctx_t *ctx, + lsm6dso_rounding_t val); +int32_t lsm6dso_rounding_mode_get(stmdev_ctx_t *ctx, + lsm6dso_rounding_t *val); + +int32_t lsm6dso_temperature_raw_get(stmdev_ctx_t *ctx, int16_t *val); + +int32_t lsm6dso_angular_rate_raw_get(stmdev_ctx_t *ctx, + int16_t *val); + +int32_t lsm6dso_acceleration_raw_get(stmdev_ctx_t *ctx, + int16_t *val); + +int32_t lsm6dso_fifo_out_raw_get(stmdev_ctx_t *ctx, uint8_t *buff); + +int32_t lsm6dso_number_of_steps_get(stmdev_ctx_t *ctx, uint16_t *val); + +int32_t lsm6dso_steps_reset(stmdev_ctx_t *ctx); + +int32_t lsm6dso_odr_cal_reg_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dso_odr_cal_reg_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + LSM6DSO_USER_BANK = 0, + LSM6DSO_SENSOR_HUB_BANK = 1, + LSM6DSO_EMBEDDED_FUNC_BANK = 2, +} lsm6dso_reg_access_t; +int32_t lsm6dso_mem_bank_set(stmdev_ctx_t *ctx, + lsm6dso_reg_access_t val); +int32_t lsm6dso_mem_bank_get(stmdev_ctx_t *ctx, + lsm6dso_reg_access_t *val); + +int32_t lsm6dso_ln_pg_write_byte(stmdev_ctx_t *ctx, uint16_t address, + uint8_t *val); +int32_t lsm6dso_ln_pg_read_byte(stmdev_ctx_t *ctx, uint16_t address, + uint8_t *val); +int32_t lsm6dso_ln_pg_write(stmdev_ctx_t *ctx, uint16_t address, + uint8_t *buf, uint8_t len); +int32_t lsm6dso_ln_pg_read(stmdev_ctx_t *ctx, uint16_t address, uint8_t *buf, + uint8_t len); + +typedef enum +{ + LSM6DSO_DRDY_LATCHED = 0, + LSM6DSO_DRDY_PULSED = 1, +} lsm6dso_dataready_pulsed_t; +int32_t lsm6dso_data_ready_mode_set(stmdev_ctx_t *ctx, + lsm6dso_dataready_pulsed_t val); +int32_t lsm6dso_data_ready_mode_get(stmdev_ctx_t *ctx, + lsm6dso_dataready_pulsed_t *val); + +int32_t lsm6dso_device_id_get(stmdev_ctx_t *ctx, uint8_t *buff); + +int32_t lsm6dso_reset_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dso_reset_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dso_auto_increment_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dso_auto_increment_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dso_boot_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dso_boot_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + LSM6DSO_XL_ST_DISABLE = 0, + LSM6DSO_XL_ST_POSITIVE = 1, + LSM6DSO_XL_ST_NEGATIVE = 2, +} lsm6dso_st_xl_t; +int32_t lsm6dso_xl_self_test_set(stmdev_ctx_t *ctx, + lsm6dso_st_xl_t val); +int32_t lsm6dso_xl_self_test_get(stmdev_ctx_t *ctx, + lsm6dso_st_xl_t *val); + +typedef enum +{ + LSM6DSO_GY_ST_DISABLE = 0, + LSM6DSO_GY_ST_POSITIVE = 1, + LSM6DSO_GY_ST_NEGATIVE = 3, +} lsm6dso_st_g_t; +int32_t lsm6dso_gy_self_test_set(stmdev_ctx_t *ctx, + lsm6dso_st_g_t val); +int32_t lsm6dso_gy_self_test_get(stmdev_ctx_t *ctx, + lsm6dso_st_g_t *val); + +int32_t lsm6dso_xl_filter_lp2_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dso_xl_filter_lp2_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dso_gy_filter_lp1_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dso_gy_filter_lp1_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dso_filter_settling_mask_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6dso_filter_settling_mask_get(stmdev_ctx_t *ctx, + uint8_t *val); + +typedef enum +{ + LSM6DSO_ULTRA_LIGHT = 0, + LSM6DSO_VERY_LIGHT = 1, + LSM6DSO_LIGHT = 2, + LSM6DSO_MEDIUM = 3, + LSM6DSO_STRONG = 4, /* not available for data rate > 1k670Hz */ + LSM6DSO_VERY_STRONG = 5, /* not available for data rate > 1k670Hz */ + LSM6DSO_AGGRESSIVE = 6, /* not available for data rate > 1k670Hz */ + LSM6DSO_XTREME = 7, /* not available for data rate > 1k670Hz */ +} lsm6dso_ftype_t; +int32_t lsm6dso_gy_lp1_bandwidth_set(stmdev_ctx_t *ctx, + lsm6dso_ftype_t val); +int32_t lsm6dso_gy_lp1_bandwidth_get(stmdev_ctx_t *ctx, + lsm6dso_ftype_t *val); + +int32_t lsm6dso_xl_lp2_on_6d_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dso_xl_lp2_on_6d_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + LSM6DSO_HP_PATH_DISABLE_ON_OUT = 0x00, + LSM6DSO_SLOPE_ODR_DIV_4 = 0x10, + LSM6DSO_HP_ODR_DIV_10 = 0x11, + LSM6DSO_HP_ODR_DIV_20 = 0x12, + LSM6DSO_HP_ODR_DIV_45 = 0x13, + LSM6DSO_HP_ODR_DIV_100 = 0x14, + LSM6DSO_HP_ODR_DIV_200 = 0x15, + LSM6DSO_HP_ODR_DIV_400 = 0x16, + LSM6DSO_HP_ODR_DIV_800 = 0x17, + LSM6DSO_HP_REF_MD_ODR_DIV_10 = 0x31, + LSM6DSO_HP_REF_MD_ODR_DIV_20 = 0x32, + LSM6DSO_HP_REF_MD_ODR_DIV_45 = 0x33, + LSM6DSO_HP_REF_MD_ODR_DIV_100 = 0x34, + LSM6DSO_HP_REF_MD_ODR_DIV_200 = 0x35, + LSM6DSO_HP_REF_MD_ODR_DIV_400 = 0x36, + LSM6DSO_HP_REF_MD_ODR_DIV_800 = 0x37, + LSM6DSO_LP_ODR_DIV_10 = 0x01, + LSM6DSO_LP_ODR_DIV_20 = 0x02, + LSM6DSO_LP_ODR_DIV_45 = 0x03, + LSM6DSO_LP_ODR_DIV_100 = 0x04, + LSM6DSO_LP_ODR_DIV_200 = 0x05, + LSM6DSO_LP_ODR_DIV_400 = 0x06, + LSM6DSO_LP_ODR_DIV_800 = 0x07, +} lsm6dso_hp_slope_xl_en_t; +int32_t lsm6dso_xl_hp_path_on_out_set(stmdev_ctx_t *ctx, + lsm6dso_hp_slope_xl_en_t val); +int32_t lsm6dso_xl_hp_path_on_out_get(stmdev_ctx_t *ctx, + lsm6dso_hp_slope_xl_en_t *val); + +int32_t lsm6dso_xl_fast_settling_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dso_xl_fast_settling_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + LSM6DSO_USE_SLOPE = 0, + LSM6DSO_USE_HPF = 1, +} lsm6dso_slope_fds_t; +int32_t lsm6dso_xl_hp_path_internal_set(stmdev_ctx_t *ctx, + lsm6dso_slope_fds_t val); +int32_t lsm6dso_xl_hp_path_internal_get(stmdev_ctx_t *ctx, + lsm6dso_slope_fds_t *val); + +typedef enum +{ + LSM6DSO_HP_FILTER_NONE = 0x00, + LSM6DSO_HP_FILTER_16mHz = 0x80, + LSM6DSO_HP_FILTER_65mHz = 0x81, + LSM6DSO_HP_FILTER_260mHz = 0x82, + LSM6DSO_HP_FILTER_1Hz04 = 0x83, +} lsm6dso_hpm_g_t; +int32_t lsm6dso_gy_hp_path_internal_set(stmdev_ctx_t *ctx, + lsm6dso_hpm_g_t val); +int32_t lsm6dso_gy_hp_path_internal_get(stmdev_ctx_t *ctx, + lsm6dso_hpm_g_t *val); + +typedef enum +{ + LSM6DSO_AUX_PULL_UP_DISC = 0, + LSM6DSO_AUX_PULL_UP_CONNECT = 1, +} lsm6dso_ois_pu_dis_t; +int32_t lsm6dso_aux_sdo_ocs_mode_set(stmdev_ctx_t *ctx, + lsm6dso_ois_pu_dis_t val); +int32_t lsm6dso_aux_sdo_ocs_mode_get(stmdev_ctx_t *ctx, + lsm6dso_ois_pu_dis_t *val); + +typedef enum +{ + LSM6DSO_AUX_ON = 1, + LSM6DSO_AUX_ON_BY_AUX_INTERFACE = 0, +} lsm6dso_ois_on_t; +int32_t lsm6dso_aux_pw_on_ctrl_set(stmdev_ctx_t *ctx, + lsm6dso_ois_on_t val); +int32_t lsm6dso_aux_pw_on_ctrl_get(stmdev_ctx_t *ctx, + lsm6dso_ois_on_t *val); + +typedef enum +{ + LSM6DSO_USE_SAME_XL_FS = 0, + LSM6DSO_USE_DIFFERENT_XL_FS = 1, +} lsm6dso_xl_fs_mode_t; +int32_t lsm6dso_aux_xl_fs_mode_set(stmdev_ctx_t *ctx, + lsm6dso_xl_fs_mode_t val); +int32_t lsm6dso_aux_xl_fs_mode_get(stmdev_ctx_t *ctx, + lsm6dso_xl_fs_mode_t *val); + +int32_t lsm6dso_aux_status_reg_get(stmdev_ctx_t *ctx, + lsm6dso_status_spiaux_t *val); + +int32_t lsm6dso_aux_xl_flag_data_ready_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6dso_aux_gy_flag_data_ready_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6dso_aux_gy_flag_settling_get(stmdev_ctx_t *ctx, + uint8_t *val); + +typedef enum +{ + LSM6DSO_AUX_XL_DISABLE = 0, + LSM6DSO_AUX_XL_POS = 1, + LSM6DSO_AUX_XL_NEG = 2, +} lsm6dso_st_xl_ois_t; +int32_t lsm6dso_aux_xl_self_test_set(stmdev_ctx_t *ctx, + lsm6dso_st_xl_ois_t val); +int32_t lsm6dso_aux_xl_self_test_get(stmdev_ctx_t *ctx, + lsm6dso_st_xl_ois_t *val); + +typedef enum +{ + LSM6DSO_AUX_DEN_ACTIVE_LOW = 0, + LSM6DSO_AUX_DEN_ACTIVE_HIGH = 1, +} lsm6dso_den_lh_ois_t; +int32_t lsm6dso_aux_den_polarity_set(stmdev_ctx_t *ctx, + lsm6dso_den_lh_ois_t val); +int32_t lsm6dso_aux_den_polarity_get(stmdev_ctx_t *ctx, + lsm6dso_den_lh_ois_t *val); + +typedef enum +{ + LSM6DSO_AUX_DEN_DISABLE = 0, + LSM6DSO_AUX_DEN_LEVEL_LATCH = 3, + LSM6DSO_AUX_DEN_LEVEL_TRIG = 2, +} lsm6dso_lvl2_ois_t; +int32_t lsm6dso_aux_den_mode_set(stmdev_ctx_t *ctx, + lsm6dso_lvl2_ois_t val); +int32_t lsm6dso_aux_den_mode_get(stmdev_ctx_t *ctx, + lsm6dso_lvl2_ois_t *val); + +int32_t lsm6dso_aux_drdy_on_int2_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dso_aux_drdy_on_int2_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + LSM6DSO_AUX_DISABLE = 0, + LSM6DSO_MODE_3_GY = 1, + LSM6DSO_MODE_4_GY_XL = 3, +} lsm6dso_ois_en_spi2_t; +int32_t lsm6dso_aux_mode_set(stmdev_ctx_t *ctx, + lsm6dso_ois_en_spi2_t val); +int32_t lsm6dso_aux_mode_get(stmdev_ctx_t *ctx, + lsm6dso_ois_en_spi2_t *val); + +typedef enum +{ + LSM6DSO_250dps_AUX = 0, + LSM6DSO_125dps_AUX = 1, + LSM6DSO_500dps_AUX = 2, + LSM6DSO_1000dps_AUX = 4, + LSM6DSO_2000dps_AUX = 6, +} lsm6dso_fs_g_ois_t; +int32_t lsm6dso_aux_gy_full_scale_set(stmdev_ctx_t *ctx, + lsm6dso_fs_g_ois_t val); +int32_t lsm6dso_aux_gy_full_scale_get(stmdev_ctx_t *ctx, + lsm6dso_fs_g_ois_t *val); + +typedef enum +{ + LSM6DSO_AUX_SPI_4_WIRE = 0, + LSM6DSO_AUX_SPI_3_WIRE = 1, +} lsm6dso_sim_ois_t; +int32_t lsm6dso_aux_spi_mode_set(stmdev_ctx_t *ctx, + lsm6dso_sim_ois_t val); +int32_t lsm6dso_aux_spi_mode_get(stmdev_ctx_t *ctx, + lsm6dso_sim_ois_t *val); + +typedef enum +{ + LSM6DSO_351Hz39 = 0, + LSM6DSO_236Hz63 = 1, + LSM6DSO_172Hz70 = 2, + LSM6DSO_937Hz91 = 3, +} lsm6dso_ftype_ois_t; +int32_t lsm6dso_aux_gy_lp1_bandwidth_set(stmdev_ctx_t *ctx, + lsm6dso_ftype_ois_t val); +int32_t lsm6dso_aux_gy_lp1_bandwidth_get(stmdev_ctx_t *ctx, + lsm6dso_ftype_ois_t *val); + +typedef enum +{ + LSM6DSO_AUX_HP_DISABLE = 0x00, + LSM6DSO_AUX_HP_Hz016 = 0x10, + LSM6DSO_AUX_HP_Hz065 = 0x11, + LSM6DSO_AUX_HP_Hz260 = 0x12, + LSM6DSO_AUX_HP_1Hz040 = 0x13, +} lsm6dso_hpm_ois_t; +int32_t lsm6dso_aux_gy_hp_bandwidth_set(stmdev_ctx_t *ctx, + lsm6dso_hpm_ois_t val); +int32_t lsm6dso_aux_gy_hp_bandwidth_get(stmdev_ctx_t *ctx, + lsm6dso_hpm_ois_t *val); + +typedef enum +{ + LSM6DSO_ENABLE_CLAMP = 0, + LSM6DSO_DISABLE_CLAMP = 1, +} lsm6dso_st_ois_clampdis_t; +int32_t lsm6dso_aux_gy_clamp_set(stmdev_ctx_t *ctx, + lsm6dso_st_ois_clampdis_t val); +int32_t lsm6dso_aux_gy_clamp_get(stmdev_ctx_t *ctx, + lsm6dso_st_ois_clampdis_t *val); + +typedef enum +{ + LSM6DSO_AUX_GY_DISABLE = 0, + LSM6DSO_AUX_GY_POS = 1, + LSM6DSO_AUX_GY_NEG = 3, +} lsm6dso_st_ois_t; +int32_t lsm6dso_aux_gy_self_test_set(stmdev_ctx_t *ctx, + lsm6dso_st_ois_t val); +int32_t lsm6dso_aux_gy_self_test_get(stmdev_ctx_t *ctx, + lsm6dso_st_ois_t *val); + +typedef enum +{ + LSM6DSO_289Hz = 0, + LSM6DSO_258Hz = 1, + LSM6DSO_120Hz = 2, + LSM6DSO_65Hz2 = 3, + LSM6DSO_33Hz2 = 4, + LSM6DSO_16Hz6 = 5, + LSM6DSO_8Hz30 = 6, + LSM6DSO_4Hz15 = 7, +} lsm6dso_filter_xl_conf_ois_t; +int32_t lsm6dso_aux_xl_bandwidth_set(stmdev_ctx_t *ctx, + lsm6dso_filter_xl_conf_ois_t val); +int32_t lsm6dso_aux_xl_bandwidth_get(stmdev_ctx_t *ctx, + lsm6dso_filter_xl_conf_ois_t *val); + +typedef enum +{ + LSM6DSO_AUX_2g = 0, + LSM6DSO_AUX_16g = 1, + LSM6DSO_AUX_4g = 2, + LSM6DSO_AUX_8g = 3, +} lsm6dso_fs_xl_ois_t; +int32_t lsm6dso_aux_xl_full_scale_set(stmdev_ctx_t *ctx, + lsm6dso_fs_xl_ois_t val); +int32_t lsm6dso_aux_xl_full_scale_get(stmdev_ctx_t *ctx, + lsm6dso_fs_xl_ois_t *val); + +typedef enum +{ + LSM6DSO_PULL_UP_DISC = 0, + LSM6DSO_PULL_UP_CONNECT = 1, +} lsm6dso_sdo_pu_en_t; +int32_t lsm6dso_sdo_sa0_mode_set(stmdev_ctx_t *ctx, + lsm6dso_sdo_pu_en_t val); +int32_t lsm6dso_sdo_sa0_mode_get(stmdev_ctx_t *ctx, + lsm6dso_sdo_pu_en_t *val); + +typedef enum +{ + LSM6DSO_SPI_4_WIRE = 0, + LSM6DSO_SPI_3_WIRE = 1, +} lsm6dso_sim_t; +int32_t lsm6dso_spi_mode_set(stmdev_ctx_t *ctx, lsm6dso_sim_t val); +int32_t lsm6dso_spi_mode_get(stmdev_ctx_t *ctx, lsm6dso_sim_t *val); + +typedef enum +{ + LSM6DSO_I2C_ENABLE = 0, + LSM6DSO_I2C_DISABLE = 1, +} lsm6dso_i2c_disable_t; +int32_t lsm6dso_i2c_interface_set(stmdev_ctx_t *ctx, + lsm6dso_i2c_disable_t val); +int32_t lsm6dso_i2c_interface_get(stmdev_ctx_t *ctx, + lsm6dso_i2c_disable_t *val); + +typedef enum +{ + LSM6DSO_I3C_DISABLE = 0x80, + LSM6DSO_I3C_ENABLE_T_50us = 0x00, + LSM6DSO_I3C_ENABLE_T_2us = 0x01, + LSM6DSO_I3C_ENABLE_T_1ms = 0x02, + LSM6DSO_I3C_ENABLE_T_25ms = 0x03, +} lsm6dso_i3c_disable_t; +int32_t lsm6dso_i3c_disable_set(stmdev_ctx_t *ctx, + lsm6dso_i3c_disable_t val); +int32_t lsm6dso_i3c_disable_get(stmdev_ctx_t *ctx, + lsm6dso_i3c_disable_t *val); + +typedef enum +{ + LSM6DSO_PULL_DOWN_DISC = 0, + LSM6DSO_PULL_DOWN_CONNECT = 1, +} lsm6dso_int1_pd_en_t; +int32_t lsm6dso_int1_mode_set(stmdev_ctx_t *ctx, + lsm6dso_int1_pd_en_t val); +int32_t lsm6dso_int1_mode_get(stmdev_ctx_t *ctx, + lsm6dso_int1_pd_en_t *val); + +typedef enum +{ + LSM6DSO_PUSH_PULL = 0, + LSM6DSO_OPEN_DRAIN = 1, +} lsm6dso_pp_od_t; +int32_t lsm6dso_pin_mode_set(stmdev_ctx_t *ctx, lsm6dso_pp_od_t val); +int32_t lsm6dso_pin_mode_get(stmdev_ctx_t *ctx, lsm6dso_pp_od_t *val); + +typedef enum +{ + LSM6DSO_ACTIVE_HIGH = 0, + LSM6DSO_ACTIVE_LOW = 1, +} lsm6dso_h_lactive_t; +int32_t lsm6dso_pin_polarity_set(stmdev_ctx_t *ctx, + lsm6dso_h_lactive_t val); +int32_t lsm6dso_pin_polarity_get(stmdev_ctx_t *ctx, + lsm6dso_h_lactive_t *val); + +int32_t lsm6dso_all_on_int1_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dso_all_on_int1_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + LSM6DSO_ALL_INT_PULSED = 0, + LSM6DSO_BASE_LATCHED_EMB_PULSED = 1, + LSM6DSO_BASE_PULSED_EMB_LATCHED = 2, + LSM6DSO_ALL_INT_LATCHED = 3, +} lsm6dso_lir_t; +int32_t lsm6dso_int_notification_set(stmdev_ctx_t *ctx, + lsm6dso_lir_t val); +int32_t lsm6dso_int_notification_get(stmdev_ctx_t *ctx, + lsm6dso_lir_t *val); + +typedef enum +{ + LSM6DSO_LSb_FS_DIV_64 = 0, + LSM6DSO_LSb_FS_DIV_256 = 1, +} lsm6dso_wake_ths_w_t; +int32_t lsm6dso_wkup_ths_weight_set(stmdev_ctx_t *ctx, + lsm6dso_wake_ths_w_t val); +int32_t lsm6dso_wkup_ths_weight_get(stmdev_ctx_t *ctx, + lsm6dso_wake_ths_w_t *val); + +int32_t lsm6dso_wkup_threshold_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dso_wkup_threshold_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dso_xl_usr_offset_on_wkup_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6dso_xl_usr_offset_on_wkup_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6dso_wkup_dur_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dso_wkup_dur_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dso_gy_sleep_mode_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dso_gy_sleep_mode_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + LSM6DSO_DRIVE_SLEEP_CHG_EVENT = 0, + LSM6DSO_DRIVE_SLEEP_STATUS = 1, +} lsm6dso_sleep_status_on_int_t; +int32_t lsm6dso_act_pin_notification_set(stmdev_ctx_t *ctx, + lsm6dso_sleep_status_on_int_t val); +int32_t lsm6dso_act_pin_notification_get(stmdev_ctx_t *ctx, + lsm6dso_sleep_status_on_int_t *val); + +typedef enum +{ + LSM6DSO_XL_AND_GY_NOT_AFFECTED = 0, + LSM6DSO_XL_12Hz5_GY_NOT_AFFECTED = 1, + LSM6DSO_XL_12Hz5_GY_SLEEP = 2, + LSM6DSO_XL_12Hz5_GY_PD = 3, +} lsm6dso_inact_en_t; +int32_t lsm6dso_act_mode_set(stmdev_ctx_t *ctx, + lsm6dso_inact_en_t val); +int32_t lsm6dso_act_mode_get(stmdev_ctx_t *ctx, + lsm6dso_inact_en_t *val); + +int32_t lsm6dso_act_sleep_dur_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dso_act_sleep_dur_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dso_tap_detection_on_z_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6dso_tap_detection_on_z_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6dso_tap_detection_on_y_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6dso_tap_detection_on_y_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6dso_tap_detection_on_x_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6dso_tap_detection_on_x_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6dso_tap_threshold_x_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dso_tap_threshold_x_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + LSM6DSO_XYZ = 0, + LSM6DSO_YXZ = 1, + LSM6DSO_XZY = 2, + LSM6DSO_ZYX = 3, + LSM6DSO_YZX = 5, + LSM6DSO_ZXY = 6, +} lsm6dso_tap_priority_t; +int32_t lsm6dso_tap_axis_priority_set(stmdev_ctx_t *ctx, + lsm6dso_tap_priority_t val); +int32_t lsm6dso_tap_axis_priority_get(stmdev_ctx_t *ctx, + lsm6dso_tap_priority_t *val); + +int32_t lsm6dso_tap_threshold_y_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dso_tap_threshold_y_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dso_tap_threshold_z_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dso_tap_threshold_z_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dso_tap_shock_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dso_tap_shock_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dso_tap_quiet_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dso_tap_quiet_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dso_tap_dur_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dso_tap_dur_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + LSM6DSO_ONLY_SINGLE = 0, + LSM6DSO_BOTH_SINGLE_DOUBLE = 1, +} lsm6dso_single_double_tap_t; +int32_t lsm6dso_tap_mode_set(stmdev_ctx_t *ctx, + lsm6dso_single_double_tap_t val); +int32_t lsm6dso_tap_mode_get(stmdev_ctx_t *ctx, + lsm6dso_single_double_tap_t *val); + +typedef enum +{ + LSM6DSO_DEG_80 = 0, + LSM6DSO_DEG_70 = 1, + LSM6DSO_DEG_60 = 2, + LSM6DSO_DEG_50 = 3, +} lsm6dso_sixd_ths_t; +int32_t lsm6dso_6d_threshold_set(stmdev_ctx_t *ctx, + lsm6dso_sixd_ths_t val); +int32_t lsm6dso_6d_threshold_get(stmdev_ctx_t *ctx, + lsm6dso_sixd_ths_t *val); + +int32_t lsm6dso_4d_mode_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dso_4d_mode_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + LSM6DSO_FF_TSH_156mg = 0, + LSM6DSO_FF_TSH_219mg = 1, + LSM6DSO_FF_TSH_250mg = 2, + LSM6DSO_FF_TSH_312mg = 3, + LSM6DSO_FF_TSH_344mg = 4, + LSM6DSO_FF_TSH_406mg = 5, + LSM6DSO_FF_TSH_469mg = 6, + LSM6DSO_FF_TSH_500mg = 7, +} lsm6dso_ff_ths_t; +int32_t lsm6dso_ff_threshold_set(stmdev_ctx_t *ctx, + lsm6dso_ff_ths_t val); +int32_t lsm6dso_ff_threshold_get(stmdev_ctx_t *ctx, + lsm6dso_ff_ths_t *val); + +int32_t lsm6dso_ff_dur_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dso_ff_dur_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dso_fifo_watermark_set(stmdev_ctx_t *ctx, uint16_t val); +int32_t lsm6dso_fifo_watermark_get(stmdev_ctx_t *ctx, uint16_t *val); + +int32_t lsm6dso_compression_algo_init_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6dso_compression_algo_init_get(stmdev_ctx_t *ctx, + uint8_t *val); + +typedef enum +{ + LSM6DSO_CMP_DISABLE = 0x00, + LSM6DSO_CMP_ALWAYS = 0x04, + LSM6DSO_CMP_8_TO_1 = 0x05, + LSM6DSO_CMP_16_TO_1 = 0x06, + LSM6DSO_CMP_32_TO_1 = 0x07, +} lsm6dso_uncoptr_rate_t; +int32_t lsm6dso_compression_algo_set(stmdev_ctx_t *ctx, + lsm6dso_uncoptr_rate_t val); +int32_t lsm6dso_compression_algo_get(stmdev_ctx_t *ctx, + lsm6dso_uncoptr_rate_t *val); + +int32_t lsm6dso_fifo_virtual_sens_odr_chg_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6dso_fifo_virtual_sens_odr_chg_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6dso_compression_algo_real_time_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6dso_compression_algo_real_time_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6dso_fifo_stop_on_wtm_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dso_fifo_stop_on_wtm_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + LSM6DSO_XL_NOT_BATCHED = 0, + LSM6DSO_XL_BATCHED_AT_12Hz5 = 1, + LSM6DSO_XL_BATCHED_AT_26Hz = 2, + LSM6DSO_XL_BATCHED_AT_52Hz = 3, + LSM6DSO_XL_BATCHED_AT_104Hz = 4, + LSM6DSO_XL_BATCHED_AT_208Hz = 5, + LSM6DSO_XL_BATCHED_AT_417Hz = 6, + LSM6DSO_XL_BATCHED_AT_833Hz = 7, + LSM6DSO_XL_BATCHED_AT_1667Hz = 8, + LSM6DSO_XL_BATCHED_AT_3333Hz = 9, + LSM6DSO_XL_BATCHED_AT_6667Hz = 10, + LSM6DSO_XL_BATCHED_AT_6Hz5 = 11, +} lsm6dso_bdr_xl_t; +int32_t lsm6dso_fifo_xl_batch_set(stmdev_ctx_t *ctx, + lsm6dso_bdr_xl_t val); +int32_t lsm6dso_fifo_xl_batch_get(stmdev_ctx_t *ctx, + lsm6dso_bdr_xl_t *val); + +typedef enum +{ + LSM6DSO_GY_NOT_BATCHED = 0, + LSM6DSO_GY_BATCHED_AT_12Hz5 = 1, + LSM6DSO_GY_BATCHED_AT_26Hz = 2, + LSM6DSO_GY_BATCHED_AT_52Hz = 3, + LSM6DSO_GY_BATCHED_AT_104Hz = 4, + LSM6DSO_GY_BATCHED_AT_208Hz = 5, + LSM6DSO_GY_BATCHED_AT_417Hz = 6, + LSM6DSO_GY_BATCHED_AT_833Hz = 7, + LSM6DSO_GY_BATCHED_AT_1667Hz = 8, + LSM6DSO_GY_BATCHED_AT_3333Hz = 9, + LSM6DSO_GY_BATCHED_AT_6667Hz = 10, + LSM6DSO_GY_BATCHED_AT_6Hz5 = 11, +} lsm6dso_bdr_gy_t; +int32_t lsm6dso_fifo_gy_batch_set(stmdev_ctx_t *ctx, + lsm6dso_bdr_gy_t val); +int32_t lsm6dso_fifo_gy_batch_get(stmdev_ctx_t *ctx, + lsm6dso_bdr_gy_t *val); + +typedef enum +{ + LSM6DSO_BYPASS_MODE = 0, + LSM6DSO_FIFO_MODE = 1, + LSM6DSO_STREAM_TO_FIFO_MODE = 3, + LSM6DSO_BYPASS_TO_STREAM_MODE = 4, + LSM6DSO_STREAM_MODE = 6, + LSM6DSO_BYPASS_TO_FIFO_MODE = 7, +} lsm6dso_fifo_mode_t; +int32_t lsm6dso_fifo_mode_set(stmdev_ctx_t *ctx, + lsm6dso_fifo_mode_t val); +int32_t lsm6dso_fifo_mode_get(stmdev_ctx_t *ctx, + lsm6dso_fifo_mode_t *val); + +typedef enum +{ + LSM6DSO_TEMP_NOT_BATCHED = 0, + LSM6DSO_TEMP_BATCHED_AT_1Hz6 = 1, + LSM6DSO_TEMP_BATCHED_AT_12Hz5 = 2, + LSM6DSO_TEMP_BATCHED_AT_52Hz = 3, +} lsm6dso_odr_t_batch_t; +int32_t lsm6dso_fifo_temp_batch_set(stmdev_ctx_t *ctx, + lsm6dso_odr_t_batch_t val); +int32_t lsm6dso_fifo_temp_batch_get(stmdev_ctx_t *ctx, + lsm6dso_odr_t_batch_t *val); + +typedef enum +{ + LSM6DSO_NO_DECIMATION = 0, + LSM6DSO_DEC_1 = 1, + LSM6DSO_DEC_8 = 2, + LSM6DSO_DEC_32 = 3, +} lsm6dso_odr_ts_batch_t; +int32_t lsm6dso_fifo_timestamp_decimation_set(stmdev_ctx_t *ctx, + lsm6dso_odr_ts_batch_t val); +int32_t lsm6dso_fifo_timestamp_decimation_get(stmdev_ctx_t *ctx, + lsm6dso_odr_ts_batch_t *val); + +typedef enum +{ + LSM6DSO_XL_BATCH_EVENT = 0, + LSM6DSO_GYRO_BATCH_EVENT = 1, +} lsm6dso_trig_counter_bdr_t; + +typedef enum +{ + LSM6DSO_GYRO_NC_TAG = 1, + LSM6DSO_XL_NC_TAG, + LSM6DSO_TEMPERATURE_TAG, + LSM6DSO_TIMESTAMP_TAG, + LSM6DSO_CFG_CHANGE_TAG, + LSM6DSO_XL_NC_T_2_TAG, + LSM6DSO_XL_NC_T_1_TAG, + LSM6DSO_XL_2XC_TAG, + LSM6DSO_XL_3XC_TAG, + LSM6DSO_GYRO_NC_T_2_TAG, + LSM6DSO_GYRO_NC_T_1_TAG, + LSM6DSO_GYRO_2XC_TAG, + LSM6DSO_GYRO_3XC_TAG, + LSM6DSO_SENSORHUB_SLAVE0_TAG, + LSM6DSO_SENSORHUB_SLAVE1_TAG, + LSM6DSO_SENSORHUB_SLAVE2_TAG, + LSM6DSO_SENSORHUB_SLAVE3_TAG, + LSM6DSO_STEP_COUNTER_TAG, + LSM6DSO_GAME_ROTATION_TAG, + LSM6DSO_GEOMAG_ROTATION_TAG, + LSM6DSO_ROTATION_TAG, + LSM6DSO_SENSORHUB_NACK_TAG = 0x19, +} lsm6dso_fifo_tag_t; +int32_t lsm6dso_fifo_cnt_event_batch_set(stmdev_ctx_t *ctx, + lsm6dso_trig_counter_bdr_t val); +int32_t lsm6dso_fifo_cnt_event_batch_get(stmdev_ctx_t *ctx, + lsm6dso_trig_counter_bdr_t *val); + +int32_t lsm6dso_rst_batch_counter_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dso_rst_batch_counter_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6dso_batch_counter_threshold_set(stmdev_ctx_t *ctx, + uint16_t val); +int32_t lsm6dso_batch_counter_threshold_get(stmdev_ctx_t *ctx, + uint16_t *val); + +int32_t lsm6dso_fifo_data_level_get(stmdev_ctx_t *ctx, uint16_t *val); + +int32_t lsm6dso_fifo_status_get(stmdev_ctx_t *ctx, + lsm6dso_fifo_status2_t *val); + +int32_t lsm6dso_fifo_full_flag_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dso_fifo_ovr_flag_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dso_fifo_wtm_flag_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dso_fifo_sensor_tag_get(stmdev_ctx_t *ctx, + lsm6dso_fifo_tag_t *val); + +int32_t lsm6dso_fifo_pedo_batch_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dso_fifo_pedo_batch_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dso_sh_batch_slave_set(stmdev_ctx_t *ctx, uint8_t idx, uint8_t val); +int32_t lsm6dso_sh_batch_slave_get(stmdev_ctx_t *ctx, uint8_t idx, uint8_t *val); + +typedef enum +{ + LSM6DSO_DEN_DISABLE = 0, + LSM6DSO_LEVEL_FIFO = 6, + LSM6DSO_LEVEL_LETCHED = 3, + LSM6DSO_LEVEL_TRIGGER = 2, + LSM6DSO_EDGE_TRIGGER = 4, +} lsm6dso_den_mode_t; +int32_t lsm6dso_den_mode_set(stmdev_ctx_t *ctx, + lsm6dso_den_mode_t val); +int32_t lsm6dso_den_mode_get(stmdev_ctx_t *ctx, + lsm6dso_den_mode_t *val); + +typedef enum +{ + LSM6DSO_DEN_ACT_LOW = 0, + LSM6DSO_DEN_ACT_HIGH = 1, +} lsm6dso_den_lh_t; +int32_t lsm6dso_den_polarity_set(stmdev_ctx_t *ctx, + lsm6dso_den_lh_t val); +int32_t lsm6dso_den_polarity_get(stmdev_ctx_t *ctx, + lsm6dso_den_lh_t *val); + +typedef enum +{ + LSM6DSO_STAMP_IN_GY_DATA = 0, + LSM6DSO_STAMP_IN_XL_DATA = 1, + LSM6DSO_STAMP_IN_GY_XL_DATA = 2, +} lsm6dso_den_xl_g_t; +int32_t lsm6dso_den_enable_set(stmdev_ctx_t *ctx, + lsm6dso_den_xl_g_t val); +int32_t lsm6dso_den_enable_get(stmdev_ctx_t *ctx, + lsm6dso_den_xl_g_t *val); + +int32_t lsm6dso_den_mark_axis_x_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dso_den_mark_axis_x_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dso_den_mark_axis_y_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dso_den_mark_axis_y_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dso_den_mark_axis_z_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dso_den_mark_axis_z_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + LSM6DSO_PEDO_BASE_MODE = 0x00, + LSM6DSO_FALSE_STEP_REJ = 0x10, + LSM6DSO_FALSE_STEP_REJ_ADV_MODE = 0x30, +} lsm6dso_pedo_md_t; +int32_t lsm6dso_pedo_sens_set(stmdev_ctx_t *ctx, + lsm6dso_pedo_md_t val); +int32_t lsm6dso_pedo_sens_get(stmdev_ctx_t *ctx, + lsm6dso_pedo_md_t *val); + +int32_t lsm6dso_pedo_step_detect_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dso_pedo_debounce_steps_set(stmdev_ctx_t *ctx, + uint8_t *buff); +int32_t lsm6dso_pedo_debounce_steps_get(stmdev_ctx_t *ctx, + uint8_t *buff); + +int32_t lsm6dso_pedo_steps_period_set(stmdev_ctx_t *ctx, + uint16_t val); +int32_t lsm6dso_pedo_steps_period_get(stmdev_ctx_t *ctx, + uint16_t *val); + +typedef enum +{ + LSM6DSO_EVERY_STEP = 0, + LSM6DSO_COUNT_OVERFLOW = 1, +} lsm6dso_carry_count_en_t; +int32_t lsm6dso_pedo_int_mode_set(stmdev_ctx_t *ctx, + lsm6dso_carry_count_en_t val); +int32_t lsm6dso_pedo_int_mode_get(stmdev_ctx_t *ctx, + lsm6dso_carry_count_en_t *val); + +int32_t lsm6dso_motion_flag_data_ready_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6dso_tilt_flag_data_ready_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6dso_mag_sensitivity_set(stmdev_ctx_t *ctx, uint16_t val); +int32_t lsm6dso_mag_sensitivity_get(stmdev_ctx_t *ctx, uint16_t *val); + +int32_t lsm6dso_mag_offset_set(stmdev_ctx_t *ctx, int16_t *val); +int32_t lsm6dso_mag_offset_get(stmdev_ctx_t *ctx, int16_t *val); + +int32_t lsm6dso_mag_soft_iron_set(stmdev_ctx_t *ctx, int16_t *val); +int32_t lsm6dso_mag_soft_iron_get(stmdev_ctx_t *ctx, int16_t *val); + +typedef enum +{ + LSM6DSO_Z_EQ_Y = 0, + LSM6DSO_Z_EQ_MIN_Y = 1, + LSM6DSO_Z_EQ_X = 2, + LSM6DSO_Z_EQ_MIN_X = 3, + LSM6DSO_Z_EQ_MIN_Z = 4, + LSM6DSO_Z_EQ_Z = 5, +} lsm6dso_mag_z_axis_t; +int32_t lsm6dso_mag_z_orient_set(stmdev_ctx_t *ctx, + lsm6dso_mag_z_axis_t val); +int32_t lsm6dso_mag_z_orient_get(stmdev_ctx_t *ctx, + lsm6dso_mag_z_axis_t *val); + +typedef enum +{ + LSM6DSO_Y_EQ_Y = 0, + LSM6DSO_Y_EQ_MIN_Y = 1, + LSM6DSO_Y_EQ_X = 2, + LSM6DSO_Y_EQ_MIN_X = 3, + LSM6DSO_Y_EQ_MIN_Z = 4, + LSM6DSO_Y_EQ_Z = 5, +} lsm6dso_mag_y_axis_t; +int32_t lsm6dso_mag_y_orient_set(stmdev_ctx_t *ctx, + lsm6dso_mag_y_axis_t val); +int32_t lsm6dso_mag_y_orient_get(stmdev_ctx_t *ctx, + lsm6dso_mag_y_axis_t *val); + +typedef enum +{ + LSM6DSO_X_EQ_Y = 0, + LSM6DSO_X_EQ_MIN_Y = 1, + LSM6DSO_X_EQ_X = 2, + LSM6DSO_X_EQ_MIN_X = 3, + LSM6DSO_X_EQ_MIN_Z = 4, + LSM6DSO_X_EQ_Z = 5, +} lsm6dso_mag_x_axis_t; +int32_t lsm6dso_mag_x_orient_set(stmdev_ctx_t *ctx, + lsm6dso_mag_x_axis_t val); +int32_t lsm6dso_mag_x_orient_get(stmdev_ctx_t *ctx, + lsm6dso_mag_x_axis_t *val); + +int32_t lsm6dso_long_cnt_flag_data_ready_get(stmdev_ctx_t *ctx, + uint8_t *val); + +typedef struct +{ + lsm6dso_fsm_enable_a_t fsm_enable_a; + lsm6dso_fsm_enable_b_t fsm_enable_b; +} lsm6dso_emb_fsm_enable_t; +int32_t lsm6dso_fsm_enable_set(stmdev_ctx_t *ctx, + lsm6dso_emb_fsm_enable_t *val); +int32_t lsm6dso_fsm_enable_get(stmdev_ctx_t *ctx, + lsm6dso_emb_fsm_enable_t *val); + +int32_t lsm6dso_long_cnt_set(stmdev_ctx_t *ctx, uint16_t val); +int32_t lsm6dso_long_cnt_get(stmdev_ctx_t *ctx, uint16_t *val); + +typedef enum +{ + LSM6DSO_LC_NORMAL = 0, + LSM6DSO_LC_CLEAR = 1, + LSM6DSO_LC_CLEAR_DONE = 2, +} lsm6dso_fsm_lc_clr_t; +int32_t lsm6dso_long_clr_set(stmdev_ctx_t *ctx, + lsm6dso_fsm_lc_clr_t val); +int32_t lsm6dso_long_clr_get(stmdev_ctx_t *ctx, + lsm6dso_fsm_lc_clr_t *val); + +typedef struct +{ + lsm6dso_fsm_outs1_t fsm_outs1; + lsm6dso_fsm_outs2_t fsm_outs2; + lsm6dso_fsm_outs3_t fsm_outs3; + lsm6dso_fsm_outs4_t fsm_outs4; + lsm6dso_fsm_outs5_t fsm_outs5; + lsm6dso_fsm_outs6_t fsm_outs6; + lsm6dso_fsm_outs7_t fsm_outs7; + lsm6dso_fsm_outs8_t fsm_outs8; + lsm6dso_fsm_outs9_t fsm_outs9; + lsm6dso_fsm_outs10_t fsm_outs10; + lsm6dso_fsm_outs11_t fsm_outs11; + lsm6dso_fsm_outs12_t fsm_outs12; + lsm6dso_fsm_outs13_t fsm_outs13; + lsm6dso_fsm_outs14_t fsm_outs14; + lsm6dso_fsm_outs15_t fsm_outs15; + lsm6dso_fsm_outs16_t fsm_outs16; +} lsm6dso_fsm_out_t; +int32_t lsm6dso_fsm_out_get(stmdev_ctx_t *ctx, + lsm6dso_fsm_out_t *val); + +typedef enum +{ + LSM6DSO_ODR_FSM_12Hz5 = 0, + LSM6DSO_ODR_FSM_26Hz = 1, + LSM6DSO_ODR_FSM_52Hz = 2, + LSM6DSO_ODR_FSM_104Hz = 3, +} lsm6dso_fsm_odr_t; +int32_t lsm6dso_fsm_data_rate_set(stmdev_ctx_t *ctx, + lsm6dso_fsm_odr_t val); +int32_t lsm6dso_fsm_data_rate_get(stmdev_ctx_t *ctx, + lsm6dso_fsm_odr_t *val); + +int32_t lsm6dso_fsm_init_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dso_fsm_init_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dso_long_cnt_int_value_set(stmdev_ctx_t *ctx, + uint16_t val); +int32_t lsm6dso_long_cnt_int_value_get(stmdev_ctx_t *ctx, + uint16_t *val); + +int32_t lsm6dso_fsm_number_of_programs_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6dso_fsm_number_of_programs_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6dso_fsm_start_address_set(stmdev_ctx_t *ctx, + uint16_t val); +int32_t lsm6dso_fsm_start_address_get(stmdev_ctx_t *ctx, + uint16_t *val); + +int32_t lsm6dso_sh_read_data_raw_get(stmdev_ctx_t *ctx, uint8_t *val, + uint8_t len); + +typedef enum +{ + LSM6DSO_SLV_0 = 0, + LSM6DSO_SLV_0_1 = 1, + LSM6DSO_SLV_0_1_2 = 2, + LSM6DSO_SLV_0_1_2_3 = 3, +} lsm6dso_aux_sens_on_t; +int32_t lsm6dso_sh_slave_connected_set(stmdev_ctx_t *ctx, + lsm6dso_aux_sens_on_t val); +int32_t lsm6dso_sh_slave_connected_get(stmdev_ctx_t *ctx, + lsm6dso_aux_sens_on_t *val); + +int32_t lsm6dso_sh_master_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dso_sh_master_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + LSM6DSO_EXT_PULL_UP = 0, + LSM6DSO_INTERNAL_PULL_UP = 1, +} lsm6dso_shub_pu_en_t; +int32_t lsm6dso_sh_pin_mode_set(stmdev_ctx_t *ctx, + lsm6dso_shub_pu_en_t val); +int32_t lsm6dso_sh_pin_mode_get(stmdev_ctx_t *ctx, + lsm6dso_shub_pu_en_t *val); + +int32_t lsm6dso_sh_pass_through_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dso_sh_pass_through_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + LSM6DSO_EXT_ON_INT2_PIN = 1, + LSM6DSO_XL_GY_DRDY = 0, +} lsm6dso_start_config_t; +int32_t lsm6dso_sh_syncro_mode_set(stmdev_ctx_t *ctx, + lsm6dso_start_config_t val); +int32_t lsm6dso_sh_syncro_mode_get(stmdev_ctx_t *ctx, + lsm6dso_start_config_t *val); + +typedef enum +{ + LSM6DSO_EACH_SH_CYCLE = 0, + LSM6DSO_ONLY_FIRST_CYCLE = 1, +} lsm6dso_write_once_t; +int32_t lsm6dso_sh_write_mode_set(stmdev_ctx_t *ctx, + lsm6dso_write_once_t val); +int32_t lsm6dso_sh_write_mode_get(stmdev_ctx_t *ctx, + lsm6dso_write_once_t *val); + +int32_t lsm6dso_sh_reset_set(stmdev_ctx_t *ctx); +int32_t lsm6dso_sh_reset_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + LSM6DSO_SH_ODR_104Hz = 0, + LSM6DSO_SH_ODR_52Hz = 1, + LSM6DSO_SH_ODR_26Hz = 2, + LSM6DSO_SH_ODR_13Hz = 3, +} lsm6dso_shub_odr_t; +int32_t lsm6dso_sh_data_rate_set(stmdev_ctx_t *ctx, + lsm6dso_shub_odr_t val); +int32_t lsm6dso_sh_data_rate_get(stmdev_ctx_t *ctx, + lsm6dso_shub_odr_t *val); + +typedef struct +{ + uint8_t slv0_add; + uint8_t slv0_subadd; + uint8_t slv0_data; +} lsm6dso_sh_cfg_write_t; +int32_t lsm6dso_sh_cfg_write(stmdev_ctx_t *ctx, + lsm6dso_sh_cfg_write_t *val); + +typedef struct +{ + uint8_t slv_add; + uint8_t slv_subadd; + uint8_t slv_len; +} lsm6dso_sh_cfg_read_t; +int32_t lsm6dso_sh_slv_cfg_read(stmdev_ctx_t *ctx, uint8_t idx, + lsm6dso_sh_cfg_read_t *val); + +int32_t lsm6dso_sh_status_get(stmdev_ctx_t *ctx, + lsm6dso_status_master_t *val); + + +typedef struct +{ + uint8_t ui; + uint8_t aux; +} lsm6dso_id_t; +int32_t lsm6dso_id_get(stmdev_ctx_t *ctx, stmdev_ctx_t *aux_ctx, + lsm6dso_id_t *val); + +typedef enum +{ + LSM6DSO_SEL_BY_HW = 0x00, /* bus mode select by HW (SPI 3W disable) */ + LSM6DSO_SPI_4W = 0x06, /* Only SPI: SDO / SDI separated pins */ + LSM6DSO_SPI_3W = 0x07, /* Only SPI: SDO / SDI share the same pin */ + LSM6DSO_I2C = 0x04, /* Only I2C */ + LSM6DSO_I3C_T_50us = 0x02, /* I3C: available time equal to 50 us */ + LSM6DSO_I3C_T_2us = 0x12, /* I3C: available time equal to 2 us */ + LSM6DSO_I3C_T_1ms = 0x22, /* I3C: available time equal to 1 ms */ + LSM6DSO_I3C_T_25ms = 0x32, /* I3C: available time equal to 25 ms */ +} lsm6dso_ui_bus_md_t; + +typedef enum +{ + LSM6DSO_SPI_4W_AUX = 0x00, + LSM6DSO_SPI_3W_AUX = 0x01, +} lsm6dso_aux_bus_md_t; + +typedef struct +{ + lsm6dso_ui_bus_md_t ui_bus_md; + lsm6dso_aux_bus_md_t aux_bus_md; +} lsm6dso_bus_mode_t; +int32_t lsm6dso_bus_mode_set(stmdev_ctx_t *ctx, stmdev_ctx_t *aux_ctx, + lsm6dso_bus_mode_t val); +int32_t lsm6dso_bus_mode_get(stmdev_ctx_t *ctx, stmdev_ctx_t *aux_ctx, + lsm6dso_bus_mode_t *val); + +typedef enum +{ + LSM6DSO_DRV_RDY = 0x00, /* Initialize the device for driver usage */ + LSM6DSO_BOOT = 0x01, /* Restore calib. param. ( it takes 10ms ) */ + LSM6DSO_RESET = 0x02, /* Reset configuration registers */ + LSM6DSO_FIFO_COMP = 0x04, /* FIFO compression initialization request. */ + LSM6DSO_FSM = 0x08, /* Finite State Machine initialization request */ + LSM6DSO_PEDO = 0x20, /* Pedometer algo initialization request. */ + LSM6DSO_TILT = 0x40, /* Tilt algo initialization request */ + LSM6DSO_SMOTION = 0x80, /* Significant Motion initialization request */ +} lsm6dso_init_t; +int32_t lsm6dso_init_set(stmdev_ctx_t *ctx, lsm6dso_init_t val); + +typedef struct +{ +uint8_t sw_reset : + 1; /* Restoring configuration registers */ + uint8_t boot : 1; /* Restoring calibration parameters */ + uint8_t drdy_xl : 1; /* Accelerometer data ready */ + uint8_t drdy_g : 1; /* Gyroscope data ready */ + uint8_t drdy_temp : 1; /* Temperature data ready */ + uint8_t ois_drdy_xl : 1; /* Accelerometer data ready on OIS */ + uint8_t ois_drdy_g : 1; /* Gyroscope data ready on OIS */ +uint8_t ois_gyro_settling : + 1; /* Gyroscope is in the settling phase */ +} lsm6dso_status_t; +int32_t lsm6dso_status_get(stmdev_ctx_t *ctx, stmdev_ctx_t *aux_ctx, + lsm6dso_status_t *val); + +typedef struct +{ + uint8_t sdo_sa0_pull_up : 1; /* 1 = pull-up on SDO/SA0 pin */ +uint8_t aux_sdo_ocs_pull_up : + 1; /* 1 = pull-up on OCS_Aux/SDO_Aux pins */ + uint8_t int1_int2_push_pull : 1; /* 1 = push-pull / 0 = open-drain*/ +uint8_t int1_pull_down : + 1; /* 1 = pull-down always disabled (0=auto) */ +} lsm6dso_pin_conf_t; +int32_t lsm6dso_pin_conf_set(stmdev_ctx_t *ctx, + lsm6dso_pin_conf_t val); +int32_t lsm6dso_pin_conf_get(stmdev_ctx_t *ctx, + lsm6dso_pin_conf_t *val); + +typedef struct +{ + uint8_t active_low : 1; /* 1 = active low / 0 = active high */ +uint8_t base_latched : + 1; /* base functions are: FF, WU, 6D, Tap, Act/Inac */ +uint8_t emb_latched : + 1; /* emb functions are: Pedo, Tilt, SMot, Timestamp */ +} lsm6dso_int_mode_t; +int32_t lsm6dso_interrupt_mode_set(stmdev_ctx_t *ctx, + lsm6dso_int_mode_t val); +int32_t lsm6dso_interrupt_mode_get(stmdev_ctx_t *ctx, + lsm6dso_int_mode_t *val); + +typedef struct +{ + uint8_t drdy_xl : 1; /* Accelerometer data ready */ + uint8_t drdy_g : 1; /* Gyroscope data ready */ +uint8_t drdy_temp : + 1; /* Temperature data ready (1 = int2 pin disable) */ + uint8_t boot : 1; /* Restoring calibration parameters */ + uint8_t fifo_th : 1; /* FIFO threshold reached */ + uint8_t fifo_ovr : 1; /* FIFO overrun */ + uint8_t fifo_full : 1; /* FIFO full */ + uint8_t fifo_bdr : 1; /* FIFO Batch counter threshold reached */ +uint8_t den_flag : + 1; /* external trigger level recognition (DEN) */ + uint8_t sh_endop : 1; /* sensor hub end operation */ +uint8_t timestamp : + 1; /* timestamp overflow (1 = int2 pin disable) */ + uint8_t six_d : 1; /* orientation change (6D/4D detection) */ + uint8_t double_tap : 1; /* double-tap event */ + uint8_t free_fall : 1; /* free fall event */ + uint8_t wake_up : 1; /* wake up event */ + uint8_t single_tap : 1; /* single-tap event */ +uint8_t sleep_change : + 1; /* Act/Inact (or Vice-versa) status changed */ + uint8_t step_detector : 1; /* Step detected */ + uint8_t tilt : 1; /* Relative tilt event detected */ + uint8_t sig_mot : 1; /* "significant motion" event detected */ +uint8_t fsm_lc : + 1; /* fsm long counter timeout interrupt event */ + uint8_t fsm1 : 1; /* fsm 1 interrupt event */ + uint8_t fsm2 : 1; /* fsm 2 interrupt event */ + uint8_t fsm3 : 1; /* fsm 3 interrupt event */ + uint8_t fsm4 : 1; /* fsm 4 interrupt event */ + uint8_t fsm5 : 1; /* fsm 5 interrupt event */ + uint8_t fsm6 : 1; /* fsm 6 interrupt event */ + uint8_t fsm7 : 1; /* fsm 7 interrupt event */ + uint8_t fsm8 : 1; /* fsm 8 interrupt event */ + uint8_t fsm9 : 1; /* fsm 9 interrupt event */ + uint8_t fsm10 : 1; /* fsm 10 interrupt event */ + uint8_t fsm11 : 1; /* fsm 11 interrupt event */ + uint8_t fsm12 : 1; /* fsm 12 interrupt event */ + uint8_t fsm13 : 1; /* fsm 13 interrupt event */ + uint8_t fsm14 : 1; /* fsm 14 interrupt event */ + uint8_t fsm15 : 1; /* fsm 15 interrupt event */ + uint8_t fsm16 : 1; /* fsm 16 interrupt event */ + uint8_t mlc1 : 1; /* mlc 1 interrupt event */ + uint8_t mlc2 : 1; /* mlc 2 interrupt event */ + uint8_t mlc3 : 1; /* mlc 3 interrupt event */ + uint8_t mlc4 : 1; /* mlc 4 interrupt event */ + uint8_t mlc5 : 1; /* mlc 5 interrupt event */ + uint8_t mlc6 : 1; /* mlc 6 interrupt event */ + uint8_t mlc7 : 1; /* mlc 7 interrupt event */ + uint8_t mlc8 : 1; /* mlc 8 interrupt event */ +} lsm6dso_pin_int1_route_t; + +int32_t lsm6dso_pin_int1_route_set(stmdev_ctx_t *ctx, + lsm6dso_pin_int1_route_t val); +int32_t lsm6dso_pin_int1_route_get(stmdev_ctx_t *ctx, + lsm6dso_pin_int1_route_t *val); + +typedef struct +{ + uint8_t drdy_ois : 1; /* OIS chain data ready */ + uint8_t drdy_xl : 1; /* Accelerometer data ready */ + uint8_t drdy_g : 1; /* Gyroscope data ready */ + uint8_t drdy_temp : 1; /* Temperature data ready */ + uint8_t fifo_th : 1; /* FIFO threshold reached */ + uint8_t fifo_ovr : 1; /* FIFO overrun */ + uint8_t fifo_full : 1; /* FIFO full */ + uint8_t fifo_bdr : 1; /* FIFO Batch counter threshold reached */ + uint8_t timestamp : 1; /* timestamp overflow */ + uint8_t six_d : 1; /* orientation change (6D/4D detection) */ + uint8_t double_tap : 1; /* double-tap event */ + uint8_t free_fall : 1; /* free fall event */ + uint8_t wake_up : 1; /* wake up event */ + uint8_t single_tap : 1; /* single-tap event */ +uint8_t sleep_change : + 1; /* Act/Inact (or Vice-versa) status changed */ + uint8_t step_detector : 1; /* Step detected */ + uint8_t tilt : 1; /* Relative tilt event detected */ + uint8_t sig_mot : 1; /* "significant motion" event detected */ +uint8_t fsm_lc : + 1; /* fsm long counter timeout interrupt event */ + uint8_t fsm1 : 1; /* fsm 1 interrupt event */ + uint8_t fsm2 : 1; /* fsm 2 interrupt event */ + uint8_t fsm3 : 1; /* fsm 3 interrupt event */ + uint8_t fsm4 : 1; /* fsm 4 interrupt event */ + uint8_t fsm5 : 1; /* fsm 5 interrupt event */ + uint8_t fsm6 : 1; /* fsm 6 interrupt event */ + uint8_t fsm7 : 1; /* fsm 7 interrupt event */ + uint8_t fsm8 : 1; /* fsm 8 interrupt event */ + uint8_t fsm9 : 1; /* fsm 9 interrupt event */ + uint8_t fsm10 : 1; /* fsm 10 interrupt event */ + uint8_t fsm11 : 1; /* fsm 11 interrupt event */ + uint8_t fsm12 : 1; /* fsm 12 interrupt event */ + uint8_t fsm13 : 1; /* fsm 13 interrupt event */ + uint8_t fsm14 : 1; /* fsm 14 interrupt event */ + uint8_t fsm15 : 1; /* fsm 15 interrupt event */ + uint8_t fsm16 : 1; /* fsm 16 interrupt event */ + uint8_t mlc1 : 1; /* mlc 1 interrupt event */ + uint8_t mlc2 : 1; /* mlc 2 interrupt event */ + uint8_t mlc3 : 1; /* mlc 3 interrupt event */ + uint8_t mlc4 : 1; /* mlc 4 interrupt event */ + uint8_t mlc5 : 1; /* mlc 5 interrupt event */ + uint8_t mlc6 : 1; /* mlc 6 interrupt event */ + uint8_t mlc7 : 1; /* mlc 7 interrupt event */ + uint8_t mlc8 : 1; /* mlc 8 interrupt event */ +} lsm6dso_pin_int2_route_t; + +int32_t lsm6dso_pin_int2_route_set(stmdev_ctx_t *ctx, + stmdev_ctx_t *aux_ctx, + lsm6dso_pin_int2_route_t val); +int32_t lsm6dso_pin_int2_route_get(stmdev_ctx_t *ctx, + stmdev_ctx_t *aux_ctx, + lsm6dso_pin_int2_route_t *val); + +typedef struct +{ + uint8_t drdy_xl : 1; /* Accelerometer data ready */ + uint8_t drdy_g : 1; /* Gyroscope data ready */ + uint8_t drdy_temp : 1; /* Temperature data ready */ +uint8_t den_flag : + 1; /* external trigger level recognition (DEN) */ +uint8_t timestamp : + 1; /* timestamp overflow (1 = int2 pin disable) */ + uint8_t free_fall : 1; /* free fall event */ + uint8_t wake_up : 1; /* wake up event */ + uint8_t wake_up_z : 1; /* wake up on Z axis event */ + uint8_t wake_up_y : 1; /* wake up on Y axis event */ + uint8_t wake_up_x : 1; /* wake up on X axis event */ + uint8_t single_tap : 1; /* single-tap event */ + uint8_t double_tap : 1; /* double-tap event */ + uint8_t tap_z : 1; /* single-tap on Z axis event */ + uint8_t tap_y : 1; /* single-tap on Y axis event */ + uint8_t tap_x : 1; /* single-tap on X axis event */ + uint8_t tap_sign : 1; /* sign of tap event (0-pos / 1-neg) */ +uint8_t six_d : + 1; /* orientation change (6D/4D detection) */ +uint8_t six_d_xl : + 1; /* X-axis low 6D/4D event (under threshold) */ +uint8_t six_d_xh : + 1; /* X-axis high 6D/4D event (over threshold) */ +uint8_t six_d_yl : + 1; /* Y-axis low 6D/4D event (under threshold) */ +uint8_t six_d_yh : + 1; /* Y-axis high 6D/4D event (over threshold) */ +uint8_t six_d_zl : + 1; /* Z-axis low 6D/4D event (under threshold) */ +uint8_t six_d_zh : + 1; /* Z-axis high 6D/4D event (over threshold) */ +uint8_t sleep_change : + 1; /* Act/Inact (or Vice-versa) status changed */ +uint8_t sleep_state : + 1; /* Act/Inact status flag (0-Act / 1-Inact) */ + uint8_t step_detector : 1; /* Step detected */ + uint8_t tilt : 1; /* Relative tilt event detected */ +uint8_t sig_mot : + 1; /* "significant motion" event detected */ +uint8_t fsm_lc : + 1; /* fsm long counter timeout interrupt event */ + uint8_t fsm1 : 1; /* fsm 1 interrupt event */ + uint8_t fsm2 : 1; /* fsm 2 interrupt event */ + uint8_t fsm3 : 1; /* fsm 3 interrupt event */ + uint8_t fsm4 : 1; /* fsm 4 interrupt event */ + uint8_t fsm5 : 1; /* fsm 5 interrupt event */ + uint8_t fsm6 : 1; /* fsm 6 interrupt event */ + uint8_t fsm7 : 1; /* fsm 7 interrupt event */ + uint8_t fsm8 : 1; /* fsm 8 interrupt event */ + uint8_t fsm9 : 1; /* fsm 9 interrupt event */ + uint8_t fsm10 : 1; /* fsm 10 interrupt event */ + uint8_t fsm11 : 1; /* fsm 11 interrupt event */ + uint8_t fsm12 : 1; /* fsm 12 interrupt event */ + uint8_t fsm13 : 1; /* fsm 13 interrupt event */ + uint8_t fsm14 : 1; /* fsm 14 interrupt event */ + uint8_t fsm15 : 1; /* fsm 15 interrupt event */ + uint8_t fsm16 : 1; /* fsm 16 interrupt event */ + uint8_t mlc1 : 1; /* mlc 1 interrupt event */ + uint8_t mlc2 : 1; /* mlc 2 interrupt event */ + uint8_t mlc3 : 1; /* mlc 3 interrupt event */ + uint8_t mlc4 : 1; /* mlc 4 interrupt event */ + uint8_t mlc5 : 1; /* mlc 5 interrupt event */ + uint8_t mlc6 : 1; /* mlc 6 interrupt event */ + uint8_t mlc7 : 1; /* mlc 7 interrupt event */ + uint8_t mlc8 : 1; /* mlc 8 interrupt event */ + uint8_t sh_endop : 1; /* sensor hub end operation */ +uint8_t sh_slave0_nack : + 1; /* Not acknowledge on sensor hub slave 0 */ +uint8_t sh_slave1_nack : + 1; /* Not acknowledge on sensor hub slave 1 */ +uint8_t sh_slave2_nack : + 1; /* Not acknowledge on sensor hub slave 2 */ +uint8_t sh_slave3_nack : + 1; /* Not acknowledge on sensor hub slave 3 */ +uint8_t sh_wr_once : + 1; /* "WRITE_ONCE" end on sensor hub slave 0 */ +uint16_t fifo_diff : + 10; /* Number of unread sensor data in FIFO*/ + uint8_t fifo_ovr_latched : 1; /* Latched FIFO overrun status */ +uint8_t fifo_bdr : + 1; /* FIFO Batch counter threshold reached */ + uint8_t fifo_full : 1; /* FIFO full */ + uint8_t fifo_ovr : 1; /* FIFO overrun */ + uint8_t fifo_th : 1; /* FIFO threshold reached */ +} lsm6dso_all_sources_t; +int32_t lsm6dso_all_sources_get(stmdev_ctx_t *ctx, + lsm6dso_all_sources_t *val); + +typedef struct +{ + uint8_t odr_fine_tune; +} dev_cal_t; +int32_t lsm6dso_calibration_get(stmdev_ctx_t *ctx, dev_cal_t *val); + +typedef enum +{ + LSM6DSO_XL_UI_OFF = 0x00, /* in power down */ + LSM6DSO_XL_UI_1Hz6_LP = 0x1B, /* @1Hz6 (low power) */ + LSM6DSO_XL_UI_1Hz6_ULP = 0x2B, /* @1Hz6 (ultra low/Gy, OIS imu off) */ + LSM6DSO_XL_UI_12Hz5_HP = 0x01, /* @12Hz5 (high performance) */ + LSM6DSO_XL_UI_12Hz5_LP = 0x11, /* @12Hz5 (low power) */ + LSM6DSO_XL_UI_12Hz5_ULP = 0x21, /* @12Hz5 (ultra low/Gy, OIS imu off) */ + LSM6DSO_XL_UI_26Hz_HP = 0x02, /* @26Hz (high performance) */ + LSM6DSO_XL_UI_26Hz_LP = 0x12, /* @26Hz (low power) */ + LSM6DSO_XL_UI_26Hz_ULP = 0x22, /* @26Hz (ultra low/Gy, OIS imu off) */ + LSM6DSO_XL_UI_52Hz_HP = 0x03, /* @52Hz (high performance) */ + LSM6DSO_XL_UI_52Hz_LP = 0x13, /* @52Hz (low power) */ + LSM6DSO_XL_UI_52Hz_ULP = 0x23, /* @52Hz (ultra low/Gy, OIS imu off) */ + LSM6DSO_XL_UI_104Hz_HP = 0x04, /* @104Hz (high performance) */ + LSM6DSO_XL_UI_104Hz_NM = 0x14, /* @104Hz (normal mode) */ + LSM6DSO_XL_UI_104Hz_ULP = 0x24, /* @104Hz (ultra low/Gy, OIS imu off) */ + LSM6DSO_XL_UI_208Hz_HP = 0x05, /* @208Hz (high performance) */ + LSM6DSO_XL_UI_208Hz_NM = 0x15, /* @208Hz (normal mode) */ + LSM6DSO_XL_UI_208Hz_ULP = 0x25, /* @208Hz (ultra low/Gy, OIS imu off) */ + LSM6DSO_XL_UI_416Hz_HP = 0x06, /* @416Hz (high performance) */ + LSM6DSO_XL_UI_833Hz_HP = 0x07, /* @833Hz (high performance) */ + LSM6DSO_XL_UI_1667Hz_HP = 0x08, /* @1kHz66 (high performance) */ + LSM6DSO_XL_UI_3333Hz_HP = 0x09, /* @3kHz33 (high performance) */ + LSM6DSO_XL_UI_6667Hz_HP = 0x0A, /* @6kHz66 (high performance) */ +} lsm6dso_odr_xl_ui_t; + +typedef enum +{ + LSM6DSO_XL_UI_2g = 0, + LSM6DSO_XL_UI_4g = 2, + LSM6DSO_XL_UI_8g = 3, + LSM6DSO_XL_UI_16g = 1, /* OIS full scale is also forced to be 16g */ +} lsm6dso_fs_xl_ui_t; + +typedef enum +{ + LSM6DSO_GY_UI_OFF = 0x00, /* gy in power down */ + LSM6DSO_GY_UI_12Hz5_LP = 0x11, /* gy @12Hz5 (low power) */ + LSM6DSO_GY_UI_12Hz5_HP = 0x01, /* gy @12Hz5 (high performance) */ + LSM6DSO_GY_UI_26Hz_LP = 0x12, /* gy @26Hz (low power) */ + LSM6DSO_GY_UI_26Hz_HP = 0x02, /* gy @26Hz (high performance) */ + LSM6DSO_GY_UI_52Hz_LP = 0x13, /* gy @52Hz (low power) */ + LSM6DSO_GY_UI_52Hz_HP = 0x03, /* gy @52Hz (high performance) */ + LSM6DSO_GY_UI_104Hz_NM = 0x14, /* gy @104Hz (low power) */ + LSM6DSO_GY_UI_104Hz_HP = 0x04, /* gy @104Hz (high performance) */ + LSM6DSO_GY_UI_208Hz_NM = 0x15, /* gy @208Hz (low power) */ + LSM6DSO_GY_UI_208Hz_HP = 0x05, /* gy @208Hz (high performance) */ + LSM6DSO_GY_UI_416Hz_HP = 0x06, /* gy @416Hz (high performance) */ + LSM6DSO_GY_UI_833Hz_HP = 0x07, /* gy @833Hz (high performance) */ + LSM6DSO_GY_UI_1667Hz_HP = 0x08, /* gy @1kHz66 (high performance) */ + LSM6DSO_GY_UI_3333Hz_HP = 0x09, /* gy @3kHz33 (high performance) */ + LSM6DSO_GY_UI_6667Hz_HP = 0x0A, /* gy @6kHz66 (high performance) */ +} lsm6dso_odr_g_ui_t; + +typedef enum +{ + LSM6DSO_GY_UI_250dps = 0, + LSM6DSO_GY_UI_125dps = 1, + LSM6DSO_GY_UI_500dps = 2, + LSM6DSO_GY_UI_1000dps = 4, + LSM6DSO_GY_UI_2000dps = 6, +} lsm6dso_fs_g_ui_t; + +typedef enum +{ + LSM6DSO_OIS_ONLY_AUX = 0x00, /* Auxiliary SPI full control */ + LSM6DSO_OIS_MIXED = 0x01, /* Enabling by UI / read-config by AUX */ +} lsm6dso_ctrl_md_t; + +typedef enum +{ + LSM6DSO_XL_OIS_OFF = 0x00, /* in power down */ + LSM6DSO_XL_OIS_6667Hz_HP = 0x01, /* @6kHz OIS imu active/NO ULP on UI */ +} lsm6dso_odr_xl_ois_noaux_t; + +typedef enum +{ + LSM6DSO_XL_OIS_2g = 0, + LSM6DSO_XL_OIS_4g = 2, + LSM6DSO_XL_OIS_8g = 3, + LSM6DSO_XL_OIS_16g = 1, /* UI full scale is also forced to be 16g */ +} lsm6dso_fs_xl_ois_noaux_t; + +typedef enum +{ + LSM6DSO_GY_OIS_OFF = 0x00, /* in power down */ + LSM6DSO_GY_OIS_6667Hz_HP = 0x01, /* @6kHz No Ultra Low Power*/ +} lsm6dso_odr_g_ois_noaux_t; + +typedef enum +{ + LSM6DSO_GY_OIS_250dps = 0, + LSM6DSO_GY_OIS_125dps = 1, + LSM6DSO_GY_OIS_500dps = 2, + LSM6DSO_GY_OIS_1000dps = 4, + LSM6DSO_GY_OIS_2000dps = 6, +} lsm6dso_fs_g_ois_noaux_t; + +typedef enum +{ + LSM6DSO_FSM_DISABLE = 0x00, + LSM6DSO_FSM_XL = 0x01, + LSM6DSO_FSM_GY = 0x02, + LSM6DSO_FSM_XL_GY = 0x03, +} lsm6dso_sens_fsm_t; + +typedef enum +{ + LSM6DSO_FSM_12Hz5 = 0x00, + LSM6DSO_FSM_26Hz = 0x01, + LSM6DSO_FSM_52Hz = 0x02, + LSM6DSO_FSM_104Hz = 0x03, +} lsm6dso_odr_fsm_t; + +typedef struct +{ + struct + { + struct + { + lsm6dso_odr_xl_ui_t odr; + lsm6dso_fs_xl_ui_t fs; + } xl; + struct + { + lsm6dso_odr_g_ui_t odr; + lsm6dso_fs_g_ui_t fs; + } gy; + } ui; + struct + { + lsm6dso_ctrl_md_t ctrl_md; + struct + { + lsm6dso_odr_xl_ois_noaux_t odr; + lsm6dso_fs_xl_ois_noaux_t fs; + } xl; + struct + { + lsm6dso_odr_g_ois_noaux_t odr; + lsm6dso_fs_g_ois_noaux_t fs; + } gy; + } ois; + struct + { + lsm6dso_sens_fsm_t sens; + lsm6dso_odr_fsm_t odr; + } fsm; +} lsm6dso_md_t; +int32_t lsm6dso_mode_set(stmdev_ctx_t *ctx, stmdev_ctx_t *aux_ctx, + lsm6dso_md_t *val); +int32_t lsm6dso_mode_get(stmdev_ctx_t *ctx, stmdev_ctx_t *aux_ctx, + lsm6dso_md_t *val); +typedef struct +{ + struct + { + struct + { + float_t mg[3]; + int16_t raw[3]; + } xl; + struct + { + float_t mdps[3]; + int16_t raw[3]; + } gy; + struct + { + float_t deg_c; + int16_t raw; + } heat; + } ui; + struct + { + struct + { + float_t mg[3]; + int16_t raw[3]; + } xl; + struct + { + float_t mdps[3]; + int16_t raw[3]; + } gy; + } ois; +} lsm6dso_data_t; +int32_t lsm6dso_data_get(stmdev_ctx_t *ctx, stmdev_ctx_t *aux_ctx, + lsm6dso_md_t *md, lsm6dso_data_t *data); + +typedef struct +{ + uint8_t sig_mot : 1; /* significant motion */ + uint8_t tilt : 1; /* tilt detection */ + uint8_t step : 1; /* step counter/detector */ + uint8_t step_adv : 1; /* step counter advanced mode */ + uint8_t fsm : 1; /* finite state machine */ + uint8_t fifo_compr : 1; /* FIFO compression */ +} lsm6dso_emb_sens_t; +int32_t lsm6dso_embedded_sens_set(stmdev_ctx_t *ctx, + lsm6dso_emb_sens_t *emb_sens); +int32_t lsm6dso_embedded_sens_get(stmdev_ctx_t *ctx, + lsm6dso_emb_sens_t *emb_sens); +int32_t lsm6dso_embedded_sens_off(stmdev_ctx_t *ctx); + +/** + * @} + * + */ + +#ifdef __cplusplus +} +#endif + +#endif /*LSM6DSO_DRIVER_H */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ \ No newline at end of file diff --git a/inc/types/lsm6ds_common.h b/inc/types/lsm6ds_common.h new file mode 100644 index 0000000..d2e2b6f --- /dev/null +++ b/inc/types/lsm6ds_common.h @@ -0,0 +1,42 @@ +/** + * @file lsm6ds_common_type.h + * + * @brief IMU common types. + * + * @author Bruno Machado + * + * @date 08/2023 + */ + +#ifndef LSM6DS_COMMON_H +#define LSM6DS_COMMON_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +/* Error codes */ +typedef int8_t LSM6DS_error; +#define LSM6DS_ERROR_NONE ((LSM6DS_error) 0) +#define LSM6DS_ERROR_WHO_AM_I ((LSM6DS_error) 1) +#define LSM6DS_ERROR_WRITE_REGISTER ((LSM6DS_error) 2) +#define LSM6DS_ERROR_NO_INIT ((LSM6DS_error) 3) +#define LSM6DS_ERROR_INVALID_SETTING ((LSM6DS_error) 4) + +typedef union { + int16_t i16bit[3]; + uint8_t u8bit[6]; +} axis3bit16_t; + +typedef union { + int16_t i16bit; + uint8_t u8bit[2]; +} axis1bit16_t; + +#ifdef __cplusplus +} +#endif + +#endif // LSM6DS_COMMON_H diff --git a/inc/types/lsm6ds_pinout.h b/inc/types/lsm6ds_pinout.h new file mode 100644 index 0000000..58c19c9 --- /dev/null +++ b/inc/types/lsm6ds_pinout.h @@ -0,0 +1,58 @@ +/** + * @file lsm6ds_pinout.h + * + * @brief IMU pinout settings. + * + * @author Bruno Machado + * + * @date 08/2023 + */ + +#ifndef __LSM6DS_PINOUT_H__ +#define __LSM6DS_PINOUT_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include "gpio.h" + +typedef struct lsm6ds_I2C_pinout { + uint8_t int1_pin; + GPIO_TypeDef* int1_port; + uint8_t int2_pin; + GPIO_TypeDef* int2_port; + void (*I2C_init)(); + void* sensor_bus; +} lsm6ds_I2C_pinout_t; + +typedef struct lsm6ds_SPI_pinout { + uint8_t int1_pin; + GPIO_TypeDef* int1_port; + uint8_t int2_pin; + GPIO_TypeDef* int2_port; + uint8_t cs_gpio_pin; + GPIO_TypeDef* cs_gpio_port; + void (*SPI_init)(); + void* sensor_bus; +} lsm6ds_SPI_pinout_t; + +typedef struct lsm6ds_common_pinout { + uint8_t int1_pin; + GPIO_TypeDef* int1_port; + uint8_t int2_pin; + GPIO_TypeDef* int2_port; +} lsm6ds_common_pinout; + +typedef union lsm6ds_config { + lsm6ds_I2C_pinout_t I2C_pinout; + lsm6ds_SPI_pinout_t SPI_pinout; + lsm6ds_common_pinout common_pinout; +} lsm6ds_config_t; + +#ifdef __cplusplus +} +#endif + +#endif // __LSM6DS_PINOUT_H__ diff --git a/inc/types/lsm6ds_settings_type.h b/inc/types/lsm6ds_settings_type.h new file mode 100644 index 0000000..2215829 --- /dev/null +++ b/inc/types/lsm6ds_settings_type.h @@ -0,0 +1,147 @@ +/** + * @file lsm6ds_settings_type.h + * + * @brief IMU settings types. + * + * @author Pedro Henrique + * + * @date 07/2023 + */ + +#ifndef LSM6DS_SETTINGS_TYPE_H +#define LSM6DS_SETTINGS_TYPE_H + + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +typedef enum { + LSM6DS_2g = 0, + LSM6DS_16g = 1, + LSM6DS_4g = 2, + LSM6DS_8g = 3, +} lsm6ds_xl_fs_t; + +typedef enum { + LSM6DS_250dps = 0, + LSM6DS_125dps = 1, + LSM6DS_500dps = 2, + LSM6DS_1000dps = 4, + LSM6DS_2000dps = 6, +} lsm6ds_fs_g_t; + +typedef enum { + LSM6DS_XL_ODR_OFF = 0, + LSM6DS_XL_ODR_12Hz5 = 1, + LSM6DS_XL_ODR_26Hz = 2, + LSM6DS_XL_ODR_52Hz = 3, + LSM6DS_XL_ODR_104Hz = 4, + LSM6DS_XL_ODR_208Hz = 5, + LSM6DS_XL_ODR_416Hz = 6, + LSM6DS_XL_ODR_833Hz = 7, + LSM6DS_XL_ODR_1667Hz = 8, + LSM6DS_XL_ODR_3333Hz = 9, + LSM6DS_XL_ODR_6667Hz = 10, +} lsm6ds_odr_xl_t; + +typedef enum { + LSM6DS_GY_ODR_OFF = 0, + LSM6DS_GY_ODR_12Hz5 = 1, + LSM6DS_GY_ODR_26Hz = 2, + LSM6DS_GY_ODR_52Hz = 3, + LSM6DS_GY_ODR_104Hz = 4, + LSM6DS_GY_ODR_208Hz = 5, + LSM6DS_GY_ODR_416Hz = 6, + LSM6DS_GY_ODR_833Hz = 7, + LSM6DS_GY_ODR_1667Hz = 8, + LSM6DS_GY_ODR_3333Hz = 9, + LSM6DS_GY_ODR_6667Hz = 10, +} lsm6ds_odr_g_t; + +typedef enum { + LSM6DS_BYPASS_MODE = 0, + LSM6DS_FIFO_MODE = 1, + LSM6DS_STREAM_TO_FIFO_MODE = 3, + LSM6DS_BYPASS_TO_STREAM_MODE = 4, + LSM6DS_STREAM_MODE = 6, +} lsm6ds_fifo_md_t; + +typedef enum { + LSM6DS_FIFO_DISABLE = 0, + LSM6DS_FIFO_12Hz5 = 1, + LSM6DS_FIFO_26Hz = 2, + LSM6DS_FIFO_52Hz = 3, + LSM6DS_FIFO_104Hz = 4, + LSM6DS_FIFO_208Hz = 5, + LSM6DS_FIFO_416Hz = 6, + LSM6DS_FIFO_833Hz = 7, + LSM6DS_FIFO_1k66Hz = 8, + LSM6DS_FIFO_3k33Hz = 9, + LSM6DS_FIFO_6k66Hz = 10, +} lsm6ds_odr_fifo_t; + +typedef enum { + LSM6DS_FIFO_GY_DISABLE = 0, + LSM6DS_FIFO_GY_NO_DEC = 1, + LSM6DS_FIFO_GY_DEC_2 = 2, + LSM6DS_FIFO_GY_DEC_3 = 3, + LSM6DS_FIFO_GY_DEC_4 = 4, + LSM6DS_FIFO_GY_DEC_8 = 5, + LSM6DS_FIFO_GY_DEC_16 = 6, + LSM6DS_FIFO_GY_DEC_32 = 7, +} lsm6ds_dec_fifo_gyro_t; + +typedef enum { + LSM6DS_FIFO_XL_DISABLE = 0, + LSM6DS_FIFO_XL_NO_DEC = 1, + LSM6DS_FIFO_XL_DEC_2 = 2, + LSM6DS_FIFO_XL_DEC_3 = 3, + LSM6DS_FIFO_XL_DEC_4 = 4, + LSM6DS_FIFO_XL_DEC_8 = 5, + LSM6DS_FIFO_XL_DEC_16 = 6, + LSM6DS_FIFO_XL_DEC_32 = 7, +} lsm6ds_dec_fifo_xl_t; + +typedef enum lsm6ds_gy_interrupt { + LSM6DS_GY_DRDY_NONE, + LSM6DS_GY_DRDY_INT_1, + LSM6DS_GY_DRDY_INT_2 +} lsm6ds_gy_interrupt_t; + +typedef enum lsm6ds_xl_interrupt { + LSM6DS_XL_DRDY_NONE, + LSM6DS_XL_DRDY_INT_1, + LSM6DS_XL_DRDY_INT_2 +} lsm6ds_xl_interrupt_t; + +typedef enum lsm6ds_fifo_interrupt { + LSM6DS_FIFO_FULL_NONE, + LSM6DS_FIFO_FULL_INT_1, + LSM6DS_FIFO_FULL_INT_2 +} lsm6ds_fifo_interrupt_t; + + +/* Struct of acc/gyro sensitivities and data rates settings */ +typedef struct lsm6ds_settings { + lsm6ds_xl_fs_t lsm6ds_xl_fs; + lsm6ds_fs_g_t lsm6ds_fs_g; + lsm6ds_odr_xl_t lsm6ds_odr_xl; + lsm6ds_odr_g_t lsm6ds_odr_g; + lsm6ds_gy_interrupt_t lsm6ds_gy_interrupt; + lsm6ds_xl_interrupt_t lsm6ds_xl_interrupt; + lsm6ds_fifo_md_t lsm6ds_fifo_md; + lsm6ds_odr_fifo_t lsm6ds_odr_fifo; + lsm6ds_dec_fifo_gyro_t lsm6ds_dec_fifo_gyro; + lsm6ds_dec_fifo_xl_t lsm6ds_dec_fifo_xl; + lsm6ds_fifo_interrupt_t lsm6ds_fifo_interrupt; + uint16_t lsm6ds_threshold_fifo; +} lsm6ds_settings_t; + +#ifdef __cplusplus +} +#endif + +#endif // LSM6DS_SETTINGS_TYPE_H diff --git a/sources.mk b/sources.mk deleted file mode 100644 index f99aa70..0000000 --- a/sources.mk +++ /dev/null @@ -1,8 +0,0 @@ -THIS_PATH := $(patsubst %/,%,$(dir $(abspath $(lastword $(MAKEFILE_LIST))))) - -C_INCLUDES += \ - -I$(THIS_PATH)/inc - -LIB_SOURCES += $(shell find $(THIS_PATH)/src -name "*.c") - -undefine THIS_PATH diff --git a/src/interfaces/lsm6ds_c_interface.cpp b/src/interfaces/lsm6ds_c_interface.cpp new file mode 100644 index 0000000..b2313d5 --- /dev/null +++ b/src/interfaces/lsm6ds_c_interface.cpp @@ -0,0 +1,51 @@ +/** + * @file lsm6ds_c_interface.cpp + * + * @brief This file is a declaration for a c compatible interface + * + * @author Bruno Machado + * @author Pedro Machado + * + * @date 08/2023 + */ + +#include "lsm6ds_c_interface.h" +#include "lsm6ds_interface.hpp" + +extern "C" { + +static LSM6DS_Interface sensor_interface; + +int8_t lsm6ds_init_I2C(lsm6ds_settings_t lsm6ds_settings, lsm6ds_I2C_pinout I2C_pinout_config) { + return sensor_interface.init(lsm6ds_settings, I2C_pinout_config); +} + +int8_t lsm6ds_init_SPI(lsm6ds_settings_t lsm6ds_settings, lsm6ds_SPI_pinout SPI_pinout_config) { + return sensor_interface.init(lsm6ds_settings, SPI_pinout_config); +} + +void lsm6ds_update_data() { + return sensor_interface.update_data(); +} + +void lsm6ds_update_data_ready_interrupt() { + sensor_interface.update_data_ready_interrupt(); +} + +void lsm6ds_update_data_fifo_full_interrupt() { + sensor_interface.update_data_fifo_full_interrupt(); +} + +void lsm6ds_reset_fifo() { + sensor_interface.reset_fifo(); +} + +float* lsm6ds_get_acc_data_mg() { + return sensor_interface.get_acc_data_mg(); +} + +float* lsm6ds_get_gyro_data_mdps() { + return sensor_interface.get_gyro_data_mdps(); +} + +} diff --git a/src/interfaces/lsm6ds_interface.cpp b/src/interfaces/lsm6ds_interface.cpp new file mode 100644 index 0000000..9d573e5 --- /dev/null +++ b/src/interfaces/lsm6ds_interface.cpp @@ -0,0 +1,112 @@ +/** + * @file lsm6ds.cpp + * + * @brief This file provide functions to get data from the sensor lsm6ds + * + * @author Bruno Guo + * @author Bruno Machado + * + * @date 08/2023 + */ + +#include "lsm6ds_interface.hpp" +#include "lsm6ds3_proxy.hpp" +#include "lsm6ds3tr-c_proxy.hpp" +#include "lsm6dso_proxy.hpp" +#include "platform.h" +#include + +/***************************************** + * Constants + *****************************************/ + +#define WHO_AM_I_ID 0x0FU +#define LSM6DS3TRC_ID 0x6AU +#define LSM6DSO_ID 0x6C +#define LSM6DS3_ID 0x69U + +/***************************************** + * Class Public Methods Bodies Definitions + *****************************************/ + +int8_t LSM6DS_Interface::init(lsm6ds_settings_t lsm6ds_settings, lsm6ds_I2C_pinout_t I2C_pinout_config) { + int32_t rst; + uint8_t whoamI; + I2C_pinout_config.I2C_init(); + rst = platform_read_I2C((lsm6ds_config_t*) &I2C_pinout_config, WHO_AM_I_ID, &whoamI, sizeof(uint8_t)); + + if (rst != 0) { + return LSM6DS_ERROR_WRITE_REGISTER; + } + + switch (whoamI) { + case(LSM6DS3_ID): { + lsm6ds_sensor = std::make_unique(); + return lsm6ds_sensor->init(lsm6ds_settings, I2C_pinout_config, platform_read_I2C, platform_write_I2C); + } + case(LSM6DS3TRC_ID): { + lsm6ds_sensor = std::make_unique(); + return lsm6ds_sensor->init(lsm6ds_settings, I2C_pinout_config, platform_read_I2C, platform_write_I2C); + } + case(LSM6DSO_ID): { + lsm6ds_sensor = std::make_unique(); + return lsm6ds_sensor->init(lsm6ds_settings, I2C_pinout_config, platform_read_I2C, platform_write_I2C); + } + default: { + return LSM6DS_ERROR_WHO_AM_I; + } + } +} + +int8_t LSM6DS_Interface::init(lsm6ds_settings_t lsm6ds_settings, lsm6ds_SPI_pinout_t SPI_pinout_config) { + int32_t rst; + int8_t whoamI; + SPI_pinout_config.SPI_init(); + rst = platform_read_SPI((lsm6ds_config_t*) &SPI_pinout_config, WHO_AM_I_ID, (uint8_t*)&whoamI, sizeof(int8_t)); + + if (rst != 0) { + return LSM6DS_ERROR_WRITE_REGISTER; + } + + switch (whoamI) { + case(LSM6DS3_ID): { + lsm6ds_sensor = std::make_unique(); + return lsm6ds_sensor->init(lsm6ds_settings, SPI_pinout_config, platform_read_SPI, platform_write_SPI); + } + case(LSM6DS3TRC_ID): { + lsm6ds_sensor = std::make_unique(); + return lsm6ds_sensor->init(lsm6ds_settings, SPI_pinout_config, platform_read_SPI, platform_write_SPI); + } + case(LSM6DSO_ID): { + lsm6ds_sensor = std::make_unique(); + return lsm6ds_sensor->init(lsm6ds_settings, SPI_pinout_config, platform_read_SPI, platform_write_SPI); + } + default: { + return LSM6DS_ERROR_WHO_AM_I; + } + } +} + +void LSM6DS_Interface::update_data() { + lsm6ds_sensor->update_data(); +} + +void LSM6DS_Interface::update_data_ready_interrupt() { + lsm6ds_sensor->update_data_ready_interrupt(); +} + +void LSM6DS_Interface::update_data_fifo_full_interrupt() { + lsm6ds_sensor->update_data_fifo_full_interrupt(); +} + +void LSM6DS_Interface::reset_fifo() { + lsm6ds_sensor->reset_fifo(); +} + +float* LSM6DS_Interface::get_acc_data_mg() { + return lsm6ds_sensor->get_acc_data_mg(); +} + +float* LSM6DS_Interface::get_gyro_data_mdps() { + return lsm6ds_sensor->get_gyro_data_mdps(); +} diff --git a/src/lsm6ds3.c b/src/lsm6ds3.c deleted file mode 100644 index a214a33..0000000 --- a/src/lsm6ds3.c +++ /dev/null @@ -1,343 +0,0 @@ -/** - * @file lsm6ds3.c - * - * @brief This file provide functions to get data from the sensor lsm6ds3 - * - * @author Bruno Guo - * - * @date 11/2019 - */ - -/* Uncomment the line with the used protocol and comment the other */ -#define SPI -// #define I2C - -/* Uncommnet if using interrupt pins */ -#define USE_INTERRUPT - -#include "lsm6ds3.h" -#include -#if defined(SPI) -#include "spi.h" -#elif defined(I2C) -#include "i2c.h" -#endif - -#if defined(SPI) -#define SENSOR_BUS hspi1 -#define SPI_INIT MX_SPI1_Init -#define CS_GPIO_PORT GPIOA -#define CS_PIN GPIO_PIN_4 -#elif defined(I2C) -#define SENSOR_BUS hi2c1 -#define I2C_INIT MX_I2C1_Init -#endif - -#if defined(USE_INTERRUPT) -#define INT_GPIO_PORT_XL GPIOC -#define INT_PIN_XL GPIO_PIN_1 -#define INT_GPIO_PORT_G GPIOC -#define INT_PIN_G GPIO_PIN_2 -#define INT1_DRDY_XL PROPERTY_ENABLE -#define INT1_DRDY_G PROPERTY_DISABLE -#define INT2_DRDY_XL PROPERTY_DISABLE -#define INT2_DRDY_G PROPERTY_ENABLE -#endif - -typedef union { - int16_t i16bit[3]; - uint8_t u8bit[6]; -} axis3bit16_t; - -typedef union { - int16_t i16bit; - uint8_t u8bit[2]; -} axis1bit16_t; - -/******************************** - * Private variables - ********************************/ - -static axis3bit16_t data_raw_acceleration; -static axis3bit16_t data_raw_angular_rate; -static axis1bit16_t data_raw_temperature; -static float acceleration_mg[3]; -static float angular_rate_mdps[3]; -static float temperature_degC; -uint8_t whoamI, rst, error_value; -stmdev_ctx_t dev_ctx; -float_t (* acc_conversion_f)(int16_t lsm6ds3_xl_fs); -float_t (* gyro_conversion_f)(int16_t lsm6ds3_fs_g); -lsm6ds3_int1_route_t int_1_reg; -lsm6ds3_int2_route_t int_2_reg; - -/******************************** - * Private functions declarations - ********************************/ - -/** - * @brief Write generic device register (platform dependent) - * - * @param handle customizable argument. In this examples is used in - * order to select the correct sensor bus handler. - * @param reg register to write - * @param bufp pointer to data to write in register reg - * @param len number of consecutive register to write - * - */ -static int32_t platform_write(void* handle, uint8_t reg, uint8_t* bufp, uint16_t len); - -/** - * @brief Read generic device register (platform dependent) - * - * @param handle customizable argument. In this examples is used in - * order to select the correct sensor bus handler. - * @param reg register to read - * @param bufp pointer to buffer that store the data read - * @param len number of consecutive register to read - * - */ -static int32_t platform_read(void* handle, uint8_t reg, uint8_t* bufp, uint16_t len); - -/** - * @brief platform specific initialization (platform dependent) - */ -static void platform_init(); - -/******************************** - * Public Functions Definitions - ********************************/ - -int8_t lsm6ds3_init(lsm6ds3_settings_t lsm6ds3_settings) { - dev_ctx.write_reg = platform_write; - dev_ctx.read_reg = platform_read; - dev_ctx.handle = &SENSOR_BUS; - - /* Initialize platform specific hardware */ - platform_init(&dev_ctx.handle); - - /* Check device ID */ - lsm6ds3_device_id_get(&dev_ctx, &whoamI); - - if (whoamI != LSM6DS3_ID) { - return LSM6DS3_ERROR_WHO_AM_I; - } - - /* Restore default configuration */ - lsm6ds3_reset_set(&dev_ctx, PROPERTY_ENABLE); - - do { - lsm6ds3_reset_get(&dev_ctx, &rst); - } while (rst); - - /* Enable Block Data Update */ - error_value = lsm6ds3_block_data_update_set(&dev_ctx, PROPERTY_ENABLE); - - if (error_value != 0) { - return LSM6DS3_ERROR_WRITE_REGISTER; - } - - /* Set full scale */ - error_value = lsm6ds3_xl_full_scale_set(&dev_ctx, lsm6ds3_settings.lsm6ds3_xl_fs); - - if (error_value != 0) { - return LSM6DS3_ERROR_WRITE_REGISTER; - } - - error_value = lsm6ds3_gy_full_scale_set(&dev_ctx, lsm6ds3_settings.lsm6ds3_fs_g); - - if (error_value != 0) { - return LSM6DS3_ERROR_WRITE_REGISTER; - } - - /* Select the function to convert acc raw data to mg */ - switch (lsm6ds3_settings.lsm6ds3_xl_fs) { - case 0: - acc_conversion_f = lsm6ds3_from_fs2g_to_mg; - break; - - case 1: - acc_conversion_f = lsm6ds3_from_fs16g_to_mg; - break; - - case 2: - acc_conversion_f = lsm6ds3_from_fs4g_to_mg; - break; - - case 3: - acc_conversion_f = lsm6ds3_from_fs8g_to_mg; - break; - - default: - acc_conversion_f = lsm6ds3_from_fs2g_to_mg; - break; - } - - /* Select the function to convert gyro raw data to mdps */ - switch (lsm6ds3_settings.lsm6ds3_fs_g) { - case 0: - gyro_conversion_f = lsm6ds3_from_fs250dps_to_mdps; - break; - - case 1: - gyro_conversion_f = lsm6ds3_from_fs125dps_to_mdps; - break; - - case 2: - gyro_conversion_f = lsm6ds3_from_fs500dps_to_mdps; - break; - - case 4: - gyro_conversion_f = lsm6ds3_from_fs1000dps_to_mdps; - break; - - case 6: - gyro_conversion_f = lsm6ds3_from_fs2000dps_to_mdps; - break; - - default: - gyro_conversion_f = lsm6ds3_from_fs250dps_to_mdps; - break; - } - - /* Set Output Data Rate for Acc and Gyro */ - error_value = lsm6ds3_xl_data_rate_set(&dev_ctx, lsm6ds3_settings.lsm6ds3_odr_xl); - - if (error_value != 0) { - return LSM6DS3_ERROR_WRITE_REGISTER; - } - - error_value = lsm6ds3_gy_data_rate_set(&dev_ctx, lsm6ds3_settings.lsm6ds3_odr_g); - - if (error_value != 0) { - return LSM6DS3_ERROR_WRITE_REGISTER; - } - -#if defined(USE_INTERRUPT) - - /* Enable interrupt generation on DRDY INT1 and INT2 pin */ - lsm6ds3_pin_int1_route_get(&dev_ctx, &int_1_reg); - int_1_reg.int1_drdy_xl = INT1_DRDY_XL; - int_1_reg.int1_drdy_g = INT1_DRDY_G; - lsm6ds3_pin_int1_route_set(&dev_ctx, &int_1_reg); - - lsm6ds3_pin_int2_route_get(&dev_ctx, &int_2_reg); - int_2_reg.int2_drdy_xl = INT2_DRDY_XL; - int_2_reg.int2_drdy_g = INT2_DRDY_G; - lsm6ds3_pin_int2_route_set(&dev_ctx, &int_2_reg); -#endif - - return LSM6DS3_ERROR_NONE; -} - -void lsm6ds3_update_data() { - uint8_t reg; - - /* Read output only if new value is available */ - lsm6ds3_xl_flag_data_ready_get(&dev_ctx, ®); - - if (reg) { - /* Read acceleration field data */ - memset(data_raw_acceleration.u8bit, 0x00, 3 * sizeof(int16_t)); - lsm6ds3_acceleration_raw_get(&dev_ctx, data_raw_acceleration.u8bit); - - for (int i = 0; i < 3; i++) { - acceleration_mg[i] = acc_conversion_f(data_raw_acceleration.i16bit[i]); - } - } - - lsm6ds3_gy_flag_data_ready_get(&dev_ctx, ®); - - if (reg) { - /* Read angular rate field data */ - memset(data_raw_angular_rate.u8bit, 0x00, 3 * sizeof(int16_t)); - lsm6ds3_angular_rate_raw_get(&dev_ctx, data_raw_angular_rate.u8bit); - - for (int j = 0; j < 3; j++) { - angular_rate_mdps[j] = gyro_conversion_f(data_raw_angular_rate.i16bit[j]); - } - } - - // lsm6ds3_temp_flag_data_ready_get(&dev_ctx, ®); - // if (reg) { - // /* Read temperature data */ - // memset(data_raw_temperature.u8bit, 0x00, sizeof(int16_t)); - // lsm6ds3_temperature_raw_get(&dev_ctx, data_raw_temperature.u8bit); - // temperature_degC = lsm6ds3_from_lsb_to_celsius(data_raw_temperature.i16bit); - // } -} - -#if defined(USE_INTERRUPT) -void lsm6ds3_update_data_interrupt() { - /* Read acceleration field data when corresponding INT is HIGH */ - if (HAL_GPIO_ReadPin(INT_GPIO_PORT_XL, INT_PIN_XL) == 1) { - memset(data_raw_acceleration.u8bit, 0x00, 3 * sizeof(int16_t)); - lsm6ds3_acceleration_raw_get(&dev_ctx, data_raw_acceleration.u8bit); - - for (int i = 0; i < 3; i++) { - acceleration_mg[i] = acc_conversion_f(data_raw_acceleration.i16bit[i]); - } - } - - /* Read angular rate field data when corresponding INT is HIGH */ - if (HAL_GPIO_ReadPin(INT_GPIO_PORT_G, INT_PIN_G) == 1) { - memset(data_raw_angular_rate.u8bit, 0x00, 3 * sizeof(int16_t)); - lsm6ds3_angular_rate_raw_get(&dev_ctx, data_raw_angular_rate.u8bit); - - for (int i = 0; i < 3; i++) { - angular_rate_mdps[i] = gyro_conversion_f(data_raw_angular_rate.i16bit[i]); - } - } -} - -#endif - -float* lsm6ds3_get_acc_data_mg() { - return acceleration_mg; -} - -float* lsm6ds3_get_gyro_data_mdps() { - return angular_rate_mdps; -} - -float lsm6ds3_get_temperature_degC() { - return temperature_degC; -} - -/******************************** - * Private Functions Definitions - ********************************/ - -static int32_t platform_write(void* handle, uint8_t reg, uint8_t* bufp, uint16_t len) { -#if defined(I2C) - HAL_I2C_Mem_Write(handle, LSM6DS3_I2C_ADD_H, reg, I2C_MEMADD_SIZE_8BIT, bufp, len, 1000); -#elif defined(SPI) - HAL_GPIO_WritePin(CS_GPIO_PORT, CS_PIN, GPIO_PIN_RESET); - HAL_SPI_Transmit(handle, ®, 1, 1000); - HAL_SPI_Transmit(handle, bufp, len, 1000); - HAL_GPIO_WritePin(CS_GPIO_PORT, CS_PIN, GPIO_PIN_SET); -#endif - return 0; -} - -static int32_t platform_read(void* handle, uint8_t reg, uint8_t* bufp, uint16_t len) { -#if defined(I2C) - HAL_I2C_Mem_Read(handle, LSM6DS3_I2C_ADD_H, reg, I2C_MEMADD_SIZE_8BIT, bufp, len, 1000); -#elif defined(SPI) - /* MSB must be 1 when reading */ - reg |= 0x80; - HAL_GPIO_WritePin(CS_GPIO_PORT, CS_PIN, GPIO_PIN_RESET); - HAL_SPI_Transmit(handle, ®, 1, 1000); - HAL_SPI_Receive(handle, bufp, len, 1000); - HAL_GPIO_WritePin(CS_GPIO_PORT, CS_PIN, GPIO_PIN_SET); -#endif - return 0; -} - -static void platform_init() { -#if defined(I2C) - I2C_INIT(); -#elif defined(SPI) - SPI_INIT(); -#endif -} diff --git a/src/lsm6ds3_reg.c b/src/lsm6ds3_reg.c deleted file mode 100644 index 09550a3..0000000 --- a/src/lsm6ds3_reg.c +++ /dev/null @@ -1,6122 +0,0 @@ -/* - ****************************************************************************** - * @file lsm6ds3_reg.c - * @author Sensors Software Solution Team - * @brief LSM6DS3 driver file - ****************************************************************************** - * @attention - * - *

© Copyright (c) 2019 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 "lsm6ds3_reg.h" - -/** - * @defgroup LSM6DS3 - * @brief This file provides a set of functions needed to drive the - * lsm6ds3 enhanced inertial module. - * @{ - * - */ - -/** - * @defgroup LSM6DS3_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 read / write interface definitions(ptr) - * @param reg register to read - * @param data pointer to buffer that store the data read(ptr) - * @param len number of consecutive register to read - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_read_reg(stmdev_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 read / write interface definitions(ptr) - * @param reg register to write - * @param data pointer to data to write in register reg(ptr) - * @param len number of consecutive register to write - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_write_reg(stmdev_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 LSM6DS3_Sensitivity - * @brief These functions convert raw-data into engineering units. - * @{ - * - */ - -float_t lsm6ds3_from_fs2g_to_mg(int16_t lsb) { - return ((float_t) lsb * 61.0f / 1000.0f); -} - -float_t lsm6ds3_from_fs4g_to_mg(int16_t lsb) { - return ((float_t) lsb * 122.0f / 1000.0f); -} - -float_t lsm6ds3_from_fs8g_to_mg(int16_t lsb) { - return ((float_t) lsb * 244.0f / 1000.0f); -} - -float_t lsm6ds3_from_fs16g_to_mg(int16_t lsb) { - return ((float_t) lsb * 488.0f / 1000.0f); -} - -float_t lsm6ds3_from_fs125dps_to_mdps(int16_t lsb) { - return ((float_t) lsb * 4375.0f / 1000.0f); -} - -float_t lsm6ds3_from_fs250dps_to_mdps(int16_t lsb) { - return ((float_t) lsb * 8750.0f / 1000.0f); -} - -float_t lsm6ds3_from_fs500dps_to_mdps(int16_t lsb) { - return ((float_t) lsb * 1750.0f / 100.0f); -} - -float_t lsm6ds3_from_fs1000dps_to_mdps(int16_t lsb) { - return ((float_t) lsb * 35.0f); -} - -float_t lsm6ds3_from_fs2000dps_to_mdps(int16_t lsb) { - return ((float_t) lsb * 70.0f); -} - -float_t lsm6ds3_from_lsb_to_celsius(int16_t lsb) { - return ((float_t) lsb / 16.0f + 25.0f); -} - -/** - * @} - * - */ - -/** - * @defgroup LSM6DS3_Data_generation - * @brief This section groups all the functions concerning - * data generation - * @{ - * - */ - -/** - * @brief Gyroscope directional user-orientation selection.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of orient in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_gy_data_orient_set(stmdev_ctx_t* ctx, lsm6ds3_gy_orient_t val) { - lsm6ds3_orient_cfg_g_t orient_cfg_g; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_ORIENT_CFG_G, - (uint8_t*) &orient_cfg_g, 1); - - if (ret == 0) { - orient_cfg_g.orient = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_ORIENT_CFG_G, - (uint8_t*) &orient_cfg_g, 1); - } - - return ret; -} - -/** - * @brief Gyroscope directional user-orientation selection.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of orient in reg ORIENT_CFG_G - * - */ -int32_t lsm6ds3_gy_data_orient_get(stmdev_ctx_t* ctx, lsm6ds3_gy_orient_t* val) { - lsm6ds3_orient_cfg_g_t orient_cfg_g; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_ORIENT_CFG_G, - (uint8_t*) &orient_cfg_g, 1); - - switch (orient_cfg_g.orient) { - case LSM6DS3_GY_ORIENT_XYZ: - *val = LSM6DS3_GY_ORIENT_XYZ; - break; - - case LSM6DS3_GY_ORIENT_XZY: - *val = LSM6DS3_GY_ORIENT_XZY; - break; - - case LSM6DS3_GY_ORIENT_YXZ: - *val = LSM6DS3_GY_ORIENT_YXZ; - break; - - case LSM6DS3_GY_ORIENT_YZX: - *val = LSM6DS3_GY_ORIENT_YZX; - break; - - case LSM6DS3_GY_ORIENT_ZXY: - *val = LSM6DS3_GY_ORIENT_ZXY; - break; - - case LSM6DS3_GY_ORIENT_ZYX: - *val = LSM6DS3_GY_ORIENT_ZYX; - break; - - default: - *val = LSM6DS3_GY_ORIENT_XYZ; - break; - } - - return ret; -} - -/** - * @brief angular rate sign.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of sign_g in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_gy_data_sign_set(stmdev_ctx_t* ctx, lsm6ds3_gy_sgn_t val) { - lsm6ds3_orient_cfg_g_t orient_cfg_g; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_ORIENT_CFG_G, - (uint8_t*) &orient_cfg_g, 1); - - if (ret == 0) { - orient_cfg_g.sign_g = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_ORIENT_CFG_G, - (uint8_t*) &orient_cfg_g, 1); - } - - return ret; -} - -/** - * @brief angularratesign.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of sign_g in reg ORIENT_CFG_G - * - */ -int32_t lsm6ds3_gy_data_sign_get(stmdev_ctx_t* ctx, lsm6ds3_gy_sgn_t* val) { - lsm6ds3_orient_cfg_g_t orient_cfg_g; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_ORIENT_CFG_G, - (uint8_t*) &orient_cfg_g, 1); - - switch (orient_cfg_g.sign_g) { - case LSM6DS3_GY_SIGN_PPP: - *val = LSM6DS3_GY_SIGN_PPP; - break; - - case LSM6DS3_GY_SIGN_PPN: - *val = LSM6DS3_GY_SIGN_PPN; - break; - - case LSM6DS3_GY_SIGN_PNP: - *val = LSM6DS3_GY_SIGN_PNP; - break; - - case LSM6DS3_GY_SIGN_NPP: - *val = LSM6DS3_GY_SIGN_NPP; - break; - - case LSM6DS3_GY_SIGN_NNP: - *val = LSM6DS3_GY_SIGN_NNP; - break; - - case LSM6DS3_GY_SIGN_NPN: - *val = LSM6DS3_GY_SIGN_NPN; - break; - - case LSM6DS3_GY_SIGN_PNN: - *val = LSM6DS3_GY_SIGN_PNN; - break; - - case LSM6DS3_GY_SIGN_NNN: - *val = LSM6DS3_GY_SIGN_NNN; - break; - - default: - *val = LSM6DS3_GY_SIGN_PPP; - break; - } - - return ret; -} - -/** - * @brief Accelerometer full-scale selection.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of fs_xl in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_xl_full_scale_set(stmdev_ctx_t* ctx, lsm6ds3_xl_fs_t val) { - lsm6ds3_ctrl1_xl_t ctrl1_xl; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL1_XL, (uint8_t*) &ctrl1_xl, 1); - - if (ret == 0) { - ctrl1_xl.fs_xl = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL1_XL, (uint8_t*) &ctrl1_xl, 1); - } - - return ret; -} - -/** - * @brief Accelerometer full-scale selection.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of fs_xl in reg CTRL1_XL - * - */ -int32_t lsm6ds3_xl_full_scale_get(stmdev_ctx_t* ctx, lsm6ds3_xl_fs_t* val) { - lsm6ds3_ctrl1_xl_t ctrl1_xl; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL1_XL, (uint8_t*) &ctrl1_xl, 1); - - switch (ctrl1_xl.fs_xl) { - case LSM6DS3_2g: - *val = LSM6DS3_2g; - break; - - case LSM6DS3_16g: - *val = LSM6DS3_16g; - break; - - case LSM6DS3_4g: - *val = LSM6DS3_4g; - break; - - case LSM6DS3_8g: - *val = LSM6DS3_8g; - break; - - default: - *val = LSM6DS3_2g; - break; - } - - return ret; -} - -/** - * @brief Accelerometer data rate selection.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of odr_xl in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_xl_data_rate_set(stmdev_ctx_t* ctx, lsm6ds3_odr_xl_t val) { - lsm6ds3_ctrl1_xl_t ctrl1_xl; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL1_XL, (uint8_t*) &ctrl1_xl, 1); - - if (ret == 0) { - ctrl1_xl.odr_xl = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL1_XL, (uint8_t*) &ctrl1_xl, 1); - } - - return ret; -} - -/** - * @brief Accelerometer data rate selection.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of odr_xl in reg CTRL1_XL - * - */ -int32_t lsm6ds3_xl_data_rate_get(stmdev_ctx_t* ctx, lsm6ds3_odr_xl_t* val) { - lsm6ds3_ctrl1_xl_t ctrl1_xl; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL1_XL, (uint8_t*) &ctrl1_xl, 1); - - switch (ctrl1_xl.odr_xl) { - case LSM6DS3_XL_ODR_OFF: - *val = LSM6DS3_XL_ODR_OFF; - break; - - case LSM6DS3_XL_ODR_12Hz5: - *val = LSM6DS3_XL_ODR_12Hz5; - break; - - case LSM6DS3_XL_ODR_26Hz: - *val = LSM6DS3_XL_ODR_26Hz; - break; - - case LSM6DS3_XL_ODR_52Hz: - *val = LSM6DS3_XL_ODR_52Hz; - break; - - case LSM6DS3_XL_ODR_104Hz: - *val = LSM6DS3_XL_ODR_104Hz; - break; - - case LSM6DS3_XL_ODR_208Hz: - *val = LSM6DS3_XL_ODR_208Hz; - break; - - case LSM6DS3_XL_ODR_416Hz: - *val = LSM6DS3_XL_ODR_416Hz; - break; - - case LSM6DS3_XL_ODR_833Hz: - *val = LSM6DS3_XL_ODR_833Hz; - break; - - case LSM6DS3_XL_ODR_1k66Hz: - *val = LSM6DS3_XL_ODR_1k66Hz; - break; - - case LSM6DS3_XL_ODR_3k33Hz: - *val = LSM6DS3_XL_ODR_3k33Hz; - break; - - case LSM6DS3_XL_ODR_6k66Hz: - *val = LSM6DS3_XL_ODR_6k66Hz; - break; - - default: - *val = LSM6DS3_XL_ODR_OFF; - break; - } - - return ret; -} - -/** - * @brief Gyroscope UI chain full-scale selection.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of fs_g in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_gy_full_scale_set(stmdev_ctx_t* ctx, lsm6ds3_fs_g_t val) { - lsm6ds3_ctrl2_g_t ctrl2_g; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL2_G, (uint8_t*) &ctrl2_g, 1); - - if (ret == 0) { - ctrl2_g.fs_g = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL2_G, (uint8_t*) &ctrl2_g, 1); - } - - return ret; -} - -/** - * @brief Gyroscope UI chain full-scale selection.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of fs_g in reg CTRL2_G - * - */ -int32_t lsm6ds3_gy_full_scale_get(stmdev_ctx_t* ctx, lsm6ds3_fs_g_t* val) { - lsm6ds3_ctrl2_g_t ctrl2_g; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL2_G, (uint8_t*) &ctrl2_g, 1); - - switch (ctrl2_g.fs_g) { - case LSM6DS3_250dps: - *val = LSM6DS3_250dps; - break; - - case LSM6DS3_125dps: - *val = LSM6DS3_125dps; - break; - - case LSM6DS3_500dps: - *val = LSM6DS3_500dps; - break; - - case LSM6DS3_1000dps: - *val = LSM6DS3_1000dps; - break; - - case LSM6DS3_2000dps: - *val = LSM6DS3_2000dps; - break; - - default: - *val = LSM6DS3_250dps; - break; - } - - return ret; -} - -/** - * @brief Gyroscope UI data rate selection.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of odr_g in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_gy_data_rate_set(stmdev_ctx_t* ctx, lsm6ds3_odr_g_t val) { - lsm6ds3_ctrl2_g_t ctrl2_g; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL2_G, (uint8_t*) &ctrl2_g, 1); - - if (ret == 0) { - ctrl2_g.odr_g = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL2_G, (uint8_t*) &ctrl2_g, 1); - } - - return ret; -} - -/** - * @brief Gyroscope UI data rate selection.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of odr_g in reg CTRL2_G - * - */ -int32_t lsm6ds3_gy_data_rate_get(stmdev_ctx_t* ctx, lsm6ds3_odr_g_t* val) { - lsm6ds3_ctrl2_g_t ctrl2_g; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL2_G, (uint8_t*) &ctrl2_g, 1); - - switch (ctrl2_g.odr_g) { - case LSM6DS3_GY_ODR_OFF: - *val = LSM6DS3_GY_ODR_OFF; - break; - - case LSM6DS3_GY_ODR_12Hz5: - *val = LSM6DS3_GY_ODR_12Hz5; - break; - - case LSM6DS3_GY_ODR_26Hz: - *val = LSM6DS3_GY_ODR_26Hz; - break; - - case LSM6DS3_GY_ODR_52Hz: - *val = LSM6DS3_GY_ODR_52Hz; - break; - - case LSM6DS3_GY_ODR_104Hz: - *val = LSM6DS3_GY_ODR_104Hz; - break; - - case LSM6DS3_GY_ODR_208Hz: - *val = LSM6DS3_GY_ODR_208Hz; - break; - - case LSM6DS3_GY_ODR_416Hz: - *val = LSM6DS3_GY_ODR_416Hz; - break; - - case LSM6DS3_GY_ODR_833Hz: - *val = LSM6DS3_GY_ODR_833Hz; - break; - - case LSM6DS3_GY_ODR_1k66Hz: - *val = LSM6DS3_GY_ODR_1k66Hz; - break; - - default: - *val = LSM6DS3_GY_ODR_OFF; - break; - } - - return ret; -} - -/** - * @brief Blockdataupdate.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of bdu in reg CTRL3_C - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_block_data_update_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_ctrl3_c_t ctrl3_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t*) &ctrl3_c, 1); - - if (ret == 0) { - ctrl3_c.bdu = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t*) &ctrl3_c, 1); - } - - return ret; -} - -/** - * @brief Blockdataupdate.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of bdu in reg CTRL3_C - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_block_data_update_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_ctrl3_c_t ctrl3_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t*) &ctrl3_c, 1); - *val = (uint8_t) ctrl3_c.bdu; - - return ret; -} - -/** - * @brief High-performance operating mode for accelerometer.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of xl_hm_mode in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_xl_power_mode_set(stmdev_ctx_t* ctx, lsm6ds3_xl_hm_mode_t val) { - lsm6ds3_ctrl6_c_t ctrl6_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL6_C, (uint8_t*) &ctrl6_c, 1); - - if (ret == 0) { - ctrl6_c.xl_hm_mode = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL6_C, (uint8_t*) &ctrl6_c, 1); - } - - return ret; -} - -/** - * @brief High-performance operating mode for accelerometer.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of xl_hm_mode in reg CTRL6_C - * - */ -int32_t lsm6ds3_xl_power_mode_get(stmdev_ctx_t* ctx, lsm6ds3_xl_hm_mode_t* val) { - lsm6ds3_ctrl6_c_t ctrl6_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL6_C, (uint8_t*) &ctrl6_c, 1); - - switch (ctrl6_c.xl_hm_mode) { - case LSM6DS3_XL_HIGH_PERFORMANCE: - *val = LSM6DS3_XL_HIGH_PERFORMANCE; - break; - - case LSM6DS3_XL_NORMAL: - *val = LSM6DS3_XL_NORMAL; - break; - - default: - *val = LSM6DS3_XL_HIGH_PERFORMANCE; - break; - } - - return ret; -} - -/** - * @brief Source register rounding function on ADD HERE ROUNDING REGISTERS.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of rounding_status in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_rounding_on_status_set(stmdev_ctx_t* ctx, lsm6ds3_rnd_stat_t val) { - lsm6ds3_ctrl7_g_t ctrl7_g; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL7_G, (uint8_t*) &ctrl7_g, 1); - - if (ret == 0) { - ctrl7_g.rounding_status = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL7_G, (uint8_t*) &ctrl7_g, 1); - } - - return ret; -} - -/** - * @brief Source register rounding function on ADD HERE ROUNDING REGISTERS.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of rounding_status in reg CTRL7_G - * - */ -int32_t lsm6ds3_rounding_on_status_get(stmdev_ctx_t* ctx, lsm6ds3_rnd_stat_t* val) { - lsm6ds3_ctrl7_g_t ctrl7_g; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL7_G, (uint8_t*) &ctrl7_g, 1); - - switch (ctrl7_g.rounding_status) { - case LSM6DS3_STAT_RND_DISABLE: - *val = LSM6DS3_STAT_RND_DISABLE; - break; - - case LSM6DS3_STAT_RND_ENABLE: - *val = LSM6DS3_STAT_RND_ENABLE; - break; - - default: - *val = LSM6DS3_STAT_RND_DISABLE; - break; - } - - return ret; -} - -/** - * @brief High-performance operating mode disable for gyroscope.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of g_hm_mode in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_gy_power_mode_set(stmdev_ctx_t* ctx, lsm6ds3_g_hm_mode_t val) { - lsm6ds3_ctrl7_g_t ctrl7_g; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL7_G, (uint8_t*) &ctrl7_g, 1); - - if (ret == 0) { - ctrl7_g.g_hm_mode = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL7_G, (uint8_t*) &ctrl7_g, 1); - } - - return ret; -} - -/** - * @brief High-performance operating mode disable for gyroscope.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of g_hm_mode in reg CTRL7_G - * - */ -int32_t lsm6ds3_gy_power_mode_get(stmdev_ctx_t* ctx, lsm6ds3_g_hm_mode_t* val) { - lsm6ds3_ctrl7_g_t ctrl7_g; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL7_G, (uint8_t*) &ctrl7_g, 1); - - switch (ctrl7_g.g_hm_mode) { - case LSM6DS3_GY_HIGH_PERFORMANCE: - *val = LSM6DS3_GY_HIGH_PERFORMANCE; - break; - - case LSM6DS3_GY_NORMAL: - *val = LSM6DS3_GY_NORMAL; - break; - - default: - *val = LSM6DS3_GY_HIGH_PERFORMANCE; - break; - } - - return ret; -} - -/** - * @brief Accelerometer X-axis output enable/disable.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of xen_xl in reg CTRL9_XL - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_xl_axis_x_data_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_ctrl9_xl_t ctrl9_xl; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL9_XL, (uint8_t*) &ctrl9_xl, 1); - - if (ret == 0) { - ctrl9_xl.xen_xl = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL9_XL, (uint8_t*) &ctrl9_xl, 1); - } - - return ret; -} - -/** - * @brief Accelerometer X-axis output enable/disable.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of xen_xl in reg CTRL9_XL - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_xl_axis_x_data_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_ctrl9_xl_t ctrl9_xl; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL9_XL, (uint8_t*) &ctrl9_xl, 1); - *val = (uint8_t) ctrl9_xl.xen_xl; - - return ret; -} - -/** - * @brief Accelerometer Y-axis output enable/disable.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of yen_xl in reg CTRL9_XL - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_xl_axis_y_data_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_ctrl9_xl_t ctrl9_xl; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL9_XL, (uint8_t*) &ctrl9_xl, 1); - - if (ret == 0) { - ctrl9_xl.yen_xl = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL9_XL, (uint8_t*) &ctrl9_xl, 1); - } - - return ret; -} - -/** - * @brief Accelerometer Y-axis output enable/disable.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of yen_xl in reg CTRL9_XL - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_xl_axis_y_data_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_ctrl9_xl_t ctrl9_xl; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL9_XL, (uint8_t*) &ctrl9_xl, 1); - *val = (uint8_t) ctrl9_xl.yen_xl; - - return ret; -} - -/** - * @brief Accelerometer Z-axis output enable/disable.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of zen_xl in reg CTRL9_XL - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_xl_axis_z_data_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_ctrl9_xl_t ctrl9_xl; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL9_XL, (uint8_t*) &ctrl9_xl, 1); - - if (ret == 0) { - ctrl9_xl.zen_xl = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL9_XL, (uint8_t*) &ctrl9_xl, 1); - } - - return ret; -} - -/** - * @brief Accelerometer Z-axis output enable/disable.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of zen_xl in reg CTRL9_XL - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_xl_axis_z_data_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_ctrl9_xl_t ctrl9_xl; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL9_XL, (uint8_t*) &ctrl9_xl, 1); - *val = (uint8_t) ctrl9_xl.zen_xl; - - return ret; -} - -/** - * @brief Gyroscope pitch axis (X) output enable/disable.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of xen_g in reg CTRL10_C - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_gy_axis_x_data_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_ctrl10_c_t ctrl10_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t*) &ctrl10_c, 1); - - if (ret == 0) { - ctrl10_c.xen_g = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t*) &ctrl10_c, 1); - } - - return ret; -} - -/** - * @brief Gyroscope pitch axis (X) output enable/disable.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of xen_g in reg CTRL10_C - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_gy_axis_x_data_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_ctrl10_c_t ctrl10_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t*) &ctrl10_c, 1); - *val = (uint8_t) ctrl10_c.xen_g; - - return ret; -} - -/** - * @brief Gyroscope pitch axis (Y) output enable/disable.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of yen_g in reg CTRL10_C - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_gy_axis_y_data_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_ctrl10_c_t ctrl10_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t*) &ctrl10_c, 1); - - if (ret == 0) { - ctrl10_c.yen_g = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t*) &ctrl10_c, 1); - } - - return ret; -} - -/** - * @brief Gyroscope pitch axis (Y) output enable/disable.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of yen_g in reg CTRL10_C - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_gy_axis_y_data_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_ctrl10_c_t ctrl10_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t*) &ctrl10_c, 1); - *val = (uint8_t) ctrl10_c.yen_g; - - return ret; -} - -/** - * @brief Gyroscope pitch axis (Z) output enable/disable.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of zen_g in reg CTRL10_C - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_gy_axis_z_data_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_ctrl10_c_t ctrl10_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t*) &ctrl10_c, 1); - - if (ret == 0) { - ctrl10_c.zen_g = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t*) &ctrl10_c, 1); - } - - return ret; -} - -/** - * @brief Gyroscope pitch axis (Z) output enable/disable.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of zen_g in reg CTRL10_C - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_gy_axis_z_data_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_ctrl10_c_t ctrl10_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t*) &ctrl10_c, 1); - *val = (uint8_t) ctrl10_c.zen_g; - - return ret; -} - -/** - * @brief Read all the interrupt/status flag of the device. ELENCA I REGISTRI[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val Read all the interrupt flag of the device: - * WAKE_UP_SRC, TAP_SRC, D6D_SRC, FUNC_SRC. - * - */ -int32_t lsm6ds3_all_sources_get(stmdev_ctx_t* ctx, lsm6ds3_all_src_t* val) { - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_WAKE_UP_SRC, - (uint8_t*) &(val->wake_up_src), 1); - - if (ret == 0) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_SRC, - (uint8_t*) &(val->tap_src), 1); - } - - if (ret == 0) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_D6D_SRC, - (uint8_t*) &(val->d6d_src), 1); - } - - if (ret == 0) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FUNC_SRC, - (uint8_t*) &(val->func_src), 1); - } - - return ret; -} - -/** - * @brief The STATUS_REG register of the device.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val The STATUS_REG register of the device. - * - */ -int32_t lsm6ds3_status_reg_get(stmdev_ctx_t* ctx, lsm6ds3_status_reg_t* val) { - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_STATUS_REG, (uint8_t*) &val, 1); - - return ret; -} - -/** - * @brief Accelerometer new data available.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of xlda in reg STATUS_REG - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_xl_flag_data_ready_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_status_reg_t status_reg; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_STATUS_REG, (uint8_t*) &status_reg, 1); - *val = (uint8_t) status_reg.xlda; - - return ret; -} - -/** - * @brief Gyroscope new data available.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of gda in reg STATUS_REG - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_gy_flag_data_ready_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_status_reg_t status_reg; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_STATUS_REG, (uint8_t*) &status_reg, 1); - *val = (uint8_t) status_reg.gda; - - return ret; -} - -/** - * @brief Temperature new data available.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of tda in reg STATUS_REG - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_temp_flag_data_ready_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_status_reg_t status_reg; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_STATUS_REG, (uint8_t*) &status_reg, 1); - *val = (uint8_t) status_reg.tda; - - return ret; -} - -/** - * @} - * - */ - -/** - * @defgroup LSM6DS3_Timestamp - * @brief This section groups all the functions that manage the - * timestamp generation. - * @{ - * - */ - -/** - * @brief Timestamp first byte data output register (r). The value is - * expressed as a 24-bit word and the bit resolution is defined - * by setting the value in WAKE_UP_DUR (5Ch).[get] - * - * @param ctx read / write interface definitions(ptr) - * @param buff buffer that stores data read - * - */ -int32_t lsm6ds3_timestamp_raw_get(stmdev_ctx_t* ctx, uint8_t* buff) { - int32_t ret; - ret = lsm6ds3_read_reg(ctx, LSM6DS3_TIMESTAMP0_REG, buff, - 3); - return ret; -} - -/** - * @brief Reset the timer.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param buff buffer that stores data to be write - * - */ -int32_t lsm6ds3_timestamp_rst_set(stmdev_ctx_t* ctx) { - int32_t ret; - uint8_t rst_val = 0xAA; - - ret = lsm6ds3_write_reg(ctx, LSM6DS3_TIMESTAMP2_REG, &rst_val, 1); - return ret; -} - -/** - * @brief Timestamp count enable, output data are collected in - * TIMESTAMP0_REG (40h), TIMESTAMP1_REG (41h), - * TIMESTAMP2_REG (42h) register.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of timer_en in reg TAP_CFG - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_timestamp_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_tap_cfg_t tap_cfg; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t*) &tap_cfg, 1); - - if (ret == 0) { - tap_cfg.timer_en = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t*) &tap_cfg, 1); - } - - return ret; -} - -/** - * @brief Timestamp count enable, output data are collected in - * TIMESTAMP0_REG (40h), TIMESTAMP1_REG (41h), - * TIMESTAMP2_REG (42h) register.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of timer_en in reg TAP_CFG - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_timestamp_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_tap_cfg_t tap_cfg; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t*) &tap_cfg, 1); - *val = (uint8_t) tap_cfg.timer_en; - - return ret; -} - -/** - * @brief Timestamp register resolution setting.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of timer_hr in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_timestamp_res_set(stmdev_ctx_t* ctx, lsm6ds3_ts_res_t val) { - lsm6ds3_wake_up_dur_t wake_up_dur; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_WAKE_UP_DUR, (uint8_t*) &wake_up_dur, 1); - - if (ret == 0) { - wake_up_dur.timer_hr = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_WAKE_UP_DUR, - (uint8_t*) &wake_up_dur, 1); - } - - return ret; -} - -/** - * @brief Timestamp register resolution setting.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of timer_hr in reg WAKE_UP_DUR - * - */ -int32_t lsm6ds3_timestamp_res_get(stmdev_ctx_t* ctx, lsm6ds3_ts_res_t* val) { - lsm6ds3_wake_up_dur_t wake_up_dur; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_WAKE_UP_DUR, (uint8_t*) &wake_up_dur, 1); - - switch (wake_up_dur.timer_hr) { - case LSM6DS3_LSB_6ms4: - *val = LSM6DS3_LSB_6ms4; - break; - - case LSM6DS3_LSB_25us: - *val = LSM6DS3_LSB_25us; - break; - - default: - *val = LSM6DS3_LSB_6ms4; - break; - } - - return ret; -} - -/** - * @} - * - */ - -/** - * @defgroup LSM6DS3_Dataoutput - * @brief This section groups all the data output functions. - * @{ - * - */ - -/** - * @brief Circular burst-mode (rounding) read from output registers - * through the primary interface.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of rounding in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_rounding_mode_set(stmdev_ctx_t* ctx, lsm6ds3_rounding_t val) { - lsm6ds3_ctrl5_c_t ctrl5_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL5_C, (uint8_t*) &ctrl5_c, 1); - - if (ret == 0) { - ctrl5_c.rounding = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL5_C, (uint8_t*) &ctrl5_c, 1); - } - - return ret; -} - -/** - * @brief Circular burst-mode (rounding) read from output registers - * through the primary interface.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of rounding in reg CTRL5_C - * - */ -int32_t lsm6ds3_rounding_mode_get(stmdev_ctx_t* ctx, lsm6ds3_rounding_t* val) { - lsm6ds3_ctrl5_c_t ctrl5_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL5_C, (uint8_t*) &ctrl5_c, 1); - - switch (ctrl5_c.rounding) { - case LSM6DS3_ROUND_DISABLE: - *val = LSM6DS3_ROUND_DISABLE; - break; - - case LSM6DS3_ROUND_XL: - *val = LSM6DS3_ROUND_XL; - break; - - case LSM6DS3_ROUND_GY: - *val = LSM6DS3_ROUND_GY; - break; - - case LSM6DS3_ROUND_GY_XL: - *val = LSM6DS3_ROUND_GY_XL; - break; - - case LSM6DS3_ROUND_SH1_TO_SH6: - *val = LSM6DS3_ROUND_SH1_TO_SH6; - break; - - case LSM6DS3_ROUND_XL_SH1_TO_SH6: - *val = LSM6DS3_ROUND_XL_SH1_TO_SH6; - break; - - case LSM6DS3_ROUND_GY_XL_SH1_TO_SH12: - *val = LSM6DS3_ROUND_GY_XL_SH1_TO_SH12; - break; - - case LSM6DS3_ROUND_GY_XL_SH1_TO_SH6: - *val = LSM6DS3_ROUND_GY_XL_SH1_TO_SH6; - break; - - default: - *val = LSM6DS3_ROUND_DISABLE; - break; - } - - return ret; -} - -/** - * @brief Temperature data output register (r). L and H registers together - * express a 16-bit word in two’s complement.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param buff buffer that stores data read - * - */ -int32_t lsm6ds3_temperature_raw_get(stmdev_ctx_t* ctx, uint8_t* buff) { - int32_t ret; - ret = lsm6ds3_read_reg(ctx, LSM6DS3_OUT_TEMP_L, buff, 2); - return ret; -} - -/** - * @brief Angular rate sensor. The value is expressed as a 16-bit word - * in two’s complement..[get] - * - * @param ctx read / write interface definitions(ptr) - * @param buff buffer that stores data read - * - */ -int32_t lsm6ds3_angular_rate_raw_get(stmdev_ctx_t* ctx, uint8_t* buff) { - int32_t ret; - ret = lsm6ds3_read_reg(ctx, LSM6DS3_OUTX_L_G, buff, 6); - return ret; -} - -/** - * @brief Linear acceleration output register. The value is expressed - * as a 16-bit word in two’s complement.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param buff buffer that stores data read - * - */ -int32_t lsm6ds3_acceleration_raw_get(stmdev_ctx_t* ctx, uint8_t* buff) { - int32_t ret; - ret = lsm6ds3_read_reg(ctx, LSM6DS3_OUTX_L_XL, buff, 6); - return ret; -} - -/** - * @brief fifo_raw_data: [get] read data in FIFO. - * - * @param stmdev_ctx_t *ctx: read / write interface definitions - * @param uint8_t *: data buffer to store FIFO data. - * @param uint8_t : number of data to read from FIFO. - * - */ -int32_t lsm6ds3_fifo_raw_data_get(stmdev_ctx_t* ctx, uint8_t* buffer, uint8_t len) { - int32_t ret; - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_DATA_OUT_L, buffer, len); - return ret; -} - -/** - * @brief Step counter output register..[get] - * - * @param ctx read / write interface definitions(ptr) - * @param buff buffer that stores data read - * - */ -int32_t lsm6ds3_number_of_steps_get(stmdev_ctx_t* ctx, uint8_t* buff) { - int32_t ret; - ret = lsm6ds3_read_reg(ctx, LSM6DS3_STEP_COUNTER_L, buff, 2); - return ret; -} - -/** - * @brief magnetometer raw data.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param buff buffer that stores data read - * - */ -int32_t lsm6ds3_mag_calibrated_raw_get(stmdev_ctx_t* ctx, uint8_t* buff) { - int32_t ret; - ret = lsm6ds3_read_reg(ctx, LSM6DS3_OUT_MAG_RAW_X_L, buff, 6); - return ret; -} - -/** - * @} - * - */ - -/** - * @defgroup LSM6DS3_Common - * @brief This section groups common usefull functions. - * @{ - * - */ - -/** - * @brief Enable access to the embedded functions.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of func_cfg_en in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_mem_bank_set(stmdev_ctx_t* ctx, lsm6ds3_func_cfg_en_t val) { - lsm6ds3_func_cfg_access_t func_cfg_access; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FUNC_CFG_ACCESS, - (uint8_t*) &func_cfg_access, 1); - - if (ret == 0) { - func_cfg_access.func_cfg_en = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_FUNC_CFG_ACCESS, - (uint8_t*) &func_cfg_access, 1); - } - - return ret; -} - -/** - * @brief Enable access to the embedded functions.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of func_cfg_en in reg FUNC_CFG_ACCESS - * - */ -int32_t lsm6ds3_mem_bank_get(stmdev_ctx_t* ctx, lsm6ds3_func_cfg_en_t* val) { - lsm6ds3_func_cfg_access_t func_cfg_access; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FUNC_CFG_ACCESS, - (uint8_t*) &func_cfg_access, 1); - - switch (func_cfg_access.func_cfg_en) { - case LSM6DS3_USER_BANK: - *val = LSM6DS3_USER_BANK; - break; - - case LSM6DS3_EMBEDDED_FUNC_BANK: - *val = LSM6DS3_EMBEDDED_FUNC_BANK; - break; - - default: - *val = LSM6DS3_USER_BANK; - break; - } - - return ret; -} - -/** - * @brief DeviceWhoamI..[get] - * - * @param ctx read / write interface definitions(ptr) - * @param buff buffer that stores data read - * - */ -int32_t lsm6ds3_device_id_get(stmdev_ctx_t* ctx, uint8_t* buff) { - int32_t ret; - ret = lsm6ds3_read_reg(ctx, LSM6DS3_WHO_AM_I, buff, 1); - return ret; -} - -/** - * @brief Software reset. Restore the default values in user registers.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of sw_reset in reg CTRL3_C - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_reset_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_ctrl3_c_t ctrl3_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t*) &ctrl3_c, 1); - - if (ret == 0) { - ctrl3_c.sw_reset = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t*) &ctrl3_c, 1); - } - - return ret; -} - -/** - * @brief Software reset. Restore the default values in user registers.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of sw_reset in reg CTRL3_C - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_reset_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_ctrl3_c_t ctrl3_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t*) &ctrl3_c, 1); - *val = (uint8_t) ctrl3_c.sw_reset; - - return ret; -} - -/** - * @brief Big/Little Endian Data selection.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of ble in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_data_format_set(stmdev_ctx_t* ctx, lsm6ds3_ble_t val) { - lsm6ds3_ctrl3_c_t ctrl3_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t*) &ctrl3_c, 1); - - if (ret == 0) { - ctrl3_c.ble = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t*) &ctrl3_c, 1); - } - - return ret; -} - -/** - * @brief Big/Little Endian Data selection.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of ble in reg CTRL3_C - * - */ -int32_t lsm6ds3_data_format_get(stmdev_ctx_t* ctx, lsm6ds3_ble_t* val) { - lsm6ds3_ctrl3_c_t ctrl3_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t*) &ctrl3_c, 1); - - switch (ctrl3_c.ble) { - case LSM6DS3_LSB_AT_LOW_ADD: - *val = LSM6DS3_LSB_AT_LOW_ADD; - break; - - case LSM6DS3_MSB_AT_LOW_ADD: - *val = LSM6DS3_MSB_AT_LOW_ADD; - break; - - default: - *val = LSM6DS3_LSB_AT_LOW_ADD; - break; - } - - return ret; -} - -/** - * @brief Register address automatically incremented during a multiple - * byte access with a serial interface.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of if_inc in reg CTRL3_C - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_auto_increment_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_ctrl3_c_t ctrl3_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t*) &ctrl3_c, 1); - - if (ret == 0) { - ctrl3_c.if_inc = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t*) &ctrl3_c, 1); - } - - return ret; -} - -/** - * @brief Register address automatically incremented during a multiple - * byte access with a serial interface.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of if_inc in reg CTRL3_C - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_auto_increment_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_ctrl3_c_t ctrl3_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t*) &ctrl3_c, 1); - *val = (uint8_t) ctrl3_c.if_inc; - - return ret; -} - -/** - * @brief Reboot memory content. Reload the calibration parameters.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of boot in reg CTRL3_C - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_boot_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_ctrl3_c_t ctrl3_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t*) &ctrl3_c, 1); - - if (ret == 0) { - ctrl3_c.boot = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t*) &ctrl3_c, 1); - } - - return ret; -} - -/** - * @brief Reboot memory content. Reload the calibration parameters.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of boot in reg CTRL3_C - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_boot_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_ctrl3_c_t ctrl3_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t*) &ctrl3_c, 1); - *val = (uint8_t) ctrl3_c.boot; - - return ret; -} - -/** - * @brief Linear acceleration sensor self-test enable.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of st_xl in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_xl_self_test_set(stmdev_ctx_t* ctx, lsm6ds3_st_xl_t val) { - lsm6ds3_ctrl5_c_t ctrl5_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL5_C, (uint8_t*) &ctrl5_c, 1); - - if (ret == 0) { - ctrl5_c.st_xl = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL5_C, (uint8_t*) &ctrl5_c, 1); - } - - return ret; -} - -/** - * @brief Linear acceleration sensor self-test enable.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of st_xl in reg CTRL5_C - * - */ -int32_t lsm6ds3_xl_self_test_get(stmdev_ctx_t* ctx, lsm6ds3_st_xl_t* val) { - lsm6ds3_ctrl5_c_t ctrl5_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL5_C, (uint8_t*) &ctrl5_c, 1); - - switch (ctrl5_c.st_xl) { - case LSM6DS3_XL_ST_DISABLE: - *val = LSM6DS3_XL_ST_DISABLE; - break; - - case LSM6DS3_XL_ST_POSITIVE: - *val = LSM6DS3_XL_ST_POSITIVE; - break; - - case LSM6DS3_XL_ST_NEGATIVE: - *val = LSM6DS3_XL_ST_NEGATIVE; - break; - - default: - *val = LSM6DS3_XL_ST_DISABLE; - break; - } - - return ret; -} - -/** - * @brief Angular rate sensor self-test enable.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of st_g in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_gy_self_test_set(stmdev_ctx_t* ctx, lsm6ds3_st_g_t val) { - lsm6ds3_ctrl5_c_t ctrl5_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL5_C, (uint8_t*) &ctrl5_c, 1); - - if (ret == 0) { - ctrl5_c.st_g = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL5_C, (uint8_t*) &ctrl5_c, 1); - } - - return ret; -} - -/** - * @brief Angular rate sensor self-test enable.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of st_g in reg CTRL5_C - * - */ -int32_t lsm6ds3_gy_self_test_get(stmdev_ctx_t* ctx, lsm6ds3_st_g_t* val) { - lsm6ds3_ctrl5_c_t ctrl5_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL5_C, (uint8_t*) &ctrl5_c, 1); - - switch (ctrl5_c.st_g) { - case LSM6DS3_GY_ST_DISABLE: - *val = LSM6DS3_GY_ST_DISABLE; - break; - - case LSM6DS3_GY_ST_POSITIVE: - *val = LSM6DS3_GY_ST_POSITIVE; - break; - - case LSM6DS3_GY_ST_NEGATIVE: - *val = LSM6DS3_GY_ST_NEGATIVE; - break; - - default: - *val = LSM6DS3_GY_ST_DISABLE; - break; - } - - return ret; -} - -/** - * @} - * - */ - -/** - * @defgroup LSM6DS3_Filters - * @brief This section group all the functions concerning the - * filters configuration - * @{ - * - */ - -/** - * @brief Mask DRDY on pin (both XL & Gyro) until filter settling ends - * (XL and Gyro independently masked).[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of drdy_mask in reg CTRL4_C - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_filter_settling_mask_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_ctrl4_c_t ctrl4_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL4_C, (uint8_t*) &ctrl4_c, 1); - - if (ret == 0) { - ctrl4_c.drdy_mask = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL4_C, (uint8_t*) &ctrl4_c, 1); - } - - return ret; -} - -/** - * @brief Mask DRDY on pin (both XL & Gyro) until filter settling ends - * (XL and Gyro independently masked).[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of drdy_mask in reg CTRL4_C - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_filter_settling_mask_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_ctrl4_c_t ctrl4_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL4_C, (uint8_t*) &ctrl4_c, 1); - *val = (uint8_t) ctrl4_c.drdy_mask; - - return ret; -} - -/** - * @brief Gyroscope high-pass filter cutoff frequency selection.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of hpcf_g in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_gy_hp_bandwidth_set(stmdev_ctx_t* ctx, lsm6ds3_hpcf_g_t val) { - lsm6ds3_ctrl7_g_t ctrl7_g; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL7_G, (uint8_t*) &ctrl7_g, 1); - - if (ret == 0) { - ctrl7_g.hpcf_g = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL7_G, (uint8_t*) &ctrl7_g, 1); - } - - return ret; -} - -/** - * @brief Gyroscope high-pass filter cutoff frequency selection.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of hpcf_g in reg CTRL7_G - * - */ -int32_t lsm6ds3_gy_hp_bandwidth_get(stmdev_ctx_t* ctx, lsm6ds3_hpcf_g_t* val) { - lsm6ds3_ctrl7_g_t ctrl7_g; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL7_G, (uint8_t*) &ctrl7_g, 1); - - switch (ctrl7_g.hpcf_g) { - case LSM6DS3_HP_CUT_OFF_8mHz1: - *val = LSM6DS3_HP_CUT_OFF_8mHz1; - break; - - case LSM6DS3_HP_CUT_OFF_32mHz4: - *val = LSM6DS3_HP_CUT_OFF_32mHz4; - break; - - case LSM6DS3_HP_CUT_OFF_2Hz07: - *val = LSM6DS3_HP_CUT_OFF_2Hz07; - break; - - case LSM6DS3_HP_CUT_OFF_16Hz32: - *val = LSM6DS3_HP_CUT_OFF_16Hz32; - break; - - default: - *val = LSM6DS3_HP_CUT_OFF_8mHz1; - break; - } - - return ret; -} - -/** - * @brief Gyro digital HP filter reset.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of hp_g_rst in reg CTRL7_G - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_gy_hp_reset_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_ctrl7_g_t ctrl7_g; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL7_G, (uint8_t*) &ctrl7_g, 1); - - if (ret == 0) { - ctrl7_g.hp_g_rst = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL7_G, (uint8_t*) &ctrl7_g, 1); - } - - return ret; -} - -/** - * @brief Gyro digital HP filter reset.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of hp_g_rst in reg CTRL7_G - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_gy_hp_reset_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_ctrl7_g_t ctrl7_g; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL7_G, (uint8_t*) &ctrl7_g, 1); - *val = (uint8_t) ctrl7_g.hp_g_rst; - - return ret; -} - -/** - * @brief Accelerometer slope filter and high-pass filter configuration - * and cut-off setting.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of hp_slope_xl_en in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_xl_hp_bandwidth_set(stmdev_ctx_t* ctx, lsm6ds3_hp_bw_t val) { - lsm6ds3_ctrl8_xl_t ctrl8_xl; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL8_XL, (uint8_t*) &ctrl8_xl, 1); - - if (ret == 0) { - ctrl8_xl.hp_slope_xl_en = PROPERTY_ENABLE; - ctrl8_xl.hpcf_xl = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL8_XL, (uint8_t*) &ctrl8_xl, 1); - } - - return ret; -} - -/** - * @brief Accelerometer slope filter and high-pass filter configuration - * and cut-off setting.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of hp_slope_xl_en in reg CTRL8_XL - * - */ -int32_t lsm6ds3_xl_hp_bandwidth_get(stmdev_ctx_t* ctx, lsm6ds3_hp_bw_t* val) { - lsm6ds3_ctrl8_xl_t ctrl8_xl; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL8_XL, (uint8_t*) &ctrl8_xl, 1); - - switch (ctrl8_xl.hpcf_xl) { - case LSM6DS3_XL_HP_ODR_DIV_4: - *val = LSM6DS3_XL_HP_ODR_DIV_4; - break; - - case LSM6DS3_XL_HP_ODR_DIV_100: - *val = LSM6DS3_XL_HP_ODR_DIV_100; - break; - - case LSM6DS3_XL_HP_ODR_DIV_9: - *val = LSM6DS3_XL_HP_ODR_DIV_9; - break; - - case LSM6DS3_XL_HP_ODR_DIV_400: - *val = LSM6DS3_XL_HP_ODR_DIV_400; - break; - - default: - *val = LSM6DS3_XL_HP_ODR_DIV_4; - break; - } - - return ret; -} - -/** - * @brief Accelerometer low-pass filter configuration and - * cut-off setting.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of lpf2_xl_en in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_xl_lp2_bandwidth_set(stmdev_ctx_t* ctx, lsm6ds3_lp_bw_t val) { - lsm6ds3_ctrl8_xl_t ctrl8_xl; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL8_XL, (uint8_t*) &ctrl8_xl, 1); - - if (ret == 0) { - ctrl8_xl.lpf2_xl_en = PROPERTY_ENABLE; - ctrl8_xl.hpcf_xl = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL8_XL, (uint8_t*) &ctrl8_xl, 1); - } - - return ret; -} - -/** - * @brief Accelerometer low-pass filter configuration and cut-off - * setting.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of lpf2_xl_en in reg CTRL8_XL - * - */ -int32_t lsm6ds3_xl_lp2_bandwidth_get(stmdev_ctx_t* ctx, lsm6ds3_lp_bw_t* val) { - lsm6ds3_ctrl8_xl_t ctrl8_xl; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL8_XL, (uint8_t*) &ctrl8_xl, 1); - - switch (ctrl8_xl.hpcf_xl) { - case LSM6DS3_XL_LP_ODR_DIV_50: - *val = LSM6DS3_XL_LP_ODR_DIV_50; - break; - - case LSM6DS3_XL_LP_ODR_DIV_100: - *val = LSM6DS3_XL_LP_ODR_DIV_100; - break; - - case LSM6DS3_XL_LP_ODR_DIV_9: - *val = LSM6DS3_XL_LP_ODR_DIV_9; - break; - - case LSM6DS3_XL_LP_ODR_DIV_400: - *val = LSM6DS3_XL_LP_ODR_DIV_400; - break; - - default: - *val = LSM6DS3_XL_LP_ODR_DIV_50; - break; - } - - return ret; -} - -/** - * @brief Anti-aliasing filter bandwidth selection.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of bw_xl in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_xl_filter_analog_set(stmdev_ctx_t* ctx, lsm6ds3_bw_xl_t val) { - lsm6ds3_ctrl1_xl_t ctrl1_xl; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL1_XL, (uint8_t*) &ctrl1_xl, 1); - - if (ret == 0) { - ctrl1_xl.bw_xl = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL1_XL, (uint8_t*) &ctrl1_xl, 1); - } - - return ret; -} - -/** - * @brief Anti-aliasing filter bandwidth selection.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of bw_xl in reg CTRL1_XL - * - */ -int32_t lsm6ds3_xl_filter_analog_get(stmdev_ctx_t* ctx, lsm6ds3_bw_xl_t* val) { - lsm6ds3_ctrl1_xl_t ctrl1_xl; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL1_XL, (uint8_t*) &ctrl1_xl, 1); - - switch (ctrl1_xl.bw_xl) { - case LSM6DS3_ANTI_ALIASING_400Hz: - *val = LSM6DS3_ANTI_ALIASING_400Hz; - break; - - case LSM6DS3_ANTI_ALIASING_200Hz: - *val = LSM6DS3_ANTI_ALIASING_200Hz; - break; - - case LSM6DS3_ANTI_ALIASING_100Hz: - *val = LSM6DS3_ANTI_ALIASING_100Hz; - break; - - case LSM6DS3_ANTI_ALIASING_50Hz: - *val = LSM6DS3_ANTI_ALIASING_50Hz; - break; - - default: - *val = LSM6DS3_ANTI_ALIASING_400Hz; - break; - } - - return ret; -} - -/** - * @} - * - */ - -/** - * @defgroup LSM6DS3_Serial_interface - * @brief This section groups all the functions concerning main - * serial interface management (not auxiliary) - * @{ - * - */ - -/** - * @brief SPI Serial Interface Mode selection.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of sim in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_spi_mode_set(stmdev_ctx_t* ctx, lsm6ds3_sim_t val) { - lsm6ds3_ctrl3_c_t ctrl3_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t*) &ctrl3_c, 1); - - if (ret == 0) { - ctrl3_c.sim = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t*) &ctrl3_c, 1); - } - - return ret; -} - -/** - * @brief SPI Serial Interface Mode selection.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of sim in reg CTRL3_C - * - */ -int32_t lsm6ds3_spi_mode_get(stmdev_ctx_t* ctx, lsm6ds3_sim_t* val) { - lsm6ds3_ctrl3_c_t ctrl3_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t*) &ctrl3_c, 1); - - switch (ctrl3_c.sim) { - case LSM6DS3_SPI_4_WIRE: - *val = LSM6DS3_SPI_4_WIRE; - break; - - case LSM6DS3_SPI_3_WIRE: - *val = LSM6DS3_SPI_3_WIRE; - break; - - default: - *val = LSM6DS3_SPI_4_WIRE; - break; - } - - return ret; -} - -/** - * @brief Disable / Enable I2C interface.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of i2c_disable in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_i2c_interface_set(stmdev_ctx_t* ctx, lsm6ds3_i2c_dis_t val) { - lsm6ds3_ctrl4_c_t ctrl4_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL4_C, (uint8_t*) &ctrl4_c, 1); - - if (ret == 0) { - ctrl4_c.i2c_disable = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL4_C, (uint8_t*) &ctrl4_c, 1); - } - - return ret; -} - -/** - * @brief Disable / Enable I2C interface.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of i2c_disable in reg CTRL4_C - * - */ -int32_t lsm6ds3_i2c_interface_get(stmdev_ctx_t* ctx, lsm6ds3_i2c_dis_t* val) { - lsm6ds3_ctrl4_c_t ctrl4_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL4_C, (uint8_t*) &ctrl4_c, 1); - - switch (ctrl4_c.i2c_disable) { - case LSM6DS3_I2C_ENABLE: - *val = LSM6DS3_I2C_ENABLE; - break; - - case LSM6DS3_I2C_DISABLE: - *val = LSM6DS3_I2C_DISABLE; - break; - - default: - *val = LSM6DS3_I2C_ENABLE; - break; - } - - return ret; -} - -/** - * @} - * - */ - -/** - * @defgroup LSM6DS3_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 Select the signal that need to route on int1 pad. - * - */ -int32_t lsm6ds3_pin_int1_route_set(stmdev_ctx_t* ctx, lsm6ds3_int1_route_t* val) { - lsm6ds3_int1_ctrl_t int1_ctrl; - lsm6ds3_md1_cfg_t md1_cfg; - lsm6ds3_master_config_t master_config; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_MASTER_CONFIG, - (uint8_t*) &master_config, 1); - - if (ret == 0) { - int1_ctrl.int1_drdy_xl = val->int1_drdy_xl; - int1_ctrl.int1_drdy_g = val->int1_drdy_g; - int1_ctrl.int1_boot = val->int1_boot; - int1_ctrl.int1_fth = val->int1_fth; - int1_ctrl.int1_fifo_ovr = val->int1_fifo_ovr; - int1_ctrl.int1_full_flag = val->int1_full_flag; - int1_ctrl.int1_sign_mot = val->int1_sign_mot; - int1_ctrl.int1_step_detector = val->int1_step_detector; - md1_cfg.int1_timer = val->int1_timer; - md1_cfg.int1_tilt = val->int1_tilt; - md1_cfg.int1_6d = val->int1_6d; - md1_cfg.int1_double_tap = val->int1_double_tap; - md1_cfg.int1_ff = val->int1_ff; - md1_cfg.int1_wu = val->int1_wu; - md1_cfg.int1_single_tap = val->int1_single_tap; - md1_cfg.int1_inact_state = val->int1_inact_state; - master_config.drdy_on_int1 = val->drdy_on_int1; - - ret = lsm6ds3_write_reg(ctx, LSM6DS3_INT1_CTRL, (uint8_t*) &int1_ctrl, 1); - - if (ret == 0) { - ret = lsm6ds3_write_reg(ctx, LSM6DS3_MD1_CFG, (uint8_t*) &md1_cfg, 1); - } - - if (ret == 0) { - ret = lsm6ds3_write_reg(ctx, LSM6DS3_MASTER_CONFIG, - (uint8_t*) &master_config, 1); - } - } - - return ret; -} - -/** - * @brief Select the signal that need to route on int1 pad.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val Select the signal that need to route on int1 pad. - * - */ -int32_t lsm6ds3_pin_int1_route_get(stmdev_ctx_t* ctx, lsm6ds3_int1_route_t* val) { - lsm6ds3_int1_ctrl_t int1_ctrl; - lsm6ds3_md1_cfg_t md1_cfg; - lsm6ds3_master_config_t master_config; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_INT1_CTRL, (uint8_t*) &int1_ctrl, 1); - - if (ret == 0) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_MD1_CFG, (uint8_t*) &md1_cfg, 1); - } - - if (ret == 0) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_MASTER_CONFIG, - (uint8_t*) &master_config, 1); - } - - if (ret == 0) { - val->int1_drdy_xl = int1_ctrl.int1_drdy_xl; - val->int1_drdy_g = int1_ctrl.int1_drdy_g; - val->int1_boot = int1_ctrl.int1_boot; - val->int1_fth = int1_ctrl.int1_fth; - val->int1_fifo_ovr = int1_ctrl.int1_fifo_ovr; - val->int1_full_flag = int1_ctrl.int1_full_flag; - val->int1_sign_mot = int1_ctrl.int1_sign_mot; - val->int1_step_detector = int1_ctrl.int1_step_detector; - val->int1_timer = md1_cfg.int1_timer; - val->int1_tilt = md1_cfg.int1_tilt; - val->int1_6d = md1_cfg.int1_6d; - val->int1_double_tap = md1_cfg.int1_double_tap; - val->int1_ff = md1_cfg.int1_ff; - val->int1_wu = md1_cfg.int1_wu; - val->int1_single_tap = md1_cfg.int1_single_tap; - val->int1_inact_state = md1_cfg.int1_inact_state; - val->drdy_on_int1 = master_config.drdy_on_int1; - } - - return ret; -} - -/** - * @brief Select the signal that need to route on int1 pad.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val Select the signal that need to route on int1 pad. - * - */ -int32_t lsm6ds3_pin_int2_route_set(stmdev_ctx_t* ctx, lsm6ds3_int2_route_t* val) { - lsm6ds3_int2_ctrl_t int2_ctrl; - lsm6ds3_md2_cfg_t md2_cfg; - lsm6ds3_master_config_t master_config; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_MASTER_CONFIG, - (uint8_t*) &master_config, 1); - - if (ret == 0) { - int2_ctrl.int2_drdy_xl = val->int2_drdy_xl; - int2_ctrl.int2_drdy_g = val->int2_drdy_g; - int2_ctrl.int2_drdy_temp = val->int2_drdy_temp; - int2_ctrl.int2_fth = val->int2_fth; - int2_ctrl.int2_fifo_ovr = val->int2_fifo_ovr; - int2_ctrl.int2_full_flag = val->int2_full_flag; - int2_ctrl.int2_step_count_ov = val->int2_step_count_ov; - int2_ctrl.int2_step_delta = val->int2_step_delta; - md2_cfg.int2_iron = val->int2_iron; - md2_cfg.int2_tilt = val->int2_tilt; - md2_cfg.int2_6d = val->int2_6d; - md2_cfg.int2_double_tap = val->int2_double_tap; - md2_cfg.int2_ff = val->int2_ff; - md2_cfg.int2_wu = val->int2_wu; - md2_cfg.int2_single_tap = val->int2_single_tap; - md2_cfg.int2_inact_state = val->int2_inact_state; - master_config.start_config = val->start_config; - - ret = lsm6ds3_write_reg(ctx, LSM6DS3_INT2_CTRL, (uint8_t*) &int2_ctrl, 1); - } - - if (ret == 0) { - ret = lsm6ds3_write_reg(ctx, LSM6DS3_MD2_CFG, (uint8_t*) &md2_cfg, 1); - } - - if (ret == 0) { - ret = lsm6ds3_write_reg(ctx, LSM6DS3_MASTER_CONFIG, - (uint8_t*) &master_config, 1); - } - - return ret; -} - -/** - * @brief Select the signal that need to route on int1 pad.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val Select the signal that need to route on int1 pad. - * - */ -int32_t lsm6ds3_pin_int2_route_get(stmdev_ctx_t* ctx, lsm6ds3_int2_route_t* val) { - lsm6ds3_int2_ctrl_t int2_ctrl; - lsm6ds3_md2_cfg_t md2_cfg; - lsm6ds3_master_config_t master_config; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_INT2_CTRL, (uint8_t*) &int2_ctrl, 1); - - if (ret == 0) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_MD2_CFG, (uint8_t*) &md2_cfg, 1); - } - - if (ret == 0) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_MASTER_CONFIG, - (uint8_t*) &master_config, 1); - - val->int2_drdy_xl = int2_ctrl.int2_drdy_xl; - val->int2_drdy_g = int2_ctrl.int2_drdy_g; - val->int2_drdy_temp = int2_ctrl.int2_drdy_temp; - val->int2_fth = int2_ctrl.int2_fth; - val->int2_fifo_ovr = int2_ctrl.int2_fifo_ovr; - val->int2_full_flag = int2_ctrl.int2_full_flag; - val->int2_step_count_ov = int2_ctrl.int2_step_count_ov; - val->int2_step_delta = int2_ctrl.int2_step_delta; - val->int2_iron = md2_cfg.int2_iron; - val->int2_tilt = md2_cfg.int2_tilt; - val->int2_6d = md2_cfg.int2_6d; - val->int2_double_tap = md2_cfg.int2_double_tap; - val->int2_ff = md2_cfg.int2_ff; - val->int2_wu = md2_cfg.int2_wu; - val->int2_single_tap = md2_cfg.int2_single_tap; - val->int2_inact_state = md2_cfg.int2_inact_state; - val->start_config = master_config.start_config; - } - - return ret; -} - -/** - * @brief Push-pull/open drain selection on interrupt pads.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of pp_od in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_pin_mode_set(stmdev_ctx_t* ctx, lsm6ds3_pp_od_t val) { - lsm6ds3_ctrl3_c_t ctrl3_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t*) &ctrl3_c, 1); - - if (ret == 0) { - ctrl3_c.pp_od = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t*) &ctrl3_c, 1); - } - - return ret; -} - -/** - * @brief Push-pull/open drain selection on interrupt pads.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of pp_od in reg CTRL3_C - * - */ -int32_t lsm6ds3_pin_mode_get(stmdev_ctx_t* ctx, lsm6ds3_pp_od_t* val) { - lsm6ds3_ctrl3_c_t ctrl3_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t*) &ctrl3_c, 1); - - switch (ctrl3_c.pp_od) { - case LSM6DS3_PUSH_PULL: - *val = LSM6DS3_PUSH_PULL; - break; - - case LSM6DS3_OPEN_DRAIN: - *val = LSM6DS3_OPEN_DRAIN; - break; - - default: - *val = LSM6DS3_PUSH_PULL; - break; - } - - return ret; -} - -/** - * @brief Interrupt active-high/low.Interrupt active-high/low.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of h_lactive in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_pin_polarity_set(stmdev_ctx_t* ctx, lsm6ds3_pin_pol_t val) { - lsm6ds3_ctrl3_c_t ctrl3_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t*) &ctrl3_c, 1); - - if (ret == 0) { - ctrl3_c.h_lactive = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t*) &ctrl3_c, 1); - } - - return ret; -} - -/** - * @brief Interrupt active-high/low.Interrupt active-high/low.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of h_lactive in reg CTRL3_C - * - */ -int32_t lsm6ds3_pin_polarity_get(stmdev_ctx_t* ctx, lsm6ds3_pin_pol_t* val) { - lsm6ds3_ctrl3_c_t ctrl3_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t*) &ctrl3_c, 1); - - switch (ctrl3_c.h_lactive) { - case LSM6DS3_ACTIVE_HIGH: - *val = LSM6DS3_ACTIVE_HIGH; - break; - - case LSM6DS3_ACTIVE_LOW: - *val = LSM6DS3_ACTIVE_LOW; - break; - - default: - *val = LSM6DS3_ACTIVE_HIGH; - break; - } - - return ret; -} - -/** - * @brief All interrupt signals become available on INT1 pin.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of int2_on_int1 in reg CTRL4_C - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_all_on_int1_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_ctrl4_c_t ctrl4_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL4_C, (uint8_t*) &ctrl4_c, 1); - - if (ret == 0) { - ctrl4_c.int2_on_int1 = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL4_C, (uint8_t*) &ctrl4_c, 1); - } - - return ret; -} - -/** - * @brief All interrupt signals become available on INT1 pin.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of int2_on_int1 in reg CTRL4_C - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_all_on_int1_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_ctrl4_c_t ctrl4_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL4_C, (uint8_t*) &ctrl4_c, 1); - *val = (uint8_t) ctrl4_c.int2_on_int1; - - return ret; -} - -/** - * @brief Latched/pulsed interrupt.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of lir in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_int_notification_set(stmdev_ctx_t* ctx, lsm6ds3_lir_t val) { - lsm6ds3_tap_cfg_t tap_cfg; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t*) &tap_cfg, 1); - - if (ret == 0) { - tap_cfg.lir = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t*) &tap_cfg, 1); - } - - return ret; -} - -/** - * @brief Latched/pulsed interrupt.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of lir in reg TAP_CFG - * - */ -int32_t lsm6ds3_int_notification_get(stmdev_ctx_t* ctx, lsm6ds3_lir_t* val) { - lsm6ds3_tap_cfg_t tap_cfg; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t*) &tap_cfg, 1); - - switch (tap_cfg.lir) { - case LSM6DS3_INT_PULSED: - *val = LSM6DS3_INT_PULSED; - break; - - case LSM6DS3_INT_LATCHED: - *val = LSM6DS3_INT_LATCHED; - break; - - default: - *val = LSM6DS3_INT_PULSED; - break; - } - - return ret; -} - -/** - * @} - * - */ - -/** - * @defgroup LSM6DS3_Wake_Up_event - * @brief This section groups all the functions that manage the - * Wake Up event generation. - * @{ - * - */ - -/** - * @brief Read the wake_up_src status flag of the device.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val Read the wake_up_src status flag of the device. - * - */ -int32_t lsm6ds3_wkup_src_get(stmdev_ctx_t* ctx, lsm6ds3_wake_up_src_t* val) { - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_WAKE_UP_SRC, (uint8_t*) val, 1); - - return ret; -} - -/** - * @brief Threshold for wakeup (1 LSB = FS_XL / 64).[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of wk_ths in reg WAKE_UP_THS - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_wkup_threshold_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_wake_up_ths_t wake_up_ths; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_WAKE_UP_THS, (uint8_t*) &wake_up_ths, 1); - - if (ret == 0) { - wake_up_ths.wk_ths = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_WAKE_UP_THS, - (uint8_t*) &wake_up_ths, 1); - } - - return ret; -} - -/** - * @brief Threshold for wakeup (1 LSB = FS_XL / 64).[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of wk_ths in reg WAKE_UP_THS - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_wkup_threshold_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_wake_up_ths_t wake_up_ths; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_WAKE_UP_THS, (uint8_t*) &wake_up_ths, 1); - *val = (uint8_t) wake_up_ths.wk_ths; - - return ret; -} - -/** - * @brief Wake up duration event.1LSb = 1 / ODR[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of wake_dur in reg WAKE_UP_DUR - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_wkup_dur_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_wake_up_dur_t wake_up_dur; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_WAKE_UP_DUR, (uint8_t*) &wake_up_dur, 1); - - if (ret == 0) { - wake_up_dur.wake_dur = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_WAKE_UP_DUR, - (uint8_t*) &wake_up_dur, 1); - } - - return ret; -} - -/** - * @brief Wake up duration event.1LSb = 1 / ODR[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of wake_dur in reg WAKE_UP_DUR - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_wkup_dur_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_wake_up_dur_t wake_up_dur; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_WAKE_UP_DUR, (uint8_t*) &wake_up_dur, 1); - *val = (uint8_t) wake_up_dur.wake_dur; - - return ret; -} - -/** - * @} - * - */ - -/** - * @defgroup LSM6DS3_Activity/Inactivity_detection - * @brief This section groups all the functions concerning - * activity/inactivity detection. - * @{ - * - */ - -/** - * @brief Enables gyroscope Sleep mode.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of sleep_g in reg CTRL4_C - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_gy_sleep_mode_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_ctrl4_c_t ctrl4_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL4_C, (uint8_t*) &ctrl4_c, 1); - - if (ret == 0) { - ctrl4_c.sleep_g = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL4_C, (uint8_t*) &ctrl4_c, 1); - } - - return ret; -} - -/** - * @brief Enables gyroscope Sleep mode.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of sleep_g in reg CTRL4_C - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_gy_sleep_mode_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_ctrl4_c_t ctrl4_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL4_C, (uint8_t*) &ctrl4_c, 1); - *val = (uint8_t) ctrl4_c.sleep_g; - - return ret; -} - -/** - * @brief Enable/Disable inactivity function.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of inactivity in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_act_mode_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_wake_up_ths_t wake_up_ths; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_WAKE_UP_THS, (uint8_t*) &wake_up_ths, 1); - - if (ret == 0) { - wake_up_ths.inactivity = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_WAKE_UP_THS, - (uint8_t*) &wake_up_ths, 1); - } - - return ret; -} - -/** - * @brief Enable/Disable inactivity function.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of inactivity in reg WAKE_UP_THS - * - */ -int32_t lsm6ds3_act_mode_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_wake_up_ths_t wake_up_ths; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_WAKE_UP_THS, (uint8_t*) &wake_up_ths, 1); - *val = wake_up_ths.inactivity; - - return ret; -} - -/** - * @brief Duration to go in sleep mode. 1 LSb = 512 / ODR[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of sleep_dur in reg WAKE_UP_DUR - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_act_sleep_dur_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_wake_up_dur_t wake_up_dur; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_WAKE_UP_DUR, (uint8_t*) &wake_up_dur, 1); - - if (ret == 0) { - wake_up_dur.sleep_dur = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_WAKE_UP_DUR, - (uint8_t*) &wake_up_dur, 1); - } - - return ret; -} - -/** - * @brief Duration to go in sleep mode. 1 LSb = 512 / ODR[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of sleep_dur in reg WAKE_UP_DUR - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_act_sleep_dur_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_wake_up_dur_t wake_up_dur; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_WAKE_UP_DUR, (uint8_t*) &wake_up_dur, 1); - *val = (uint8_t) wake_up_dur.sleep_dur; - - return ret; -} - -/** - * @} - * - */ - -/** - * @defgroup LSM6DS3_Tap_generator - * @brief This section groups all the functions that manage the - * tap and double tap event generation. - * @{ - * - */ - -/** - * @brief Read the tap_src status flag of the device.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val Read the tap_src status flag of the device. - * - */ -int32_t lsm6ds3_tap_src_get(stmdev_ctx_t* ctx, lsm6ds3_tap_src_t* val) { - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_SRC, (uint8_t*) val, 1); - - return ret; -} - -/** - * @brief Enable Z direction in tap recognition.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of tap_z_en in reg TAP_CFG - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_tap_detection_on_z_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_tap_cfg_t tap_cfg; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t*) &tap_cfg, 1); - - if (ret == 0) { - tap_cfg.tap_z_en = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t*) &tap_cfg, 1); - } - - return ret; -} - -/** - * @brief Enable Z direction in tap recognition.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of tap_z_en in reg TAP_CFG - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_tap_detection_on_z_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_tap_cfg_t tap_cfg; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t*) &tap_cfg, 1); - *val = (uint8_t) tap_cfg.tap_z_en; - - return ret; -} - -/** - * @brief Enable Y direction in tap recognition.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of tap_y_en in reg TAP_CFG - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_tap_detection_on_y_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_tap_cfg_t tap_cfg; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t*) &tap_cfg, 1); - - if (ret == 0) { - tap_cfg.tap_y_en = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t*) &tap_cfg, 1); - } - - return ret; -} - -/** - * @brief Enable Y direction in tap recognition.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of tap_y_en in reg TAP_CFG - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_tap_detection_on_y_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_tap_cfg_t tap_cfg; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t*) &tap_cfg, 1); - *val = (uint8_t) tap_cfg.tap_y_en; - - return ret; -} - -/** - * @brief Enable X direction in tap recognition.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of tap_x_en in reg TAP_CFG - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_tap_detection_on_x_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_tap_cfg_t tap_cfg; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t*) &tap_cfg, 1); - - if (ret == 0) { - tap_cfg.tap_x_en = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t*) &tap_cfg, 1); - } - - return ret; -} - -/** - * @brief Enable X direction in tap recognition.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of tap_x_en in reg TAP_CFG - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_tap_detection_on_x_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_tap_cfg_t tap_cfg; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t*) &tap_cfg, 1); - *val = (uint8_t) tap_cfg.tap_x_en; - - return ret; -} - -/** - * @brief Threshold for tap recognition.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of tap_ths in reg TAP_THS_6D - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_tap_threshold_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_tap_ths_6d_t tap_ths_6d; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_THS_6D, (uint8_t*) &tap_ths_6d, 1); - - if (ret == 0) { - tap_ths_6d.tap_ths = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_TAP_THS_6D, (uint8_t*) &tap_ths_6d, 1); - } - - return ret; -} - -/** - * @brief Threshold for tap recognition.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of tap_ths in reg TAP_THS_6D - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_tap_threshold_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_tap_ths_6d_t tap_ths_6d; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_THS_6D, (uint8_t*) &tap_ths_6d, 1); - *val = (uint8_t) tap_ths_6d.tap_ths; - - return ret; -} - -/** - * @brief 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[1:0] bits are set to a different value, - * 1LSB corresponds to 8*ODR_XL time.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of shock in reg INT_DUR2 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_tap_shock_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_int_dur2_t int_dur2; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_INT_DUR2, (uint8_t*) &int_dur2, 1); - - if (ret == 0) { - int_dur2.shock = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_INT_DUR2, (uint8_t*) &int_dur2, 1); - } - - return ret; -} - -/** - * @brief 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[1:0] bits are set to a different - * value, 1LSB corresponds to 8*ODR_XL time.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of shock in reg INT_DUR2 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_tap_shock_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_int_dur2_t int_dur2; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_INT_DUR2, (uint8_t*) &int_dur2, 1); - *val = (uint8_t) int_dur2.shock; - - return ret; -} - -/** - * @brief Quiet time is the time after the first detected tap in - * which there must not be any over-threshold event. - * The default value of these bits is 00b which corresponds - * to 2*ODR_XL time. If the QUIET[1:0] bits are set to a - * different value, 1LSB corresponds to 4*ODR_XL time.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of quiet in reg INT_DUR2 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_tap_quiet_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_int_dur2_t int_dur2; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_INT_DUR2, (uint8_t*) &int_dur2, 1); - - if (ret == 0) { - int_dur2.quiet = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_INT_DUR2, (uint8_t*) &int_dur2, 1); - } - - return ret; -} - -/** - * @brief Quiet time is the time after the first detected tap in which - * there must not be any over-threshold event. The default value - * of these bits is 00b which corresponds to 2*ODR_XL time. - * If the QUIET[1:0] bits are set to a different value, - * 1LSB corresponds to 4*ODR_XL time.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of quiet in reg INT_DUR2 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_tap_quiet_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_int_dur2_t int_dur2; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_INT_DUR2, (uint8_t*) &int_dur2, 1); - *val = (uint8_t) int_dur2.quiet; - - return ret; -} - -/** - * @brief 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(ptr) - * @param val change the values of dur in reg INT_DUR2 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_tap_dur_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_int_dur2_t int_dur2; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_INT_DUR2, (uint8_t*) &int_dur2, 1); - - if (ret == 0) { - int_dur2.dur = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_INT_DUR2, (uint8_t*) &int_dur2, 1); - } - - return ret; -} - -/** - * @brief 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(ptr) - * @param val get the values of dur in reg INT_DUR2 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_tap_dur_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_int_dur2_t int_dur2; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_INT_DUR2, (uint8_t*) &int_dur2, 1); - *val = (uint8_t) int_dur2.dur; - - return ret; -} - -/** - * @brief Single/double-tap event enable.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of single_double_tap in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_tap_mode_set(stmdev_ctx_t* ctx, lsm6ds3_tap_md_t val) { - lsm6ds3_wake_up_ths_t wake_up_ths; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_WAKE_UP_THS, (uint8_t*) &wake_up_ths, 1); - - if (ret == 0) { - wake_up_ths.single_double_tap = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_WAKE_UP_THS, - (uint8_t*) &wake_up_ths, 1); - } - - return ret; -} - -/** - * @brief Single/double-tap event enable.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of single_double_tap in reg WAKE_UP_THS - * - */ -int32_t lsm6ds3_tap_mode_get(stmdev_ctx_t* ctx, lsm6ds3_tap_md_t* val) { - lsm6ds3_wake_up_ths_t wake_up_ths; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_WAKE_UP_THS, (uint8_t*) &wake_up_ths, 1); - - switch (wake_up_ths.single_double_tap) { - case LSM6DS3_ONLY_DOUBLE: - *val = LSM6DS3_ONLY_DOUBLE; - break; - - case LSM6DS3_SINGLE_DOUBLE: - *val = LSM6DS3_SINGLE_DOUBLE; - break; - - default: - *val = LSM6DS3_ONLY_DOUBLE; - break; - } - - return ret; -} - -/** - * @} - * - */ - -/** - * @defgroup LSM6DS3_Six_position_detection(6D/4D) - * @brief This section groups all the functions concerning - * six position detection (6D). - * @{ - * - */ - -/** - * @brief LPF2 feed 6D function selection.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of low_pass_on_6d in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_6d_feed_data_set(stmdev_ctx_t* ctx, lsm6ds3_low_pass_on_6d_t val) { - lsm6ds3_ctrl8_xl_t ctrl8_xl; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL8_XL, (uint8_t*) &ctrl8_xl, 1); - - if (ret == 0) { - ctrl8_xl.low_pass_on_6d = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL8_XL, (uint8_t*) &ctrl8_xl, 1); - } - - return ret; -} - -/** - * @brief LPF2 feed 6D function selection.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of low_pass_on_6d in reg CTRL8_XL - * - */ -int32_t lsm6ds3_6d_feed_data_get(stmdev_ctx_t* ctx, lsm6ds3_low_pass_on_6d_t* val) { - lsm6ds3_ctrl8_xl_t ctrl8_xl; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL8_XL, (uint8_t*) &ctrl8_xl, 1); - - switch (ctrl8_xl.low_pass_on_6d) { - case LSM6DS3_ODR_DIV_2_FEED: - *val = LSM6DS3_ODR_DIV_2_FEED; - break; - - case LSM6DS3_LPF2_FEED: - *val = LSM6DS3_LPF2_FEED; - break; - - default: - *val = LSM6DS3_ODR_DIV_2_FEED; - break; - } - - return ret; -} - -/** - * @brief Read the d6d_src status flag of the device.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val Read the d6d_src status flag of the device. - * - */ -int32_t lsm6ds3_6d_src_get(stmdev_ctx_t* ctx, lsm6ds3_d6d_src_t* val) { - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_D6D_SRC, (uint8_t*) val, 1); - - return ret; -} - -/** - * @brief Threshold for 4D/6D function.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of sixd_ths in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_6d_threshold_set(stmdev_ctx_t* ctx, lsm6ds3_sixd_ths_t val) { - lsm6ds3_tap_ths_6d_t tap_ths_6d; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_THS_6D, (uint8_t*) &tap_ths_6d, 1); - - if (ret == 0) { - tap_ths_6d.sixd_ths = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_TAP_THS_6D, - (uint8_t*) &tap_ths_6d, 1); - } - - return ret; -} - -/** - * @brief Threshold for 4D/6D function.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of sixd_ths in reg TAP_THS_6D - * - */ -int32_t lsm6ds3_6d_threshold_get(stmdev_ctx_t* ctx, lsm6ds3_sixd_ths_t* val) { - lsm6ds3_tap_ths_6d_t tap_ths_6d; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_THS_6D, (uint8_t*) &tap_ths_6d, 1); - - switch (tap_ths_6d.sixd_ths) { - case LSM6DS3_DEG_80: - *val = LSM6DS3_DEG_80; - break; - - case LSM6DS3_DEG_70: - *val = LSM6DS3_DEG_70; - break; - - case LSM6DS3_DEG_60: - *val = LSM6DS3_DEG_60; - break; - - case LSM6DS3_DEG_50: - *val = LSM6DS3_DEG_50; - break; - - default: - *val = LSM6DS3_DEG_80; - break; - } - - return ret; -} - -/** - * @brief 4D orientation detection enable.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of d4d_en in reg TAP_THS_6D - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_4d_mode_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_tap_ths_6d_t tap_ths_6d; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_THS_6D, (uint8_t*) &tap_ths_6d, 1); - - if (ret == 0) { - tap_ths_6d.d4d_en = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_TAP_THS_6D, - (uint8_t*) &tap_ths_6d, 1); - } - - return ret; -} - -/** - * @brief 4D orientation detection enable.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of d4d_en in reg TAP_THS_6D - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_4d_mode_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_tap_ths_6d_t tap_ths_6d; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_THS_6D, (uint8_t*) &tap_ths_6d, 1); - *val = (uint8_t) tap_ths_6d.d4d_en; - - return ret; -} - -/** - * @} - * - */ - -/** - * @defgroup LSM6DS3_Free_fall - * @brief This section group all the functions concerning the - * free fall detection. - * @{ - * - */ - -/** - * @brief Free fall threshold setting.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of ff_ths in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_ff_threshold_set(stmdev_ctx_t* ctx, lsm6ds3_ff_ths_t val) { - lsm6ds3_free_fall_t free_fall; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FREE_FALL, (uint8_t*) &free_fall, 1); - - if (ret == 0) { - free_fall.ff_ths = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_FREE_FALL, (uint8_t*) &free_fall, 1); - } - - return ret; -} - -/** - * @brief Free fall threshold setting.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of ff_ths in reg FREE_FALL - * - */ -int32_t lsm6ds3_ff_threshold_get(stmdev_ctx_t* ctx, lsm6ds3_ff_ths_t* val) { - lsm6ds3_free_fall_t free_fall; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FREE_FALL, (uint8_t*) &free_fall, 1); - - switch (free_fall.ff_ths) { - case LSM6DS3_156_mg: - *val = LSM6DS3_156_mg; - break; - - case LSM6DS3_219_mg: - *val = LSM6DS3_219_mg; - break; - - case LSM6DS3_250_mg: - *val = LSM6DS3_250_mg; - break; - - case LSM6DS3_312_mg: - *val = LSM6DS3_312_mg; - break; - - case LSM6DS3_344_mg: - *val = LSM6DS3_344_mg; - break; - - case LSM6DS3_406_mg: - *val = LSM6DS3_406_mg; - break; - - case LSM6DS3_469_mg: - *val = LSM6DS3_469_mg; - break; - - case LSM6DS3_500_mg: - *val = LSM6DS3_500_mg; - break; - - default: - *val = LSM6DS3_156_mg; - break; - } - - return ret; -} - -/** - * @brief Free-fall duration event. 1LSb = 1 / ODR[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of ff_dur in reg FREE_FALL - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_ff_dur_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_free_fall_t free_fall; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FREE_FALL, (uint8_t*) &free_fall, 1); - - if (ret == 0) { - free_fall.ff_dur = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_FREE_FALL, (uint8_t*) &free_fall, 1); - } - - return ret; -} - -/** - * @brief Free-fall duration event. 1LSb = 1 / ODR[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of ff_dur in reg FREE_FALL - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_ff_dur_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_free_fall_t free_fall; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FREE_FALL, (uint8_t*) &free_fall, 1); - *val = (uint8_t) free_fall.ff_dur; - - return ret; -} - -/** - * @} - * - */ - -/** - * @defgroup LSM6DS3_Fifo - * @brief This section group all the functions concerning the - * fifo usage - * @{ - * - */ - -/** - * @brief FIFO watermark level selection.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of fth in reg FIFO_CTRL1 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_fifo_watermark_set(stmdev_ctx_t* ctx, uint16_t val) { - lsm6ds3_fifo_ctrl1_t fifo_ctrl1; - lsm6ds3_fifo_ctrl2_t fifo_ctrl2; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL1, (uint8_t*) &fifo_ctrl1, 1); - - if (ret == 0) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL2, (uint8_t*) &fifo_ctrl2, 1); - } - - if (ret == 0) { - fifo_ctrl2.fth = (uint8_t) ((val & 0x0F00U) >> 8); - ret = lsm6ds3_write_reg(ctx, LSM6DS3_FIFO_CTRL2, (uint8_t*) &fifo_ctrl2, 1); - } - - if (ret == 0) { - fifo_ctrl1.fth = (uint8_t) (val & 0x00FF0U); - ret = lsm6ds3_write_reg(ctx, LSM6DS3_FIFO_CTRL1, (uint8_t*) &fifo_ctrl1, 1); - } - - return ret; -} - -/** - * @brief FIFO watermark level selection.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of fth in reg FIFO_CTRL1 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_fifo_watermark_get(stmdev_ctx_t* ctx, uint16_t* val) { - lsm6ds3_fifo_ctrl1_t fifo_ctrl1; - lsm6ds3_fifo_ctrl2_t fifo_ctrl2; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL1, (uint8_t*) &fifo_ctrl1, 1); - - if (ret == 0) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL2, (uint8_t*) &fifo_ctrl2, 1); - *val = (uint16_t) fifo_ctrl2.fth << 8; - *val |= fifo_ctrl1.fth; - } - - return ret; -} - -/** - * @brief trigger signal for FIFO write operation.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of timer_pedo_fifo_drdy in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_fifo_write_trigger_set(stmdev_ctx_t* ctx, lsm6ds3_tmr_ped_fifo_drdy_t val) { - lsm6ds3_fifo_ctrl2_t fifo_ctrl2; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL2, (uint8_t*) &fifo_ctrl2, 1); - - if (ret == 0) { - fifo_ctrl2.timer_pedo_fifo_drdy = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_FIFO_CTRL2, (uint8_t*) &fifo_ctrl2, 1); - } - - return ret; -} - -/** - * @brief trigger signal for FIFO write operation.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of timer_pedo_fifo_drdy in - * reg FIFO_CTRL2 - * - */ -int32_t lsm6ds3_fifo_write_trigger_get(stmdev_ctx_t* ctx, lsm6ds3_tmr_ped_fifo_drdy_t* val) { - lsm6ds3_fifo_ctrl2_t fifo_ctrl2; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL2, (uint8_t*) &fifo_ctrl2, 1); - - switch (fifo_ctrl2.timer_pedo_fifo_drdy) { - case LSM6DS3_TRG_XL_GY_DRDY: - *val = LSM6DS3_TRG_XL_GY_DRDY; - break; - - case LSM6DS3_TRG_STEP_DETECT: - *val = LSM6DS3_TRG_STEP_DETECT; - break; - - default: - *val = LSM6DS3_TRG_XL_GY_DRDY; - break; - } - - return ret; -} - -/** - * @brief Pedometer step counter and timestamp as 4th FIFO data set.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of timer_pedo_fifo_en in reg FIFO_CTRL2 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_fifo_pedo_batch_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_fifo_ctrl2_t fifo_ctrl2; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL2, (uint8_t*) &fifo_ctrl2, 1); - - if (ret == 0) { - fifo_ctrl2.timer_pedo_fifo_en = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_FIFO_CTRL2, - (uint8_t*) &fifo_ctrl2, 1); - } - - return ret; -} - -/** - * @brief Pedometer step counter and timestamp as 4th FIFO data set.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of timer_pedo_fifo_en in reg FIFO_CTRL2 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_fifo_pedo_batch_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_fifo_ctrl2_t fifo_ctrl2; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL2, (uint8_t*) &fifo_ctrl2, 1); - *val = (uint8_t) fifo_ctrl2.timer_pedo_fifo_en; - - return ret; -} - -/** - * @brief Selects Batching Data Rate (writing frequency in FIFO) for - * accelerometer data.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of dec_fifo_xl in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_fifo_xl_batch_set(stmdev_ctx_t* ctx, lsm6ds3_dec_fifo_xl_t val) { - lsm6ds3_fifo_ctrl3_t fifo_ctrl3; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL3, (uint8_t*) &fifo_ctrl3, 1); - - if (ret == 0) { - fifo_ctrl3.dec_fifo_xl = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_FIFO_CTRL3, - (uint8_t*) &fifo_ctrl3, 1); - } - - return ret; -} - -/** - * @brief Selects Batching Data Rate (writing frequency in FIFO) for - * accelerometer data.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of dec_fifo_xl in reg FIFO_CTRL3 - * - */ -int32_t lsm6ds3_fifo_xl_batch_get(stmdev_ctx_t* ctx, lsm6ds3_dec_fifo_xl_t* val) { - lsm6ds3_fifo_ctrl3_t fifo_ctrl3; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL3, (uint8_t*) &fifo_ctrl3, 1); - - switch (fifo_ctrl3.dec_fifo_xl) { - case LSM6DS3_FIFO_XL_DISABLE: - *val = LSM6DS3_FIFO_XL_DISABLE; - break; - - case LSM6DS3_FIFO_XL_NO_DEC: - *val = LSM6DS3_FIFO_XL_NO_DEC; - break; - - case LSM6DS3_FIFO_XL_DEC_2: - *val = LSM6DS3_FIFO_XL_DEC_2; - break; - - case LSM6DS3_FIFO_XL_DEC_3: - *val = LSM6DS3_FIFO_XL_DEC_3; - break; - - case LSM6DS3_FIFO_XL_DEC_4: - *val = LSM6DS3_FIFO_XL_DEC_4; - break; - - case LSM6DS3_FIFO_XL_DEC_8: - *val = LSM6DS3_FIFO_XL_DEC_8; - break; - - case LSM6DS3_FIFO_XL_DEC_16: - *val = LSM6DS3_FIFO_XL_DEC_16; - break; - - case LSM6DS3_FIFO_XL_DEC_32: - *val = LSM6DS3_FIFO_XL_DEC_32; - break; - - default: - *val = LSM6DS3_FIFO_XL_DISABLE; - break; - } - - return ret; -} - -/** - * @brief Selects Batching Data Rate (writing frequency in FIFO) - * for gyroscope data.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of dec_fifo_gyro in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_fifo_gy_batch_set(stmdev_ctx_t* ctx, lsm6ds3_dec_fifo_gyro_t val) { - lsm6ds3_fifo_ctrl3_t fifo_ctrl3; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL3, (uint8_t*) &fifo_ctrl3, 1); - - if (ret == 0) { - fifo_ctrl3.dec_fifo_gyro = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_FIFO_CTRL3, - (uint8_t*) &fifo_ctrl3, 1); - } - - return ret; -} - -/** - * @brief Selects Batching Data Rate (writing frequency in FIFO) - * for gyroscope data.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of dec_fifo_gyro in reg FIFO_CTRL3 - * - */ -int32_t lsm6ds3_fifo_gy_batch_get(stmdev_ctx_t* ctx, lsm6ds3_dec_fifo_gyro_t* val) { - lsm6ds3_fifo_ctrl3_t fifo_ctrl3; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL3, (uint8_t*) &fifo_ctrl3, 1); - - switch (fifo_ctrl3.dec_fifo_gyro) { - case LSM6DS3_FIFO_GY_DISABLE: - *val = LSM6DS3_FIFO_GY_DISABLE; - break; - - case LSM6DS3_FIFO_GY_NO_DEC: - *val = LSM6DS3_FIFO_GY_NO_DEC; - break; - - case LSM6DS3_FIFO_GY_DEC_2: - *val = LSM6DS3_FIFO_GY_DEC_2; - break; - - case LSM6DS3_FIFO_GY_DEC_3: - *val = LSM6DS3_FIFO_GY_DEC_3; - break; - - case LSM6DS3_FIFO_GY_DEC_4: - *val = LSM6DS3_FIFO_GY_DEC_4; - break; - - case LSM6DS3_FIFO_GY_DEC_8: - *val = LSM6DS3_FIFO_GY_DEC_8; - break; - - case LSM6DS3_FIFO_GY_DEC_16: - *val = LSM6DS3_FIFO_GY_DEC_16; - break; - - case LSM6DS3_FIFO_GY_DEC_32: - *val = LSM6DS3_FIFO_GY_DEC_32; - break; - - default: - *val = LSM6DS3_FIFO_GY_DISABLE; - break; - } - - return ret; -} - -/** - * @brief Selects Batching Data Rate (writing frequency in FIFO) - * for third data set.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of dec_ds3_fifo in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_fifo_dataset_3_batch_set(stmdev_ctx_t* ctx, lsm6ds3_dec_ds3_fifo_t val) { - lsm6ds3_fifo_ctrl4_t fifo_ctrl4; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL4, (uint8_t*) &fifo_ctrl4, 1); - - if (ret == 0) { - fifo_ctrl4.dec_ds3_fifo = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_FIFO_CTRL4, - (uint8_t*) &fifo_ctrl4, 1); - } - - return ret; -} - -/** - * @brief Selects Batching Data Rate (writing frequency in FIFO) - * for third data set.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of dec_ds3_fifo in reg FIFO_CTRL4 - * - */ -int32_t lsm6ds3_fifo_dataset_3_batch_get(stmdev_ctx_t* ctx, lsm6ds3_dec_ds3_fifo_t* val) { - lsm6ds3_fifo_ctrl4_t fifo_ctrl4; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL4, (uint8_t*) &fifo_ctrl4, 1); - - switch (fifo_ctrl4.dec_ds3_fifo) { - case LSM6DS3_FIFO_DS3_DISABLE: - *val = LSM6DS3_FIFO_DS3_DISABLE; - break; - - case LSM6DS3_FIFO_DS3_NO_DEC: - *val = LSM6DS3_FIFO_DS3_NO_DEC; - break; - - case LSM6DS3_FIFO_DS3_DEC_2: - *val = LSM6DS3_FIFO_DS3_DEC_2; - break; - - case LSM6DS3_FIFO_DS3_DEC_3: - *val = LSM6DS3_FIFO_DS3_DEC_3; - break; - - case LSM6DS3_FIFO_DS3_DEC_4: - *val = LSM6DS3_FIFO_DS3_DEC_4; - break; - - case LSM6DS3_FIFO_DS3_DEC_8: - *val = LSM6DS3_FIFO_DS3_DEC_8; - break; - - case LSM6DS3_FIFO_DS3_DEC_16: - *val = LSM6DS3_FIFO_DS3_DEC_16; - break; - - case LSM6DS3_FIFO_DS3_DEC_32: - *val = LSM6DS3_FIFO_DS3_DEC_32; - break; - - default: - *val = LSM6DS3_FIFO_DS3_DISABLE; - break; - } - - return ret; -} - -/** - * @brief Selects Batching Data Rate (writing frequency in FIFO) - * for fourth data set.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of dec_ds4_fifo in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_fifo_dataset_4_batch_set(stmdev_ctx_t* ctx, lsm6ds3_dec_ds4_fifo_t val) { - lsm6ds3_fifo_ctrl4_t fifo_ctrl4; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL4, (uint8_t*) &fifo_ctrl4, 1); - - if (ret == 0) { - fifo_ctrl4.dec_ds4_fifo = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_FIFO_CTRL4, - (uint8_t*) &fifo_ctrl4, 1); - } - - return ret; -} - -/** - * @brief Selects Batching Data Rate (writing frequency in FIFO) - * for fourth data set.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of dec_ds4_fifo in reg FIFO_CTRL4 - * - */ -int32_t lsm6ds3_fifo_dataset_4_batch_get(stmdev_ctx_t* ctx, lsm6ds3_dec_ds4_fifo_t* val) { - lsm6ds3_fifo_ctrl4_t fifo_ctrl4; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL4, (uint8_t*) &fifo_ctrl4, 1); - - switch (fifo_ctrl4.dec_ds4_fifo) { - case LSM6DS3_FIFO_DS4_DISABLE: - *val = LSM6DS3_FIFO_DS4_DISABLE; - break; - - case LSM6DS3_FIFO_DS4_NO_DEC: - *val = LSM6DS3_FIFO_DS4_NO_DEC; - break; - - case LSM6DS3_FIFO_DS4_DEC_2: - *val = LSM6DS3_FIFO_DS4_DEC_2; - break; - - case LSM6DS3_FIFO_DS4_DEC_3: - *val = LSM6DS3_FIFO_DS4_DEC_3; - break; - - case LSM6DS3_FIFO_DS4_DEC_4: - *val = LSM6DS3_FIFO_DS4_DEC_4; - break; - - case LSM6DS3_FIFO_DS4_DEC_8: - *val = LSM6DS3_FIFO_DS4_DEC_8; - break; - - case LSM6DS3_FIFO_DS4_DEC_16: - *val = LSM6DS3_FIFO_DS4_DEC_16; - break; - - case LSM6DS3_FIFO_DS4_DEC_32: - *val = LSM6DS3_FIFO_DS4_DEC_32; - break; - - default: - *val = LSM6DS3_FIFO_DS4_DISABLE; - break; - } - - return ret; -} - -/** - * @brief 8-bit data storage in FIFO.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of only_high_data in reg FIFO_CTRL4 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_fifo_xl_gy_8bit_format_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_fifo_ctrl4_t fifo_ctrl4; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL4, (uint8_t*) &fifo_ctrl4, 1); - - if (ret == 0) { - fifo_ctrl4.only_high_data = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_FIFO_CTRL4, - (uint8_t*) &fifo_ctrl4, 1); - } - - return ret; -} - -/** - * @brief 8-bit data storage in FIFO.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of only_high_data in reg FIFO_CTRL4 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_fifo_xl_gy_8bit_format_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_fifo_ctrl4_t fifo_ctrl4; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL4, (uint8_t*) &fifo_ctrl4, 1); - *val = (uint8_t) fifo_ctrl4.only_high_data; - - return ret; -} - -/** - * @brief FIFO mode selection.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of fifo_mode in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_fifo_mode_set(stmdev_ctx_t* ctx, lsm6ds3_fifo_md_t val) { - lsm6ds3_fifo_ctrl5_t fifo_ctrl5; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL5, (uint8_t*) &fifo_ctrl5, 1); - - if (ret == 0) { - fifo_ctrl5.fifo_mode = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_FIFO_CTRL5, - (uint8_t*) &fifo_ctrl5, 1); - } - - return ret; -} - -/** - * @brief FIFO mode selection.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of fifo_mode in reg FIFO_CTRL5 - * - */ -int32_t lsm6ds3_fifo_mode_get(stmdev_ctx_t* ctx, lsm6ds3_fifo_md_t* val) { - lsm6ds3_fifo_ctrl5_t fifo_ctrl5; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL5, (uint8_t*) &fifo_ctrl5, 1); - - switch (fifo_ctrl5.fifo_mode) { - case LSM6DS3_BYPASS_MODE: - *val = LSM6DS3_BYPASS_MODE; - break; - - case LSM6DS3_FIFO_MODE: - *val = LSM6DS3_FIFO_MODE; - break; - - case LSM6DS3_STREAM_TO_FIFO_MODE: - *val = LSM6DS3_STREAM_TO_FIFO_MODE; - break; - - case LSM6DS3_BYPASS_TO_STREAM_MODE: - *val = LSM6DS3_BYPASS_TO_STREAM_MODE; - break; - - default: - *val = LSM6DS3_BYPASS_MODE; - break; - } - - return ret; -} - -/** - * @brief FIFO ODR selection, setting FIFO_MODE also.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of odr_fifo in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_fifo_data_rate_set(stmdev_ctx_t* ctx, lsm6ds3_odr_fifo_t val) { - lsm6ds3_fifo_ctrl5_t fifo_ctrl5; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL5, (uint8_t*) &fifo_ctrl5, 1); - - if (ret == 0) { - fifo_ctrl5.odr_fifo = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_FIFO_CTRL5, - (uint8_t*) &fifo_ctrl5, 1); - } - - return ret; -} - -/** - * @brief FIFO ODR selection, setting FIFO_MODE also.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of odr_fifo in reg FIFO_CTRL5 - * - */ -int32_t lsm6ds3_fifo_data_rate_get(stmdev_ctx_t* ctx, lsm6ds3_odr_fifo_t* val) { - lsm6ds3_fifo_ctrl5_t fifo_ctrl5; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL5, (uint8_t*) &fifo_ctrl5, 1); - - switch (fifo_ctrl5.odr_fifo) { - case LSM6DS3_FIFO_DISABLE: - *val = LSM6DS3_FIFO_DISABLE; - break; - - case LSM6DS3_FIFO_12Hz5: - *val = LSM6DS3_FIFO_12Hz5; - break; - - case LSM6DS3_FIFO_26Hz: - *val = LSM6DS3_FIFO_26Hz; - break; - - case LSM6DS3_FIFO_52Hz: - *val = LSM6DS3_FIFO_52Hz; - break; - - case LSM6DS3_FIFO_104Hz: - *val = LSM6DS3_FIFO_104Hz; - break; - - case LSM6DS3_FIFO_208Hz: - *val = LSM6DS3_FIFO_208Hz; - break; - - case LSM6DS3_FIFO_416Hz: - *val = LSM6DS3_FIFO_416Hz; - break; - - case LSM6DS3_FIFO_833Hz: - *val = LSM6DS3_FIFO_833Hz; - break; - - case LSM6DS3_FIFO_1k66Hz: - *val = LSM6DS3_FIFO_1k66Hz; - break; - - case LSM6DS3_FIFO_3k33Hz: - *val = LSM6DS3_FIFO_3k33Hz; - break; - - case LSM6DS3_FIFO_6k66Hz: - *val = LSM6DS3_FIFO_6k66Hz; - break; - - default: - *val = LSM6DS3_FIFO_DISABLE; - break; - } - - return ret; -} - -/** - * @brief Sensing chain FIFO stop values memorization at - * threshold level.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of stop_on_fth in reg CTRL4_C - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_fifo_stop_on_wtm_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_ctrl4_c_t ctrl4_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL4_C, (uint8_t*) &ctrl4_c, 1); - - if (ret == 0) { - ctrl4_c.stop_on_fth = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL4_C, (uint8_t*) &ctrl4_c, 1); - } - - return ret; -} - -/** - * @brief Sensing chain FIFO stop values memorization at - * threshold level.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of stop_on_fth in reg CTRL4_C - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_fifo_stop_on_wtm_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_ctrl4_c_t ctrl4_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL4_C, (uint8_t*) &ctrl4_c, 1); - *val = (uint8_t) ctrl4_c.stop_on_fth; - - return ret; -} - -/** - * @brief batching of temperature data.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of fifo_temp_en in reg CTRL4_C - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_fifo_temp_batch_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_ctrl4_c_t ctrl4_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL4_C, (uint8_t*) &ctrl4_c, 1); - - if (ret == 0) { - ctrl4_c.fifo_temp_en = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL4_C, (uint8_t*) &ctrl4_c, 1); - } - - return ret; -} - -/** - * @brief batching of temperature data.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of fifo_temp_en in reg CTRL4_C - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_fifo_temp_batch_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_ctrl4_c_t ctrl4_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL4_C, (uint8_t*) &ctrl4_c, 1); - *val = (uint8_t) ctrl4_c.fifo_temp_en; - - return ret; -} - -/** - * @brief Number of unread words (16-bit axes) stored in FIFO.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of diff_fifo in reg FIFO_STATUS1 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_fifo_data_level_get(stmdev_ctx_t* ctx, uint16_t* val) { - lsm6ds3_fifo_status1_t fifo_status1; - lsm6ds3_fifo_status2_t fifo_status2; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_STATUS1, - (uint8_t*) &fifo_status1, 1); - - if (ret == 0) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_STATUS2, - (uint8_t*) &fifo_status2, 1); - - *val = (uint16_t) fifo_status2.diff_fifo << 8; - *val |= fifo_status1.diff_fifo; - } - - return ret; -} - -/** - * @brief Smart FIFO full status.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of fifo_empty in reg FIFO_STATUS2 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_fifo_full_flag_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_fifo_status2_t fifo_status2; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_STATUS2, - (uint8_t*) &fifo_status2, 1); - *val = (uint8_t) fifo_status2.fifo_empty; - - return ret; -} - -/** - * @brief FIFO overrun status.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of fifo_full in reg FIFO_STATUS2 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_fifo_ovr_flag_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_fifo_status2_t fifo_status2; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_STATUS2, - (uint8_t*) &fifo_status2, 1); - *val = (uint8_t) fifo_status2.fifo_full; - - return ret; -} - -/** - * @brief FIFO watermark status.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of fth in reg FIFO_STATUS2 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_fifo_wtm_flag_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_fifo_status2_t fifo_status2; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_STATUS2, - (uint8_t*) &fifo_status2, 1); - *val = (uint8_t) fifo_status2.fth; - - return ret; -} - -/** - * @brief Word of recursive pattern read at the next reading.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of fifo_pattern in reg FIFO_STATUS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_fifo_pattern_get(stmdev_ctx_t* ctx, uint16_t* val) { - lsm6ds3_fifo_status3_t fifo_status3; - lsm6ds3_fifo_status4_t fifo_status4; - - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_STATUS3, - (uint8_t*) &fifo_status3, 1); - - if (ret == 0) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_STATUS4, - (uint8_t*) &fifo_status4, 1); - - *val = (uint16_t) fifo_status4.fifo_pattern << 8; - *val |= fifo_status3.fifo_pattern; - } - - return ret; -} - -/** - * @} - * - */ - -/** - * @defgroup LSM6DS3_DEN_functionality - * @brief This section groups all the functions concerning DEN - * functionality. - * @{ - * - */ - -/** - * @brief DEN functionality marking mode.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of den_mode in reg CTRL6_C - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_den_mode_set(stmdev_ctx_t* ctx, lsm6ds3_den_mode_t val) { - lsm6ds3_ctrl6_c_t ctrl6_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL6_C, (uint8_t*) &ctrl6_c, 1); - - if (ret == 0) { - ctrl6_c.den_mode = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL6_C, (uint8_t*) &ctrl6_c, 1); - } - - return ret; -} - -/** - * @brief DEN functionality marking mode.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of den_mode in reg CTRL6_C - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_den_mode_get(stmdev_ctx_t* ctx, lsm6ds3_den_mode_t* val) { - lsm6ds3_ctrl6_c_t ctrl6_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL6_C, (uint8_t*) &ctrl6_c, 1); - - switch (ctrl6_c.den_mode) { - case LSM6DS3_DEN_DISABLE: - *val = LSM6DS3_DEN_DISABLE; - break; - - case LSM6DS3_LEVEL_FIFO: - *val = LSM6DS3_LEVEL_FIFO; - break; - - case LSM6DS3_LEVEL_LETCHED: - *val = LSM6DS3_LEVEL_LETCHED; - break; - - case LSM6DS3_LEVEL_TRIGGER: - *val = LSM6DS3_LEVEL_TRIGGER; - break; - - case LSM6DS3_EDGE_TRIGGER: - *val = LSM6DS3_EDGE_TRIGGER; - break; - - default: - *val = LSM6DS3_DEN_DISABLE; - break; - } - - return ret; -} - -/** - * @} - * - */ - -/** - * @defgroup LSM6DS3_Pedometer - * @brief This section groups all the functions that manage pedometer. - * @{ - * - */ - -/** - * @brief Reset pedometer step counter.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of pedo_rst_step in reg CTRL10_C - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_pedo_step_reset_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_ctrl10_c_t ctrl10_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t*) &ctrl10_c, 1); - - if (ret == 0) { - ctrl10_c.pedo_rst_step = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t*) &ctrl10_c, 1); - } - - return ret; -} - -/** - * @brief Reset pedometer step counter.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of pedo_rst_step in reg CTRL10_C - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_pedo_step_reset_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_ctrl10_c_t ctrl10_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t*) &ctrl10_c, 1); - *val = (uint8_t) ctrl10_c.pedo_rst_step; - - return ret; -} - -/** - * @brief Step counter timestamp information register (r). When a step is - * detected, the value of TIMESTAMP_REG register is copied in - * STEP_TIMESTAMP_L..[get] - * - * @param ctx read / write interface definitions(ptr) - * @param buff buffer that stores data read - * - */ -int32_t lsm6ds3_pedo_timestamp_raw_get(stmdev_ctx_t* ctx, uint8_t* buff) { - int32_t ret; - ret = lsm6ds3_read_reg(ctx, LSM6DS3_STEP_TIMESTAMP_L, buff, 2); - return ret; -} - -/** - * @brief Step detector event detection status - * (0:not detected / 1:detected).[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of step_detected in reg FUNC_SRC - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_pedo_step_detect_flag_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_func_src_t func_src; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FUNC_SRC, (uint8_t*) &func_src, 1); - *val = (uint8_t) func_src.step_detected; - - return ret; -} - -/** - * @brief Enable pedometer algorithm.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of pedo_en in reg TAP_CFG - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_pedo_sens_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_ctrl10_c_t ctrl10_c; - lsm6ds3_tap_cfg_t tap_cfg; - int32_t ret; - - ret = 0; - - if (val == PROPERTY_ENABLE) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t*) &ctrl10_c, 1); - - if (ret == 0) { - ctrl10_c.func_en = PROPERTY_ENABLE; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t*) &ctrl10_c, 1); - } - } - - if (ret == 0) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t*) &tap_cfg, 1); - } - - if (ret == 0) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t*) &tap_cfg, 1); - } - - if (ret == 0) { - tap_cfg.pedo_en = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t*) &tap_cfg, 1); - } - - return ret; -} - -/** - * @brief Enable pedometer algorithm.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of pedo_en in reg TAP_CFG - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_pedo_sens_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_tap_cfg_t tap_cfg; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t*) &tap_cfg, 1); - *val = (uint8_t) tap_cfg.pedo_en; - - return ret; -} - -/** - * @brief Configurable minimum threshold (PEDO_4G 1LSB = 16 mg , - * PEDO_2G 1LSB = 32 mg).[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of ths_min in reg PEDO_THS_REG - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_pedo_threshold_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_pedo_ths_reg_t pedo_ths_reg; - int32_t ret; - - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); - - if (ret == 0) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_PEDO_THS_REG, - (uint8_t*) &pedo_ths_reg, 1); - } - - if (ret == 0) { - pedo_ths_reg.ths_min = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_PEDO_THS_REG, - (uint8_t*) &pedo_ths_reg, 1); - } - - if (ret == 0) { - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); - } - - return ret; -} - -/** - * @brief Configurable minimum threshold (PEDO_4G 1LSB = 16 mg, - * PEDO_2G 1LSB = 32 mg).[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of ths_min in reg PEDO_THS_REG - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_pedo_threshold_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_pedo_ths_reg_t pedo_ths_reg; - int32_t ret; - - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); - - if (ret == 0) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_PEDO_THS_REG, - (uint8_t*) &pedo_ths_reg, 1); - } - - if (ret == 0) { - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); - *val = (uint8_t) pedo_ths_reg.ths_min; - } - - return ret; -} - -/** - * @brief This bit sets the internal full scale used in - * pedometer functions.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of pedo_4g in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_pedo_full_scale_set(stmdev_ctx_t* ctx, lsm6ds3_pedo_fs_t val) { - lsm6ds3_pedo_ths_reg_t pedo_ths_reg; - int32_t ret; - - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); - - if (ret == 0) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_PEDO_THS_REG, - (uint8_t*) &pedo_ths_reg, 1); - } - - if (ret == 0) { - pedo_ths_reg.pedo_4g = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_PEDO_THS_REG, - (uint8_t*) &pedo_ths_reg, 1); - } - - if (ret == 0) { - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); - } - - return ret; -} - -/** - * @brief This bit sets the internal full scale used in pedometer - * functions.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of pedo_4g in reg PEDO_THS_REG - * - */ -int32_t lsm6ds3_pedo_full_scale_get(stmdev_ctx_t* ctx, lsm6ds3_pedo_fs_t* val) { - lsm6ds3_pedo_ths_reg_t pedo_ths_reg; - int32_t ret; - - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); - - if (ret == 0) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_PEDO_THS_REG, - (uint8_t*) &pedo_ths_reg, 1); - - switch (pedo_ths_reg.pedo_4g) { - case LSM6DS3_PEDO_AT_2g: - *val = LSM6DS3_PEDO_AT_2g; - break; - - case LSM6DS3_PEDO_AT_4g: - *val = LSM6DS3_PEDO_AT_4g; - break; - - default: - *val = LSM6DS3_PEDO_AT_2g; - break; - } - } - - if (ret == 0) { - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); - } - - return ret; -} - -/** - * @brief Pedometer debounce configuration register (r/w).[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of deb_step in reg PEDO_DEB_REG - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_pedo_debounce_steps_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_pedo_deb_reg_t pedo_deb_reg; - int32_t ret; - - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); - - if (ret == 0) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_PEDO_DEB_REG, - (uint8_t*) &pedo_deb_reg, 1); - } - - if (ret == 0) { - pedo_deb_reg.deb_step = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_PEDO_DEB_REG, - (uint8_t*) &pedo_deb_reg, 1); - } - - if (ret == 0) { - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); - } - - return ret; -} - -/** - * @brief Pedometer debounce configuration register (r/w).[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of deb_step in reg PEDO_DEB_REG - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_pedo_debounce_steps_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_pedo_deb_reg_t pedo_deb_reg; - int32_t ret; - - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); - - if (ret == 0) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_PEDO_DEB_REG, - (uint8_t*) &pedo_deb_reg, 1); - } - - if (ret == 0) { - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); - *val = (uint8_t) pedo_deb_reg.deb_step; - } - - return ret; -} - -/** - * @brief Debounce time. If the time between two consecutive steps is - * greater than DEB_TIME*80ms, the debounce is reactivated.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of deb_time in reg PEDO_DEB_REG - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_pedo_timeout_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_pedo_deb_reg_t pedo_deb_reg; - int32_t ret; - - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); - - if (ret == 0) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_PEDO_DEB_REG, - (uint8_t*) &pedo_deb_reg, 1); - } - - if (ret == 0) { - pedo_deb_reg.deb_time = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_PEDO_DEB_REG, - (uint8_t*) &pedo_deb_reg, 1); - } - - if (ret == 0) { - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); - } - - return ret; -} - -/** - * @brief Debounce time. If the time between two consecutive steps is - * greater than DEB_TIME*80ms, the debounce is reactivated.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of deb_time in reg PEDO_DEB_REG - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_pedo_timeout_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_pedo_deb_reg_t pedo_deb_reg; - int32_t ret; - - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); - - if (ret == 0) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_PEDO_DEB_REG, - (uint8_t*) &pedo_deb_reg, 1); - } - - if (ret == 0) { - *val = (uint8_t) pedo_deb_reg.deb_time; - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); - } - - return ret; -} - -/** - * @} - * - */ - -/** - * @defgroup LSM6DS3_Significant_motion - * @brief This section groups all the functions that manage the - * significant motion detection. - * @{ - * - */ - -/** - * @brief Enable significant motion detection function.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of sign_motion_en in reg CTRL10_C - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_motion_sens_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_ctrl10_c_t ctrl10_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t*) &ctrl10_c, 1); - - if (ret == 0) { - ctrl10_c.sign_motion_en = (uint8_t) val; - - if (val == PROPERTY_ENABLE) { - ctrl10_c.func_en = PROPERTY_ENABLE; - } - - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t*) &ctrl10_c, 1); - } - - return ret; -} - -/** - * @brief Enable significant motion detection function.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of sign_motion_en in reg CTRL10_C - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_motion_sens_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_ctrl10_c_t ctrl10_c; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t*) &ctrl10_c, 1); - *val = (uint8_t) ctrl10_c.sign_motion_en; - - return ret; -} - -/** - * @brief Significant motion event detection status - * (0:not detected / 1:detected).[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of sign_motion_ia in reg FUNC_SRC - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_motion_event_flag_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_func_src_t func_src; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FUNC_SRC, (uint8_t*) &func_src, 1); - *val = (uint8_t) func_src.sign_motion_ia; - - return ret; -} - -/** - * @brief Significant motion threshold.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of sm_ths in reg SM_THS - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_motion_threshold_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_sm_ths_t sm_ths; - int32_t ret; - - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); - - if (ret == 0) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_SM_THS, (uint8_t*) &sm_ths, 1); - } - - if (ret == 0) { - sm_ths.sm_ths = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_SM_THS, (uint8_t*) &sm_ths, 1); - } - - if (ret == 0) { - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); - } - - return ret; -} - -/** - * @brief Significant motion threshold.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of sm_ths in reg SM_THS - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_motion_threshold_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_sm_ths_t sm_ths; - int32_t ret; - - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); - - if (ret == 0) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_SM_THS, (uint8_t*) &sm_ths, 1); - } - - if (ret == 0) { - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); - *val = (uint8_t) sm_ths.sm_ths; - } - - return ret; -} - -/** - * @brief Time period register for step detection on delta time - * (1LSB = 1.6384 s).[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of sc_delta in reg STEP_COUNT_DELTA - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_sc_delta_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_step_count_delta_t step_count_delta; - int32_t ret; - - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); - - if (ret == 0) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_STEP_COUNT_DELTA, - (uint8_t*) &step_count_delta, 1); - } - - if (ret == 0) { - step_count_delta.sc_delta = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_STEP_COUNT_DELTA, - (uint8_t*) &step_count_delta, 1); - } - - if (ret == 0) { - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); - } - - return ret; -} - -/** - * @brief Time period register for step detection on delta time - * (1LSB = 1.6384 s).[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of sc_delta in reg STEP_COUNT_DELTA - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_sc_delta_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_step_count_delta_t step_count_delta; - int32_t ret; - - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); - - if (ret == 0) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_STEP_COUNT_DELTA, - (uint8_t*) &step_count_delta, 1); - } - - if (ret == 0) { - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); - *val = (uint8_t) step_count_delta.sc_delta; - } - - return ret; -} - -/** - * @} - * - */ - -/** - * @defgroup LSM6DS3_Tilt_detection - * @brief This section groups all the functions that manage - * the tilt event detection. - * @{ - * - */ - -/** - * @brief Tilt event detection status(0:not detected / 1:detected).[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of tilt_ia in reg FUNC_SRC - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_tilt_event_flag_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_func_src_t func_src; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FUNC_SRC, (uint8_t*) &func_src, 1); - *val = (uint8_t) func_src.tilt_ia; - - return ret; -} - -/** - * @brief Enable tilt calculation.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of tilt_en in reg TAP_CFG - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_tilt_sens_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_ctrl10_c_t ctrl10_c; - lsm6ds3_tap_cfg_t tap_cfg; - int32_t ret; - - ret = 0; - - if (val == PROPERTY_ENABLE) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t*) &ctrl10_c, 1); - - if (ret == 0) { - ctrl10_c.func_en = PROPERTY_ENABLE; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t*) &ctrl10_c, 1); - } - } - - if (ret == 0) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t*) &tap_cfg, 1); - } - - if (ret == 0) { - tap_cfg.tilt_en = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t*) &tap_cfg, 1); - } - - return ret; -} - -/** - * @brief Enable tilt calculation.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of tilt_en in reg TAP_CFG - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_tilt_sens_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_tap_cfg_t tap_cfg; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t*) &tap_cfg, 1); - *val = (uint8_t) tap_cfg.tilt_en; - - return ret; -} - -/** - * @} - * - */ - -/** - * @defgroup LSM6DS3_Magnetometer_sensor - * @brief This section groups all the functions that manage - * additional magnetometer sensor. - * @{ - * - */ - -/** - * @brief Enable soft-iron correction algorithm for magnetometer.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of soft_en in reg CTRL9_XL - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_mag_soft_iron_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_ctrl10_c_t ctrl10_c; - lsm6ds3_ctrl9_xl_t ctrl9_xl; - int32_t ret; - - ret = 0; - - if (val == PROPERTY_ENABLE) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t*) &ctrl10_c, 1); - - if (ret == 0) { - ctrl10_c.func_en = PROPERTY_ENABLE; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t*) &ctrl10_c, 1); - } - } - - if (ret == 0) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL9_XL, (uint8_t*) &ctrl9_xl, 1); - } - - if (ret == 0) { - ctrl9_xl.soft_en = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL9_XL, (uint8_t*) &ctrl9_xl, 1); - } - - return ret; -} - -/** - * @brief Enable soft-iron correction algorithm for magnetometer.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of soft_en in reg CTRL9_XL - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_mag_soft_iron_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_ctrl9_xl_t ctrl9_xl; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL9_XL, (uint8_t*) &ctrl9_xl, 1); - *val = (uint8_t) ctrl9_xl.soft_en; - - return ret; -} - -/** - * @brief Enable hard-iron correction algorithm for magnetometer.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of iron_en in reg MASTER_CONFIG - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_mag_hard_iron_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_ctrl10_c_t ctrl10_c; - lsm6ds3_master_config_t master_config; - int32_t ret; - - ret = 0; - - if (val == PROPERTY_ENABLE) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t*) &ctrl10_c, 1); - ctrl10_c.func_en = PROPERTY_ENABLE; - - if (ret == 0) { - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t*) &ctrl10_c, 1); - } - } - - if (ret == 0) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_MASTER_CONFIG, - (uint8_t*) &master_config, 1); - } - - if (ret == 0) { - master_config.iron_en = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_MASTER_CONFIG, - (uint8_t*) &master_config, 1); - } - - return ret; -} - -/** - * @brief Enable hard-iron correction algorithm for magnetometer.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of iron_en in reg MASTER_CONFIG - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_mag_hard_iron_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_master_config_t master_config; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_MASTER_CONFIG, - (uint8_t*) &master_config, 1); - *val = (uint8_t) master_config.iron_en; - - return ret; -} - -/** - * @brief Hard/soft-iron calculation status (0: on-going / 1: idle).[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of si_end_op in reg FUNC_SRC - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_mag_soft_iron_end_op_flag_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_func_src_t func_src; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FUNC_SRC, (uint8_t*) &func_src, 1); - *val = (uint8_t) func_src.si_end_op; - - return ret; -} - -/** - * @brief Soft-iron matrix correction registers[set] - * - * @param ctx read / write interface definitions(ptr) - * @param buff buffer that stores data to be write - * - */ -int32_t lsm6ds3_mag_soft_iron_coeff_set(stmdev_ctx_t* ctx, uint8_t* buff) { - int32_t ret; - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); - - if (ret == 0) { - ret = lsm6ds3_write_reg(ctx, LSM6DS3_MAG_SI_XX, buff, 9); - } - - if (ret == 0) { - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); - } - - return ret; -} - -/** - * @brief Soft-iron matrix correction registers[get] - * - * @param ctx read / write interface definitions(ptr) - * @param buff buffer that stores data read - * - */ -int32_t lsm6ds3_mag_soft_iron_coeff_get(stmdev_ctx_t* ctx, uint8_t* buff) { - int32_t ret; - - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); - - if (ret == 0) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_MAG_SI_XX, buff, 9); - } - - if (ret == 0) { - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); - } - - return ret; -} - -/** - * @brief Offset for hard-iron compensation register (r/w). - * The value is expressed as a 16-bit word in two’s complement.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param buff buffer that stores data to be write - * - */ -int32_t lsm6ds3_mag_offset_set(stmdev_ctx_t* ctx, uint8_t* buff) { - int32_t ret; - - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); - - if (ret == 0) { - ret = lsm6ds3_write_reg(ctx, LSM6DS3_MAG_OFFX_L, buff, 6); - } - - if (ret == 0) { - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); - } - - return ret; -} - -/** - * @brief Offset for hard-iron compensation register(r/w). - * The value is expressed as a 16-bit word in two’s complement.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param buff buffer that stores data read - * - */ -int32_t lsm6ds3_mag_offset_get(stmdev_ctx_t* ctx, uint8_t* buff) { - int32_t ret; - - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); - - if (ret == 0) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_MAG_OFFX_L, buff, 6); - } - - if (ret == 0) { - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); - } - - return ret; -} - -/** - * @} - * - */ - -/** - * @defgroup LSM6DS3_Sensor_hub - * @brief This section groups all the functions that manage the - * sensor hub functionality. - * @{ - * - */ - -/** - * @brief Sensor synchronization time frame with the step of 500 ms - * and full range of 5 s. Unsigned 8-bit.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of tph in reg SENSOR_SYNC_TIME_FRAME - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_sh_sync_sens_frame_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_sensor_sync_time_frame_t sensor_sync_time_frame; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_SENSOR_SYNC_TIME_FRAME, - (uint8_t*) &sensor_sync_time_frame, 1); - - if (ret == 0) { - sensor_sync_time_frame.tph = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_SENSOR_SYNC_TIME_FRAME, - (uint8_t*) &sensor_sync_time_frame, 1); - } - - return ret; -} - -/** - * @brief Sensor synchronization time frame with the step of 500 ms and - * full range of 5 s. Unsigned 8-bit.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of tph in reg SENSOR_SYNC_TIME_FRAME - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_sh_sync_sens_frame_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_sensor_sync_time_frame_t sensor_sync_time_frame; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_SENSOR_SYNC_TIME_FRAME, - (uint8_t*) &sensor_sync_time_frame, 1); - *val = (uint8_t) sensor_sync_time_frame.tph; - - return ret; -} - -/** - * @brief Sensor hub I2C master enable.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of master_on in reg MASTER_CONFIG - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_sh_master_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_ctrl10_c_t ctrl10_c; - lsm6ds3_master_config_t master_config; - int32_t ret; - - ret = 0; - - if (val == PROPERTY_ENABLE) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t*) &ctrl10_c, 1); - - if (ret == 0) { - ctrl10_c.func_en = PROPERTY_ENABLE; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t*) &ctrl10_c, 1); - } - } - - if (ret == 0) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_MASTER_CONFIG, - (uint8_t*) &master_config, 1); - } - - if (ret == 0) { - master_config.master_on = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_MASTER_CONFIG, - (uint8_t*) &master_config, 1); - } - - return ret; -} - -/** - * @brief Sensor hub I2C master enable.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of master_on in reg MASTER_CONFIG - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_sh_master_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_master_config_t master_config; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_MASTER_CONFIG, - (uint8_t*) &master_config, 1); - *val = (uint8_t) master_config.master_on; - - return ret; -} - -/** - * @brief I2C interface pass-through.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of pass_through_mode in - * reg MASTER_CONFIG - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_sh_pass_through_set(stmdev_ctx_t* ctx, uint8_t val) { - lsm6ds3_master_config_t master_config; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_MASTER_CONFIG, - (uint8_t*) &master_config, 1); - - if (ret == 0) { - master_config.pass_through_mode = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_MASTER_CONFIG, - (uint8_t*) &master_config, 1); - } - - return ret; -} - -/** - * @brief I2C interface pass-through.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of pass_through_mode in reg MASTER_CONFIG - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_sh_pass_through_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_master_config_t master_config; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_MASTER_CONFIG, - (uint8_t*) &master_config, 1); - *val = (uint8_t) master_config.pass_through_mode; - - return ret; -} - -/** - * @brief Master I2C pull-up enable/disable.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of pull_up_en in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_sh_pin_mode_set(stmdev_ctx_t* ctx, lsm6ds3_sh_pin_md_t val) { - lsm6ds3_master_config_t master_config; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_MASTER_CONFIG, - (uint8_t*) &master_config, 1); - - if (ret == 0) { - master_config.pull_up_en = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_MASTER_CONFIG, - (uint8_t*) &master_config, 1); - } - - return ret; -} - -/** - * @brief Master I2C pull-up enable/disable.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of pull_up_en in reg MASTER_CONFIG - * - */ -int32_t lsm6ds3_sh_pin_mode_get(stmdev_ctx_t* ctx, lsm6ds3_sh_pin_md_t* val) { - lsm6ds3_master_config_t master_config; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_MASTER_CONFIG, - (uint8_t*) &master_config, 1); - - switch (master_config.pull_up_en) { - case LSM6DS3_EXT_PULL_UP: - *val = LSM6DS3_EXT_PULL_UP; - break; - - case LSM6DS3_INTERNAL_PULL_UP: - *val = LSM6DS3_INTERNAL_PULL_UP; - break; - - default: - *val = LSM6DS3_EXT_PULL_UP; - break; - } - - return ret; -} - -/** - * @brief Sensor hub trigger signal selection.[set] - * - * @param ctx read / write interface definitions(ptr) - * @param val change the values of start_config in reg LSM6DS3 - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_sh_syncro_mode_set(stmdev_ctx_t* ctx, lsm6ds3_start_cfg_t val) { - lsm6ds3_master_config_t master_config; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_MASTER_CONFIG, - (uint8_t*) &master_config, 1); - - if (ret == 0) { - master_config.start_config = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_MASTER_CONFIG, - (uint8_t*) &master_config, 1); - } - - return ret; -} - -/** - * @brief Sensor hub trigger signal selection.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of start_config in reg MASTER_CONFIG - * - */ -int32_t lsm6ds3_sh_syncro_mode_get(stmdev_ctx_t* ctx, lsm6ds3_start_cfg_t* val) { - lsm6ds3_master_config_t master_config; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_MASTER_CONFIG, - (uint8_t*) &master_config, 1); - - switch (master_config.start_config) { - case LSM6DS3_XL_GY_DRDY: - *val = LSM6DS3_XL_GY_DRDY; - break; - - case LSM6DS3_EXT_ON_INT2_PIN: - *val = LSM6DS3_EXT_ON_INT2_PIN; - break; - - default: - *val = LSM6DS3_XL_GY_DRDY; - break; - } - - return ret; -} - -/** - * @brief Sensor hub output registers.[get] - * - * @param ctx read / write interface definitions(ptr) - * @param buff buffer that stores data read - * - */ -int32_t lsm6ds3_sh_read_data_raw_get(stmdev_ctx_t* ctx, lsm6ds3_sh_read_t* buff) { - int32_t ret; - ret = lsm6ds3_read_reg(ctx, LSM6DS3_SENSORHUB1_REG, - (uint8_t*) &(buff->sh_byte_1), 12); - - if (ret == 0) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_SENSORHUB13_REG, - (uint8_t*) &(buff->sh_byte_13), 6); - } - - return ret; -} - -/** - * @brief sh_cfg_write: Configure slave 0 for perform a write. - * - * @param stmdev_ctx_t *ctx: read / write interface definitions - * @param lsm6ds3_sh_cfg_write_t: 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 - * - */ -int32_t lsm6ds3_sh_cfg_write(stmdev_ctx_t* ctx, lsm6ds3_sh_cfg_write_t* val) { - lsm6ds3_slv0_add_t slv0_add; - int32_t ret; - - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); - - if (ret == 0) { - slv0_add.slave0_add = val->slv0_add >> 1; - slv0_add.rw_0 = 0; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_SLV0_ADD, (uint8_t*) &slv0_add, 1); - } - - if (ret == 0) { - ret = lsm6ds3_write_reg(ctx, LSM6DS3_SLV0_SUBADD, - &(val->slv0_subadd), 1); - } - - if (ret == 0) { - ret = lsm6ds3_write_reg(ctx, LSM6DS3_DATAWRITE_SRC_MODE_SUB_SLV0, - &(val->slv0_data), 1); - } - - if (ret == 0) { - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); - } - - return ret; -} - -/** - * @brief sh_slv0_cfg_read: [get] Configure slave 0 for perform a write/read. - * - * @param stmdev_ctx_t *ctx: read / write interface definitions - * @param lsm6ds3_sh_cfg_read_t: 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_len; num of bit to read - * - */ -int32_t lsm6ds3_sh_slv0_cfg_read(stmdev_ctx_t* ctx, lsm6ds3_sh_cfg_read_t* val) { - lsm6ds3_slv0_add_t slv0_add; - lsm6ds3_slave0_config_t slave0_config; - int32_t ret; - - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); - - if (ret == 0) { - slv0_add.slave0_add = val->slv_add >> 1; - slv0_add.rw_0 = 1; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_SLV0_ADD, (uint8_t*) &slv0_add, 1); - } - - if (ret == 0) { - ret = lsm6ds3_write_reg(ctx, LSM6DS3_SLV0_SUBADD, - &(val->slv_subadd), 1); - } - - if (ret == 0) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_SLAVE0_CONFIG, - (uint8_t*) &slave0_config, 1); - } - - if (ret == 0) { - slave0_config.slave0_numop = val->slv_len; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_SLAVE0_CONFIG, - (uint8_t*) &slave0_config, 1); - } - - if (ret == 0) { - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); - } - - return ret; -} - -/** - * @brief sh_slv1_cfg_read: [get] Configure slave 0 for perform a write/read. - * - * @param stmdev_ctx_t *ctx: read / write interface definitions - * @param lsm6ds3_sh_cfg_read_t: 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_len; num of bit to read - * - */ -int32_t lsm6ds3_sh_slv1_cfg_read(stmdev_ctx_t* ctx, lsm6ds3_sh_cfg_read_t* val) { - lsm6ds3_slv1_add_t slv1_add; - lsm6ds3_slave1_config_t slave1_config; - int32_t ret; - - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); - - if (ret == 0) { - slv1_add.slave1_add = val->slv_add >> 1; - ; - slv1_add.r_1 = 1; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_SLV1_ADD, (uint8_t*) &slv1_add, 1); - } - - if (ret == 0) { - ret = lsm6ds3_write_reg(ctx, LSM6DS3_SLV1_SUBADD, - &(val->slv_subadd), 1); - } - - if (ret == 0) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_SLAVE1_CONFIG, (uint8_t*) &slave1_config, 1); - } - - if (ret == 0) { - slave1_config.slave1_numop = val->slv_len; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_SLAVE1_CONFIG, (uint8_t*) &slave1_config, 1); - } - - if (ret == 0) { - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); - } - - return ret; -} - -/** - * @brief sh_slv2_cfg_read: [get] Configure slave 0 for perform a write/read. - * - * @param stmdev_ctx_t *ctx: read / write interface definitions - * @param lsm6ds3_sh_cfg_read_t: a 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 - * - */ -int32_t lsm6ds3_sh_slv2_cfg_read(stmdev_ctx_t* ctx, lsm6ds3_sh_cfg_read_t* val) { - lsm6ds3_slv2_add_t slv2_add; - lsm6ds3_slave2_config_t slave2_config; - int32_t ret; - - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); - - if (ret == 0) { - slv2_add.slave2_add = val->slv_add >> 1; - slv2_add.r_2 = 1; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_SLV2_ADD, - (uint8_t*) &slv2_add, 1); - } - - if (ret == 0) { - ret = lsm6ds3_write_reg(ctx, LSM6DS3_SLV2_SUBADD, - &(val->slv_subadd), 1); - } - - if (ret == 0) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_SLAVE2_CONFIG, - (uint8_t*) &slave2_config, 1); - } - - if (ret == 0) { - slave2_config.slave2_numop = val->slv_len; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_SLAVE2_CONFIG, - (uint8_t*) &slave2_config, 1); - } - - if (ret == 0) { - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); - } - - return ret; -} - -/** - * @brief sh_slv3_cfg_read: [get] Configure slave 0 for perform a write/read. - * - * @param stmdev_ctx_t *ctx: read / write interface definitions - * @param lsm6ds3_sh_cfg_read_t: a 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 - * - */ -int32_t lsm6ds3_sh_slv3_cfg_read(stmdev_ctx_t* ctx, lsm6ds3_sh_cfg_read_t* val) { - lsm6ds3_slv3_add_t slv3_add; - lsm6ds3_slave3_config_t slave3_config; - int32_t ret; - - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); - - if (ret == 0) { - slv3_add.slave3_add = val->slv_add >> 1; - slv3_add.r_3 = 1; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_SLV3_ADD, (uint8_t*) &slv3_add, 1); - } - - if (ret == 0) { - ret = lsm6ds3_write_reg(ctx, LSM6DS3_SLV3_SUBADD, - &(val->slv_subadd), 1); - } - - if (ret == 0) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_SLAVE3_CONFIG, - (uint8_t*) &slave3_config, 1); - } - - if (ret == 0) { - slave3_config.slave3_numop = val->slv_len; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_SLAVE3_CONFIG, - (uint8_t*) &slave3_config, 1); - } - - if (ret == 0) { - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); - } - - return ret; -} - -/** - * @brief Sensor hub communication status (0: on-going / 1: idle).[get] - * - * @param ctx read / write interface definitions(ptr) - * @param val get the values of sensor_hub_end_op in reg FUNC_SRC - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6ds3_sh_end_op_flag_get(stmdev_ctx_t* ctx, uint8_t* val) { - lsm6ds3_func_src_t func_src; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_FUNC_SRC, (uint8_t*) &func_src, 1); - *val = (uint8_t) func_src.sensor_hub_end_op; - - return ret; -} - -/** - * @brief xl_hp_path_internal: [set] HPF or SLOPE filter selection on - * wake-up and Activity/Inactivity - * functions. - * - * @param stmdev_ctx_t *ctx: read / write interface definitions - * @param lsm6ds3_slope_fds_t: change the values of slope_fds in reg TAP_CFG - * - */ -int32_t lsm6ds3_xl_hp_path_internal_set(stmdev_ctx_t* ctx, lsm6ds3_slope_fds_t val) { - lsm6ds3_tap_cfg_t tap_cfg; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t*) &tap_cfg, 1); - - if (ret == 0) { - tap_cfg.slope_fds = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t*) &tap_cfg, 1); - } - - return ret; -} - -/** - * @brief xl_hp_path_internal: [get] HPF or SLOPE filter selection on - * wake-up and Activity/Inactivity - * functions. - * - * @param stmdev_ctx_t *ctx: read / write interface definitions - * @param lsm6ds3_slope_fds_t: Get the values of slope_fds in reg TAP_CFG - * - */ -int32_t lsm6ds3_xl_hp_path_internal_get(stmdev_ctx_t* ctx, lsm6ds3_slope_fds_t* val) { - lsm6ds3_tap_cfg_t reg; - int32_t ret; - - ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t*) ®, 1); - - switch (reg.slope_fds) { - case LSM6DS3_USE_SLOPE: - *val = LSM6DS3_USE_SLOPE; - break; - - case LSM6DS3_USE_HPF: - *val = LSM6DS3_USE_HPF; - break; - - default: - *val = LSM6DS3_USE_SLOPE; - break; - } - - return ret; -} - -/** - * @brief sh_num_of_dev_connected: [set] Number of external sensors to - * be read by the sensor hub. - * - * @param stmdev_ctx_t *ctx: read / write interface definitions - * @param lsm6ds3_aux_sens_on_t: change the values of aux_sens_on in - * reg SLAVE0_CONFIG - * - */ -int32_t lsm6ds3_sh_num_of_dev_connected_set(stmdev_ctx_t* ctx, lsm6ds3_aux_sens_on_t val) { - lsm6ds3_slave0_config_t reg; - int32_t ret; - - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); - - if (ret == 0) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_SLAVE0_CONFIG, (uint8_t*) ®, 1); - } - - if (ret == 0) { - reg.aux_sens_on = (uint8_t) val; - ret = lsm6ds3_write_reg(ctx, LSM6DS3_SLAVE0_CONFIG, (uint8_t*) ®, 1); - } - - if (ret == 0) { - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); - } - - return ret; -} - -/** - * @brief sh_num_of_dev_connected: [get] Number of external sensors to - * be read by the sensor hub. - * - * @param stmdev_ctx_t *ctx: read / write interface definitions - * @param lsm6ds3_aux_sens_on_t: Get the values of aux_sens_on in - * reg SLAVE0_CONFIG - * - */ -int32_t lsm6ds3_sh_num_of_dev_connected_get(stmdev_ctx_t* ctx, lsm6ds3_aux_sens_on_t* val) { - lsm6ds3_slave0_config_t reg; - int32_t ret; - - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); - - if (ret == 0) { - ret = lsm6ds3_read_reg(ctx, LSM6DS3_SLAVE0_CONFIG, (uint8_t*) ®, 1); - - switch (reg.aux_sens_on) { - case LSM6DS3_SLV_0: - *val = LSM6DS3_SLV_0; - break; - - case LSM6DS3_SLV_0_1: - *val = LSM6DS3_SLV_0_1; - break; - - case LSM6DS3_SLV_0_1_2: - *val = LSM6DS3_SLV_0_1_2; - break; - - case LSM6DS3_SLV_0_1_2_3: - *val = LSM6DS3_SLV_0_1_2_3; - break; - - default: - *val = LSM6DS3_SLV_0; - break; - } - } - - if (ret == 0) { - ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); - } - - return ret; -} - -/** - * @} - * - */ - -/** - * @} - * - */ - -/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/src/platforms/platform.cpp b/src/platforms/platform.cpp new file mode 100644 index 0000000..32b2d21 --- /dev/null +++ b/src/platforms/platform.cpp @@ -0,0 +1,50 @@ +/** + * @file platform.cpp + * + * @brief This file is a declaration of the Platform_I2C class + * + * @author Bruno Machado + * @author Pedro Machado + * + * @date 08/2023 + */ + +#include "platform.h" + +/***************************************** + * Constants + *****************************************/ + +#define LSM6DS_I2C_ADD_H 0xD7U + +/***************************************** + * Public Functions Bodies Definitions + *****************************************/ + +int32_t platform_write_I2C(lsm6ds_config_t* I2C_config, uint8_t reg, const uint8_t* bufp, uint16_t len) { + HAL_I2C_Mem_Write((I2C_HandleTypeDef*) I2C_config->I2C_pinout.sensor_bus, LSM6DS_I2C_ADD_H, reg, I2C_MEMADD_SIZE_8BIT, (uint8_t*)bufp, len, 1000); + return 0; +} + +int32_t platform_read_I2C(lsm6ds_config_t* I2C_config, uint8_t reg, uint8_t* bufp, uint16_t len) { + HAL_I2C_Mem_Read((I2C_HandleTypeDef*) I2C_config->I2C_pinout.sensor_bus, LSM6DS_I2C_ADD_H, reg, I2C_MEMADD_SIZE_8BIT, bufp, len, 1000); + return 0; +} + +int32_t platform_write_SPI(lsm6ds_config_t* SPI_config, uint8_t reg, const uint8_t* bufp, uint16_t len) { + HAL_GPIO_WritePin(SPI_config->SPI_pinout.cs_gpio_port, SPI_config->SPI_pinout.cs_gpio_pin, GPIO_PIN_RESET); + HAL_SPI_Transmit((SPI_HandleTypeDef*)SPI_config->SPI_pinout.sensor_bus, ®, 1, 1000); + HAL_SPI_Transmit((SPI_HandleTypeDef*)SPI_config->SPI_pinout.sensor_bus, (uint8_t*)bufp, len, 1000); + HAL_GPIO_WritePin(SPI_config->SPI_pinout.cs_gpio_port, SPI_config->SPI_pinout.cs_gpio_pin, GPIO_PIN_SET); + return 0; +} + +int32_t platform_read_SPI(lsm6ds_config_t* SPI_config, uint8_t reg, uint8_t* bufp, uint16_t len) { + /* MSB must be 1 when reading */ + reg |= 0x80; + HAL_GPIO_WritePin((SPI_config->SPI_pinout).cs_gpio_port, (SPI_config->SPI_pinout).cs_gpio_pin, GPIO_PIN_RESET); + HAL_SPI_Transmit((SPI_HandleTypeDef*)SPI_config->SPI_pinout.sensor_bus, ®, 1, 1000); + HAL_SPI_Receive((SPI_HandleTypeDef*)SPI_config->SPI_pinout.sensor_bus, bufp, len, 1000); + HAL_GPIO_WritePin((SPI_config->SPI_pinout).cs_gpio_port, (SPI_config->SPI_pinout).cs_gpio_pin, GPIO_PIN_SET); + return 0; +} diff --git a/src/proxys/lsm6ds3_proxy.cpp b/src/proxys/lsm6ds3_proxy.cpp new file mode 100644 index 0000000..bed3b3a --- /dev/null +++ b/src/proxys/lsm6ds3_proxy.cpp @@ -0,0 +1,395 @@ +/** + * @file lsm6ds3.cpp + * + * @brief This file is a definiton of the LSM6DS3 class + * + * @author Bruno Machado + * @author Pedro Machado + * + * @date 08/2023 + */ + +#include "lsm6ds3_proxy.hpp" +#include + +/***************************************** + * Class Public Methods Bodies Definitions + *****************************************/ + +int8_t LSM6DS3_Proxy::init(lsm6ds_settings_t lsm6ds_settings, lsm6ds_I2C_pinout_t I2C_pinout_config, platform_read_f platform_read, platform_write_f platform_write) { + int32_t error_code; + sensor_settings = lsm6ds_settings; + pinout_config.I2C_pinout = I2C_pinout_config; + dev_ctx.handle = (lsm6ds_config_t*) &pinout_config.I2C_pinout; + dev_ctx.read_reg = platform_read; + dev_ctx.write_reg = platform_write; + pinout_config.I2C_pinout.I2C_init(); + + error_code = base_init(); + + if (error_code != 0) { + return LSM6DS_ERROR_WRITE_REGISTER; + } + + if (sensor_settings.lsm6ds_fifo_md == LSM6DS_FIFO_MODE) { + error_code = fifo_mode_init(); + if (error_code != 0) { + return LSM6DS_ERROR_WRITE_REGISTER; + } + } + + if (sensor_settings.lsm6ds_gy_interrupt != LSM6DS_GY_DRDY_NONE || sensor_settings.lsm6ds_xl_interrupt != LSM6DS_XL_DRDY_NONE) { + error_code = fifo_mode_init(); + if (error_code != 0) { + return LSM6DS_ERROR_WRITE_REGISTER; + } + } +} + + +int8_t LSM6DS3_Proxy::init(lsm6ds_settings_t lsm6ds_settings, lsm6ds_SPI_pinout_t SPI_pinout_config, platform_read_f platform_read, platform_write_f platform_write) { + int32_t error_code; + sensor_settings = lsm6ds_settings; + pinout_config.SPI_pinout = SPI_pinout_config; + dev_ctx.handle = (lsm6ds_config_t*) &pinout_config.I2C_pinout; + dev_ctx.read_reg = platform_read; + dev_ctx.write_reg = platform_write; + pinout_config.I2C_pinout.I2C_init(); + + error_code = base_init(); + + if (error_code != 0) { + return LSM6DS_ERROR_WRITE_REGISTER; + } + + if (sensor_settings.lsm6ds_fifo_md == LSM6DS_FIFO_MODE) { + error_code = fifo_mode_init(); + if (error_code != 0) { + return LSM6DS_ERROR_WRITE_REGISTER; + } + } + + if (sensor_settings.lsm6ds_gy_interrupt != LSM6DS_GY_DRDY_NONE || sensor_settings.lsm6ds_xl_interrupt != LSM6DS_XL_DRDY_NONE) { + error_code = fifo_mode_init(); + if (error_code != 0) { + return LSM6DS_ERROR_WRITE_REGISTER; + } + } +} + + +void LSM6DS3_Proxy::update_data() { + // uint8_t reg; + // int32_t error_code; + + // /* Read output only if new value is available */ + // lsm6ds3tr_c_xl_flag_data_ready_get(&dev_ctx, ®); + + // if (reg) { + // /* Read acceleration field data */ + // memset(data_raw_acceleration.u8bit, 0x00, 3 * sizeof(int16_t)); + // lsm6ds3tr_c_acceleration_raw_get(&dev_ctx, data_raw_acceleration.i16bit); + + // for (int i = 0; i < 3; i++) { + // acceleration_mg[i] = acc_conversion_f(data_raw_acceleration.i16bit[i]); + // } + // } + + // lsm6ds3tr_c_gy_flag_data_ready_get(&dev_ctx, ®); + + // if (reg) { + // /* Read angular rate field data */ + // memset(data_raw_angular_rate.u8bit, 0x00, 3 * sizeof(int16_t)); + // lsm6ds3tr_c_angular_rate_raw_get(&dev_ctx, data_raw_angular_rate.i16bit); + + // for (int j = 0; j < 3; j++) { + // angular_rate_mdps[j] = gyro_conversion_f(data_raw_angular_rate.i16bit[j]); + // } + // } +} + +void LSM6DS3_Proxy::update_data_ready_interrupt() { + // int32_t error_value; + + // /* Read acceleration field data when corresponding INT is HIGH */ + // if (HAL_GPIO_ReadPin(int_gpio_port_xl, int_gpio_pin_xl) == 1) { + // memset(data_raw_acceleration.u8bit, 0x00, 3 * sizeof(int16_t)); + // lsm6ds3tr_c_acceleration_raw_get(&dev_ctx, data_raw_acceleration.i16bit); + + // for (int i = 0; i < 3; i++) { + // acceleration_mg[i] = acc_conversion_f(data_raw_acceleration.i16bit[i]); + // } + // } + + // /* Read angular rate field data when corresponding INT is HIGH */ + // if (HAL_GPIO_ReadPin(int_gpio_port_g, int_gpio_pin_g) == 1) { + // memset(data_raw_angular_rate.u8bit, 0x00, 3 * sizeof(int16_t)); + // lsm6ds3tr_c_angular_rate_raw_get(&dev_ctx, data_raw_angular_rate.i16bit); + + // for (int i = 0; i < 3; i++) { + // angular_rate_mdps[i] = gyro_conversion_f(data_raw_angular_rate.i16bit[i]); + // } + // } +} + +void LSM6DS3_Proxy::update_data_fifo_full_interrupt() { + // uint16_t fifo_size; + // lsm6ds3tr_c_fifo_data_level_get(&dev_ctx, &fifo_size); + // float data_sum[3] = {0}; + // uint8_t current_axis; + // for (uint8_t i = 0; i < fifo_size; i++) { + // current_axis = i % 3; + // if (sensor_settings.lsm6ds_dec_fifo_gyro != LSM6DS_GY_ODR_OFF) { + // memset(data_raw_angular_rate.u8bit, 0x00, 3 * sizeof(int16_t)); + // lsm6ds3tr_c_fifo_raw_data_get(&dev_ctx, data_raw_angular_rate.u8bit + 2 * current_axis); + // data_sum[current_axis] += gyro_conversion_f(data_raw_angular_rate.i16bit[current_axis]); + // } + // if (sensor_settings.lsm6ds_dec_fifo_xl != LSM6DS_XL_ODR_OFF ) { + // memset(data_raw_acceleration.u8bit, 0x00, 3 * sizeof(int16_t)); + // lsm6ds3tr_c_fifo_raw_data_get(&dev_ctx, data_raw_acceleration.u8bit + 2 * current_axis); + // data_sum[current_axis] += acc_conversion_f(data_raw_acceleration.i16bit[current_axis]); + // } + // } + // for (uint8_t i = 0; i < 3; i++) { + // if (sensor_settings.lsm6ds_dec_fifo_gyro != LSM6DS_GY_ODR_OFF) { + // angular_rate_mdps[i] = data_sum[i]/(fifo_size/3); + // } + // if (sensor_settings.lsm6ds_dec_fifo_xl != LSM6DS_XL_ODR_OFF) { + // acceleration_mg[i] = data_sum[i]/(fifo_size/3); + // } + // } + // lsm6ds3tr_c_fifo_mode_set(&dev_ctx, LSM6DS3TR_C_BYPASS_MODE); + // lsm6ds3tr_c_fifo_mode_set(&dev_ctx, LSM6DS3TR_C_FIFO_MODE); +} + +void LSM6DS3_Proxy::reset_fifo() { + // lsm6ds3tr_c_fifo_mode_set(&dev_ctx, LSM6DS3TR_C_BYPASS_MODE); + // lsm6ds3tr_c_fifo_mode_set(&dev_ctx, LSM6DS3TR_C_FIFO_MODE); +} + +float* LSM6DS3_Proxy::get_acc_data_mg() { + //return acceleration_mg; +} + +float* LSM6DS3_Proxy::get_gyro_data_mdps() { + //return angular_rate_mdps; +} + +/***************************************** + * Class Private Methods Bodies Definitions + *****************************************/ + +int8_t LSM6DS3_Proxy::base_init() { + // uint8_t rst; + // int32_t error_code; + // /* Restore default configuration */ + // lsm6ds3tr_c_reset_set(&dev_ctx, PROPERTY_ENABLE); + + // do { + // lsm6ds3tr_c_reset_get(&dev_ctx, &rst); + // } while (rst); + + // /* Enable Block Data Update */ + // error_code = lsm6ds3tr_c_block_data_update_set(&dev_ctx, PROPERTY_ENABLE); + + // if (error_code != 0) { + // return LSM6DS_ERROR_WRITE_REGISTER; + // } + + // /* Register address automatically incremented during a multiple byte access with a serial interface */ + // error_code = lsm6ds3tr_c_auto_increment_set(&dev_ctx, PROPERTY_ENABLE); + + // if (error_code != 0) { + // return LSM6DS_ERROR_WRITE_REGISTER; + // } + + // /* Set full scale */ + // error_code = lsm6ds3tr_c_xl_full_scale_set(&dev_ctx, (lsm6ds3tr_c_fs_xl_t) sensor_settings.lsm6ds_xl_fs); + + // if (error_code != 0) { + // return LSM6DS_ERROR_WRITE_REGISTER; + // } + + // error_code = lsm6ds3tr_c_gy_full_scale_set(&dev_ctx, (lsm6ds3tr_c_fs_g_t) sensor_settings.lsm6ds_fs_g); + + // if (error_code != 0) { + // return LSM6DS_ERROR_WRITE_REGISTER; + // } + + // /* Select the function to convert acc raw data to mg */ + // switch (sensor_settings.lsm6ds_xl_fs) { + // case 0: + // acc_conversion_f = lsm6ds3tr_c_from_fs2g_to_mg; + // break; + + // case 1: + // acc_conversion_f = lsm6ds3tr_c_from_fs16g_to_mg; + // break; + + // case 2: + // acc_conversion_f = lsm6ds3tr_c_from_fs4g_to_mg; + // break; + + // case 3: + // acc_conversion_f = lsm6ds3tr_c_from_fs8g_to_mg; + // break; + + // default: + // acc_conversion_f = lsm6ds3tr_c_from_fs2g_to_mg; + // break; + // } + + // /* Select the function to convert gyro raw data to mdps */ + // switch (sensor_settings.lsm6ds_fs_g) { + // case 0: + // gyro_conversion_f = lsm6ds3tr_c_from_fs250dps_to_mdps; + // break; + + // case 1: + // gyro_conversion_f = lsm6ds3tr_c_from_fs125dps_to_mdps; + // break; + + // case 2: + // gyro_conversion_f = lsm6ds3tr_c_from_fs500dps_to_mdps; + // break; + + // case 4: + // gyro_conversion_f = lsm6ds3tr_c_from_fs1000dps_to_mdps; + // break; + + // case 6: + // gyro_conversion_f = lsm6ds3tr_c_from_fs2000dps_to_mdps; + // break; + + // default: + // gyro_conversion_f = lsm6ds3tr_c_from_fs250dps_to_mdps; + // break; + // } + + // /* Set Output Data Rate for Acc and Gyro */ + // error_code = lsm6ds3tr_c_xl_data_rate_set(&dev_ctx, (lsm6ds3tr_c_odr_xl_t) sensor_settings.lsm6ds_odr_xl); + + // if (error_code != 0) { + // return LSM6DS_ERROR_WRITE_REGISTER; + // } + + // error_code = lsm6ds3tr_c_gy_data_rate_set(&dev_ctx, (lsm6ds3tr_c_odr_g_t) sensor_settings.lsm6ds_odr_g); + + // if (error_code != 0) { + // return LSM6DS_ERROR_WRITE_REGISTER; + // } + + // return LSM6DS_ERROR_NONE; +} + +int8_t LSM6DS3_Proxy::interrupt_init() { + // int32_t error_code; + // /* Enable interrupt generation on DRDY INT1 and INT2 pin */ + // lsm6ds3tr_c_pin_int1_route_get(&dev_ctx, &int_1_reg); + // lsm6ds3tr_c_pin_int2_route_get(&dev_ctx, &int_2_reg); + + // switch(sensor_settings.lsm6ds_xl_interrupt) { + // case(LSM6DS_DRDY_INT_1): { + // int_1_reg.int1_drdy_xl = PROPERTY_ENABLE; + // int_gpio_port_xl = pinout_config.common_pinout.int1_port; + // int_gpio_pin_xl = pinout_config.common_pinout.int1_pin; + // break; + // } + // case(LSM6DS_DRDY_INT_2): { + // int_2_reg.int2_drdy_xl = PROPERTY_ENABLE; + // int_gpio_port_xl = pinout_config.common_pinout.int2_port; + // int_gpio_pin_xl = pinout_config.common_pinout.int2_pin; + // break; + // } + // } + + // switch(sensor_settings.lsm6ds_gy_interrupt) { + // case(LSM6DS_DRDY_INT_1): { + // int_1_reg.int1_drdy_g = PROPERTY_ENABLE; + // int_gpio_port_g = pinout_config.common_pinout.int1_port; + // int_gpio_pin_g = pinout_config.common_pinout.int1_pin; + // break; + // } + // case(LSM6DS_DRDY_INT_2): { + // int_2_reg.int2_drdy_g = PROPERTY_ENABLE; + // int_gpio_port_g = pinout_config.common_pinout.int2_port; + // int_gpio_pin_g = pinout_config.common_pinout.int2_pin; + // break; + // } + // } + + // error_code = lsm6ds3tr_c_pin_int1_route_set(&dev_ctx, int_1_reg); + + // if (error_code != 0) { + // return LSM6DS_ERROR_WRITE_REGISTER; + // } + + // lsm6ds3tr_c_pin_int2_route_set(&dev_ctx, int_2_reg); + + // if (error_code != 0) { + // return LSM6DS_ERROR_WRITE_REGISTER; + // } + + // return LSM6DS_ERROR_NONE; +} + +int8_t LSM6DS3_Proxy::fifo_mode_init() { + // int32_t error_code; + // lsm6ds3tr_c_pin_int1_route_get(&dev_ctx, &int_1_reg); + // lsm6ds3tr_c_pin_int2_route_get(&dev_ctx, &int_2_reg); + + // switch (sensor_settings.lsm6ds_fifo_interrupt) { + // case(LSM6DS_FIFO_FULL_INT_1): { + // int_1_reg.int1_full_flag = PROPERTY_ENABLE; + // error_code = lsm6ds3tr_c_pin_int1_route_set(&dev_ctx, int_1_reg); + // break; + // } + // case(LSM6DS_FIFO_FULL_INT_2): { + // int_2_reg.int2_full_flag = PROPERTY_ENABLE; + // error_code = lsm6ds3tr_c_pin_int2_route_set(&dev_ctx, int_2_reg); + // break; + // } + // } + + // if (error_code != 0) { + // return LSM6DS_ERROR_WRITE_REGISTER; + // } + + // error_code = lsm6ds3tr_c_fifo_stop_on_wtm_set(&dev_ctx, PROPERTY_ENABLE); + + // if (error_code != 0) { + // return LSM6DS_ERROR_WRITE_REGISTER; + // } + + // error_code = lsm6ds3tr_c_fifo_watermark_set(&dev_ctx, sensor_settings.lsm6ds_threshold_fifo); + + // if (error_code != 0) { + // return LSM6DS_ERROR_WRITE_REGISTER; + // } + + // error_code = lsm6ds3tr_c_fifo_gy_batch_set(&dev_ctx, (lsm6ds3tr_c_dec_fifo_gyro_t) sensor_settings.lsm6ds_dec_fifo_gyro); + + // if (error_code != 0) { + // return LSM6DS_ERROR_WRITE_REGISTER; + // } + + // error_code = lsm6ds3tr_c_fifo_xl_batch_set(&dev_ctx, (lsm6ds3tr_c_dec_fifo_xl_t) sensor_settings.lsm6ds_dec_fifo_xl); + + // if (error_code != 0) { + // return LSM6DS_ERROR_WRITE_REGISTER; + // } + + // error_code = lsm6ds3tr_c_fifo_data_rate_set(&dev_ctx, (lsm6ds3tr_c_odr_fifo_t) sensor_settings.lsm6ds_odr_fifo); + + // if (error_code != 0) { + // return LSM6DS_ERROR_WRITE_REGISTER; + // } + + // error_code = lsm6ds3tr_c_fifo_mode_set(&dev_ctx, (lsm6ds3tr_c_fifo_mode_t) sensor_settings.lsm6ds_fifo_md); + + // if (error_code != 0) { + // return LSM6DS_ERROR_WRITE_REGISTER; + // } + + // return LSM6DS_ERROR_NONE; +} diff --git a/src/proxys/lsm6ds3tr-c_proxy.cpp b/src/proxys/lsm6ds3tr-c_proxy.cpp new file mode 100644 index 0000000..a95487e --- /dev/null +++ b/src/proxys/lsm6ds3tr-c_proxy.cpp @@ -0,0 +1,398 @@ +/** + * @file lsm6ds3.cpp + * + * @brief This file is a definiton of the LSM6DS3 class + * + * @author Bruno Machado + * @author Pedro Machado + * + * @date 08/2023 + */ + +#include "lsm6ds3tr-c_proxy.hpp" +#include + +/***************************************** + * Class Public Methods Bodies Definitions + *****************************************/ + +int8_t LSM6DS3TRC_Proxy::init(lsm6ds_settings_t lsm6ds_settings, lsm6ds_I2C_pinout_t I2C_pinout_config, platform_read_f platform_read, platform_write_f platform_write) { + int32_t error_code; + sensor_settings = lsm6ds_settings; + pinout_config.I2C_pinout = I2C_pinout_config; + dev_ctx.handle = (lsm6ds_config_t*) &pinout_config.I2C_pinout; + dev_ctx.read_reg = platform_read; + dev_ctx.write_reg = platform_write; + + error_code = base_init(); + + if (error_code != 0) { + return LSM6DS_ERROR_WRITE_REGISTER; + } + + if (sensor_settings.lsm6ds_fifo_md == LSM6DS_FIFO_MODE) { + error_code = fifo_mode_init(); + if (error_code != 0) { + return LSM6DS_ERROR_WRITE_REGISTER; + } + } + + if (sensor_settings.lsm6ds_gy_interrupt != LSM6DS_GY_DRDY_NONE || sensor_settings.lsm6ds_xl_interrupt != LSM6DS_XL_DRDY_NONE) { + error_code = interrupt_init(); + if (error_code != 0) { + return LSM6DS_ERROR_WRITE_REGISTER; + } + } + + return LSM6DS_ERROR_NONE; +} + + +int8_t LSM6DS3TRC_Proxy::init(lsm6ds_settings_t lsm6ds_settings, lsm6ds_SPI_pinout_t SPI_pinout_config, platform_read_f platform_read, platform_write_f platform_write) { + int32_t error_code; + sensor_settings = lsm6ds_settings; + pinout_config.SPI_pinout = SPI_pinout_config; + dev_ctx.handle = (lsm6ds_config_t*) &pinout_config.I2C_pinout; + dev_ctx.read_reg = platform_read; + dev_ctx.write_reg = platform_write; + + error_code = base_init(); + + if (error_code != 0) { + return LSM6DS_ERROR_WRITE_REGISTER; + } + + if (sensor_settings.lsm6ds_fifo_md == LSM6DS_FIFO_MODE) { + error_code = fifo_mode_init(); + if (error_code != 0) { + return LSM6DS_ERROR_WRITE_REGISTER; + } + } + + if (sensor_settings.lsm6ds_gy_interrupt != LSM6DS_GY_DRDY_NONE || sensor_settings.lsm6ds_xl_interrupt != LSM6DS_XL_DRDY_NONE) { + error_code = interrupt_init(); + if (error_code != 0) { + return LSM6DS_ERROR_WRITE_REGISTER; + } + } + + return LSM6DS_ERROR_NONE; +} + + +void LSM6DS3TRC_Proxy::update_data() { + uint8_t reg; + + /* Read output only if new value is available */ + lsm6ds3tr_c_xl_flag_data_ready_get(&dev_ctx, ®); + + if (reg) { + /* Read acceleration field data */ + memset(data_raw_acceleration.u8bit, 0x00, 3 * sizeof(int16_t)); + lsm6ds3tr_c_acceleration_raw_get(&dev_ctx, data_raw_acceleration.i16bit); + + for (int i = 0; i < 3; i++) { + acceleration_mg[i] = acc_conversion_f(data_raw_acceleration.i16bit[i]); + } + } + + lsm6ds3tr_c_gy_flag_data_ready_get(&dev_ctx, ®); + + if (reg) { + /* Read angular rate field data */ + memset(data_raw_angular_rate.u8bit, 0x00, 3 * sizeof(int16_t)); + lsm6ds3tr_c_angular_rate_raw_get(&dev_ctx, data_raw_angular_rate.i16bit); + + for (int j = 0; j < 3; j++) { + angular_rate_mdps[j] = gyro_conversion_f(data_raw_angular_rate.i16bit[j]); + } + } +} + +void LSM6DS3TRC_Proxy::update_data_ready_interrupt() { + /* Read acceleration field data when corresponding INT is HIGH */ + if (HAL_GPIO_ReadPin(int_gpio_port_xl, int_gpio_pin_xl) == 1) { + memset(data_raw_acceleration.u8bit, 0x00, 3 * sizeof(int16_t)); + lsm6ds3tr_c_acceleration_raw_get(&dev_ctx, data_raw_acceleration.i16bit); + + for (int i = 0; i < 3; i++) { + acceleration_mg[i] = acc_conversion_f(data_raw_acceleration.i16bit[i]); + } + } + + /* Read angular rate field data when corresponding INT is HIGH */ + if (HAL_GPIO_ReadPin(int_gpio_port_g, int_gpio_pin_g) == 1) { + memset(data_raw_angular_rate.u8bit, 0x00, 3 * sizeof(int16_t)); + lsm6ds3tr_c_angular_rate_raw_get(&dev_ctx, data_raw_angular_rate.i16bit); + + for (int i = 0; i < 3; i++) { + angular_rate_mdps[i] = gyro_conversion_f(data_raw_angular_rate.i16bit[i]); + } + } +} + +void LSM6DS3TRC_Proxy::update_data_fifo_full_interrupt() { + uint16_t fifo_size; + lsm6ds3tr_c_fifo_data_level_get(&dev_ctx, &fifo_size); + float data_sum[3] = {0}; + uint8_t current_axis; + for (uint8_t i = 0; i < fifo_size; i++) { + current_axis = i % 3; + if (sensor_settings.lsm6ds_dec_fifo_gyro != LSM6DS_FIFO_GY_DISABLE) { + memset(data_raw_angular_rate.u8bit, 0x00, 3 * sizeof(int16_t)); + lsm6ds3tr_c_fifo_raw_data_get(&dev_ctx, data_raw_angular_rate.u8bit + 2 * current_axis); + data_sum[current_axis] += gyro_conversion_f(data_raw_angular_rate.i16bit[current_axis]); + } + if (sensor_settings.lsm6ds_dec_fifo_xl != LSM6DS_FIFO_XL_DISABLE ) { + memset(data_raw_acceleration.u8bit, 0x00, 3 * sizeof(int16_t)); + lsm6ds3tr_c_fifo_raw_data_get(&dev_ctx, data_raw_acceleration.u8bit + 2 * current_axis); + data_sum[current_axis] += acc_conversion_f(data_raw_acceleration.i16bit[current_axis]); + } + } + for (uint8_t i = 0; i < 3; i++) { + if (sensor_settings.lsm6ds_dec_fifo_gyro != LSM6DS_FIFO_GY_DISABLE) { + angular_rate_mdps[i] = data_sum[i]/(fifo_size/3); + } + if (sensor_settings.lsm6ds_dec_fifo_xl != LSM6DS_FIFO_XL_DISABLE) { + acceleration_mg[i] = data_sum[i]/(fifo_size/3); + } + } + lsm6ds3tr_c_fifo_mode_set(&dev_ctx, LSM6DS3TR_C_BYPASS_MODE); + lsm6ds3tr_c_fifo_mode_set(&dev_ctx, LSM6DS3TR_C_FIFO_MODE); +} + +void LSM6DS3TRC_Proxy::reset_fifo() { + lsm6ds3tr_c_fifo_mode_set(&dev_ctx, LSM6DS3TR_C_BYPASS_MODE); + lsm6ds3tr_c_fifo_mode_set(&dev_ctx, LSM6DS3TR_C_FIFO_MODE); +} + +float* LSM6DS3TRC_Proxy::get_acc_data_mg() { + return acceleration_mg; +} + +float* LSM6DS3TRC_Proxy::get_gyro_data_mdps() { + return angular_rate_mdps; +} + +/***************************************** + * Class Private Methods Bodies Definitions + *****************************************/ + +int8_t LSM6DS3TRC_Proxy::base_init() { + uint8_t rst; + int32_t error_code; + /* Restore default configuration */ + lsm6ds3tr_c_reset_set(&dev_ctx, PROPERTY_ENABLE); + + do { + lsm6ds3tr_c_reset_get(&dev_ctx, &rst); + } while (rst); + + /* Enable Block Data Update */ + error_code = lsm6ds3tr_c_block_data_update_set(&dev_ctx, PROPERTY_ENABLE); + + if (error_code != 0) { + return LSM6DS_ERROR_WRITE_REGISTER; + } + /* Register address automatically incremented during a multiple byte access with a serial interface */ + error_code = lsm6ds3tr_c_auto_increment_set(&dev_ctx, PROPERTY_ENABLE); + + if (error_code != 0) { + return LSM6DS_ERROR_WRITE_REGISTER; + } + /* Set full scale */ + error_code = lsm6ds3tr_c_xl_full_scale_set(&dev_ctx, (lsm6ds3tr_c_fs_xl_t) sensor_settings.lsm6ds_xl_fs); + + if (error_code != 0) { + return LSM6DS_ERROR_WRITE_REGISTER; + } + error_code = lsm6ds3tr_c_gy_full_scale_set(&dev_ctx, (lsm6ds3tr_c_fs_g_t) sensor_settings.lsm6ds_fs_g); + + if (error_code != 0) { + return LSM6DS_ERROR_WRITE_REGISTER; + } + + /* Select the function to convert acc raw data to mg */ + switch (sensor_settings.lsm6ds_xl_fs) { + case 0: + acc_conversion_f = lsm6ds3tr_c_from_fs2g_to_mg; + break; + + case 1: + acc_conversion_f = lsm6ds3tr_c_from_fs16g_to_mg; + break; + + case 2: + acc_conversion_f = lsm6ds3tr_c_from_fs4g_to_mg; + break; + + case 3: + acc_conversion_f = lsm6ds3tr_c_from_fs8g_to_mg; + break; + + default: + acc_conversion_f = lsm6ds3tr_c_from_fs2g_to_mg; + break; + } + + /* Select the function to convert gyro raw data to mdps */ + switch (sensor_settings.lsm6ds_fs_g) { + case 0: + gyro_conversion_f = lsm6ds3tr_c_from_fs250dps_to_mdps; + break; + + case 1: + gyro_conversion_f = lsm6ds3tr_c_from_fs125dps_to_mdps; + break; + + case 2: + gyro_conversion_f = lsm6ds3tr_c_from_fs500dps_to_mdps; + break; + + case 4: + gyro_conversion_f = lsm6ds3tr_c_from_fs1000dps_to_mdps; + break; + + case 6: + gyro_conversion_f = lsm6ds3tr_c_from_fs2000dps_to_mdps; + break; + + default: + gyro_conversion_f = lsm6ds3tr_c_from_fs250dps_to_mdps; + break; + } + + /* Set Output Data Rate for Acc and Gyro */ + error_code = lsm6ds3tr_c_xl_data_rate_set(&dev_ctx, (lsm6ds3tr_c_odr_xl_t) sensor_settings.lsm6ds_odr_xl); + + if (error_code != 0) { + return LSM6DS_ERROR_WRITE_REGISTER; + } + error_code = lsm6ds3tr_c_gy_data_rate_set(&dev_ctx, (lsm6ds3tr_c_odr_g_t) sensor_settings.lsm6ds_odr_g); + + if (error_code != 0) { + return LSM6DS_ERROR_WRITE_REGISTER; + } + return LSM6DS_ERROR_NONE; +} + +int8_t LSM6DS3TRC_Proxy::interrupt_init() { + int32_t error_code; + /* Enable interrupt generation on DRDY INT1 and INT2 pin */ + lsm6ds3tr_c_pin_int1_route_get(&dev_ctx, &int_1_reg); + lsm6ds3tr_c_pin_int2_route_get(&dev_ctx, &int_2_reg); + + switch(sensor_settings.lsm6ds_xl_interrupt) { + case(LSM6DS_XL_DRDY_INT_1): { + int_1_reg.int1_drdy_xl = PROPERTY_ENABLE; + int_gpio_port_xl = pinout_config.common_pinout.int1_port; + int_gpio_pin_xl = pinout_config.common_pinout.int1_pin; + break; + } + case(LSM6DS_XL_DRDY_INT_2): { + int_2_reg.int2_drdy_xl = PROPERTY_ENABLE; + int_gpio_port_xl = pinout_config.common_pinout.int2_port; + int_gpio_pin_xl = pinout_config.common_pinout.int2_pin; + break; + } + default: { + return LSM6DS_ERROR_INVALID_SETTING; + } + } + + switch(sensor_settings.lsm6ds_gy_interrupt) { + case(LSM6DS_GY_DRDY_INT_1): { + int_1_reg.int1_drdy_g = PROPERTY_ENABLE; + int_gpio_port_g = pinout_config.common_pinout.int1_port; + int_gpio_pin_g = pinout_config.common_pinout.int1_pin; + break; + } + case(LSM6DS_GY_DRDY_INT_2): { + int_2_reg.int2_drdy_g = PROPERTY_ENABLE; + int_gpio_port_g = pinout_config.common_pinout.int2_port; + int_gpio_pin_g = pinout_config.common_pinout.int2_pin; + break; + } + default: { + return LSM6DS_ERROR_INVALID_SETTING; + } + } + + error_code = lsm6ds3tr_c_pin_int1_route_set(&dev_ctx, int_1_reg); + + if (error_code != 0) { + return LSM6DS_ERROR_WRITE_REGISTER; + } + + lsm6ds3tr_c_pin_int2_route_set(&dev_ctx, int_2_reg); + + if (error_code != 0) { + return LSM6DS_ERROR_WRITE_REGISTER; + } + + return LSM6DS_ERROR_NONE; +} + +int8_t LSM6DS3TRC_Proxy::fifo_mode_init() { + int32_t error_code; + lsm6ds3tr_c_pin_int1_route_get(&dev_ctx, &int_1_reg); + lsm6ds3tr_c_pin_int2_route_get(&dev_ctx, &int_2_reg); + + switch (sensor_settings.lsm6ds_fifo_interrupt) { + case(LSM6DS_FIFO_FULL_INT_1): { + int_1_reg.int1_full_flag = PROPERTY_ENABLE; + error_code = lsm6ds3tr_c_pin_int1_route_set(&dev_ctx, int_1_reg); + break; + } + case(LSM6DS_FIFO_FULL_INT_2): { + int_2_reg.int2_full_flag = PROPERTY_ENABLE; + error_code = lsm6ds3tr_c_pin_int2_route_set(&dev_ctx, int_2_reg); + break; + } + default: { + return LSM6DS_ERROR_INVALID_SETTING; + } + } + + if (error_code != 0) { + return LSM6DS_ERROR_WRITE_REGISTER; + } + + error_code = lsm6ds3tr_c_fifo_stop_on_wtm_set(&dev_ctx, PROPERTY_ENABLE); + + if (error_code != 0) { + return LSM6DS_ERROR_WRITE_REGISTER; + } + + error_code = lsm6ds3tr_c_fifo_watermark_set(&dev_ctx, sensor_settings.lsm6ds_threshold_fifo); + + if (error_code != 0) { + return LSM6DS_ERROR_WRITE_REGISTER; + } + + error_code = lsm6ds3tr_c_fifo_gy_batch_set(&dev_ctx, (lsm6ds3tr_c_dec_fifo_gyro_t) sensor_settings.lsm6ds_dec_fifo_gyro); + + if (error_code != 0) { + return LSM6DS_ERROR_WRITE_REGISTER; + } + + error_code = lsm6ds3tr_c_fifo_xl_batch_set(&dev_ctx, (lsm6ds3tr_c_dec_fifo_xl_t) sensor_settings.lsm6ds_dec_fifo_xl); + + if (error_code != 0) { + return LSM6DS_ERROR_WRITE_REGISTER; + } + + error_code = lsm6ds3tr_c_fifo_data_rate_set(&dev_ctx, (lsm6ds3tr_c_odr_fifo_t) sensor_settings.lsm6ds_odr_fifo); + + if (error_code != 0) { + return LSM6DS_ERROR_WRITE_REGISTER; + } + + error_code = lsm6ds3tr_c_fifo_mode_set(&dev_ctx, (lsm6ds3tr_c_fifo_mode_t) sensor_settings.lsm6ds_fifo_md); + + if (error_code != 0) { + return LSM6DS_ERROR_WRITE_REGISTER; + } + + return LSM6DS_ERROR_NONE; +} diff --git a/src/proxys/lsm6dso_proxy.cpp b/src/proxys/lsm6dso_proxy.cpp new file mode 100644 index 0000000..a3e901e --- /dev/null +++ b/src/proxys/lsm6dso_proxy.cpp @@ -0,0 +1,395 @@ +/** + * @file lsm6ds3.cpp + * + * @brief This file is a definiton of the LSM6DS3 class + * + * @author Bruno Machado + * @author Pedro Machado + * + * @date 08/2023 + */ + +#include "lsm6dso_proxy.hpp" +#include + +/***************************************** + * Class Public Methods Bodies Definitions + *****************************************/ + +int8_t LSM6DSO_Proxy::init(lsm6ds_settings_t lsm6ds_settings, lsm6ds_I2C_pinout_t I2C_pinout_config, platform_read_f platform_read, platform_write_f platform_write) { + int32_t error_code; + sensor_settings = lsm6ds_settings; + pinout_config.I2C_pinout = I2C_pinout_config; + dev_ctx.handle = (lsm6ds_config_t*) &pinout_config.I2C_pinout; + dev_ctx.read_reg = platform_read; + dev_ctx.write_reg = platform_write; + pinout_config.I2C_pinout.I2C_init(); + + error_code = base_init(); + + if (error_code != 0) { + return LSM6DS_ERROR_WRITE_REGISTER; + } + + if (sensor_settings.lsm6ds_fifo_md == LSM6DS_FIFO_MODE) { + error_code = fifo_mode_init(); + if (error_code != 0) { + return LSM6DS_ERROR_WRITE_REGISTER; + } + } + + if (sensor_settings.lsm6ds_gy_interrupt != LSM6DS_GY_DRDY_NONE || sensor_settings.lsm6ds_xl_interrupt != LSM6DS_XL_DRDY_NONE) { + error_code = fifo_mode_init(); + if (error_code != 0) { + return LSM6DS_ERROR_WRITE_REGISTER; + } + } +} + + +int8_t LSM6DSO_Proxy::init(lsm6ds_settings_t lsm6ds_settings, lsm6ds_SPI_pinout_t SPI_pinout_config, platform_read_f platform_read, platform_write_f platform_write) { + int32_t error_code; + sensor_settings = lsm6ds_settings; + pinout_config.SPI_pinout = SPI_pinout_config; + dev_ctx.handle = (lsm6ds_config_t*) &pinout_config.I2C_pinout; + dev_ctx.read_reg = platform_read; + dev_ctx.write_reg = platform_write; + pinout_config.I2C_pinout.I2C_init(); + + error_code = base_init(); + + if (error_code != 0) { + return LSM6DS_ERROR_WRITE_REGISTER; + } + + if (sensor_settings.lsm6ds_fifo_md == LSM6DS_FIFO_MODE) { + error_code = fifo_mode_init(); + if (error_code != 0) { + return LSM6DS_ERROR_WRITE_REGISTER; + } + } + + if (sensor_settings.lsm6ds_gy_interrupt != LSM6DS_GY_DRDY_NONE || sensor_settings.lsm6ds_xl_interrupt != LSM6DS_XL_DRDY_NONE) { + error_code = fifo_mode_init(); + if (error_code != 0) { + return LSM6DS_ERROR_WRITE_REGISTER; + } + } +} + + +void LSM6DSO_Proxy::update_data() { + // uint8_t reg; + // int32_t error_code; + + // /* Read output only if new value is available */ + // lsm6ds3tr_c_xl_flag_data_ready_get(&dev_ctx, ®); + + // if (reg) { + // /* Read acceleration field data */ + // memset(data_raw_acceleration.u8bit, 0x00, 3 * sizeof(int16_t)); + // lsm6ds3tr_c_acceleration_raw_get(&dev_ctx, data_raw_acceleration.i16bit); + + // for (int i = 0; i < 3; i++) { + // acceleration_mg[i] = acc_conversion_f(data_raw_acceleration.i16bit[i]); + // } + // } + + // lsm6ds3tr_c_gy_flag_data_ready_get(&dev_ctx, ®); + + // if (reg) { + // /* Read angular rate field data */ + // memset(data_raw_angular_rate.u8bit, 0x00, 3 * sizeof(int16_t)); + // lsm6ds3tr_c_angular_rate_raw_get(&dev_ctx, data_raw_angular_rate.i16bit); + + // for (int j = 0; j < 3; j++) { + // angular_rate_mdps[j] = gyro_conversion_f(data_raw_angular_rate.i16bit[j]); + // } + // } +} + +void LSM6DSO_Proxy::update_data_ready_interrupt() { + // int32_t error_value; + + // /* Read acceleration field data when corresponding INT is HIGH */ + // if (HAL_GPIO_ReadPin(int_gpio_port_xl, int_gpio_pin_xl) == 1) { + // memset(data_raw_acceleration.u8bit, 0x00, 3 * sizeof(int16_t)); + // lsm6ds3tr_c_acceleration_raw_get(&dev_ctx, data_raw_acceleration.i16bit); + + // for (int i = 0; i < 3; i++) { + // acceleration_mg[i] = acc_conversion_f(data_raw_acceleration.i16bit[i]); + // } + // } + + // /* Read angular rate field data when corresponding INT is HIGH */ + // if (HAL_GPIO_ReadPin(int_gpio_port_g, int_gpio_pin_g) == 1) { + // memset(data_raw_angular_rate.u8bit, 0x00, 3 * sizeof(int16_t)); + // lsm6ds3tr_c_angular_rate_raw_get(&dev_ctx, data_raw_angular_rate.i16bit); + + // for (int i = 0; i < 3; i++) { + // angular_rate_mdps[i] = gyro_conversion_f(data_raw_angular_rate.i16bit[i]); + // } + // } +} + +void LSM6DSO_Proxy::update_data_fifo_full_interrupt() { + // uint16_t fifo_size; + // lsm6ds3tr_c_fifo_data_level_get(&dev_ctx, &fifo_size); + // float data_sum[3] = {0}; + // uint8_t current_axis; + // for (uint8_t i = 0; i < fifo_size; i++) { + // current_axis = i % 3; + // if (sensor_settings.lsm6ds_dec_fifo_gyro != LSM6DS_GY_ODR_OFF) { + // memset(data_raw_angular_rate.u8bit, 0x00, 3 * sizeof(int16_t)); + // lsm6ds3tr_c_fifo_raw_data_get(&dev_ctx, data_raw_angular_rate.u8bit + 2 * current_axis); + // data_sum[current_axis] += gyro_conversion_f(data_raw_angular_rate.i16bit[current_axis]); + // } + // if (sensor_settings.lsm6ds_dec_fifo_xl != LSM6DS_XL_ODR_OFF ) { + // memset(data_raw_acceleration.u8bit, 0x00, 3 * sizeof(int16_t)); + // lsm6ds3tr_c_fifo_raw_data_get(&dev_ctx, data_raw_acceleration.u8bit + 2 * current_axis); + // data_sum[current_axis] += acc_conversion_f(data_raw_acceleration.i16bit[current_axis]); + // } + // } + // for (uint8_t i = 0; i < 3; i++) { + // if (sensor_settings.lsm6ds_dec_fifo_gyro != LSM6DS_GY_ODR_OFF) { + // angular_rate_mdps[i] = data_sum[i]/(fifo_size/3); + // } + // if (sensor_settings.lsm6ds_dec_fifo_xl != LSM6DS_XL_ODR_OFF) { + // acceleration_mg[i] = data_sum[i]/(fifo_size/3); + // } + // } + // lsm6ds3tr_c_fifo_mode_set(&dev_ctx, LSM6DS3TR_C_BYPASS_MODE); + // lsm6ds3tr_c_fifo_mode_set(&dev_ctx, LSM6DS3TR_C_FIFO_MODE); +} + +void LSM6DSO_Proxy::reset_fifo() { + // lsm6ds3tr_c_fifo_mode_set(&dev_ctx, LSM6DS3TR_C_BYPASS_MODE); + // lsm6ds3tr_c_fifo_mode_set(&dev_ctx, LSM6DS3TR_C_FIFO_MODE); +} + +float* LSM6DSO_Proxy::get_acc_data_mg() { + //return acceleration_mg; +} + +float* LSM6DSO_Proxy::get_gyro_data_mdps() { + //return angular_rate_mdps; +} + +/***************************************** + * Class Private Methods Bodies Definitions + *****************************************/ + +int8_t LSM6DSO_Proxy::base_init() { + // uint8_t rst; + // int32_t error_code; + // /* Restore default configuration */ + // lsm6ds3tr_c_reset_set(&dev_ctx, PROPERTY_ENABLE); + + // do { + // lsm6ds3tr_c_reset_get(&dev_ctx, &rst); + // } while (rst); + + // /* Enable Block Data Update */ + // error_code = lsm6ds3tr_c_block_data_update_set(&dev_ctx, PROPERTY_ENABLE); + + // if (error_code != 0) { + // return LSM6DS_ERROR_WRITE_REGISTER; + // } + + // /* Register address automatically incremented during a multiple byte access with a serial interface */ + // error_code = lsm6ds3tr_c_auto_increment_set(&dev_ctx, PROPERTY_ENABLE); + + // if (error_code != 0) { + // return LSM6DS_ERROR_WRITE_REGISTER; + // } + + // /* Set full scale */ + // error_code = lsm6ds3tr_c_xl_full_scale_set(&dev_ctx, (lsm6ds3tr_c_fs_xl_t) sensor_settings.lsm6ds_xl_fs); + + // if (error_code != 0) { + // return LSM6DS_ERROR_WRITE_REGISTER; + // } + + // error_code = lsm6ds3tr_c_gy_full_scale_set(&dev_ctx, (lsm6ds3tr_c_fs_g_t) sensor_settings.lsm6ds_fs_g); + + // if (error_code != 0) { + // return LSM6DS_ERROR_WRITE_REGISTER; + // } + + // /* Select the function to convert acc raw data to mg */ + // switch (sensor_settings.lsm6ds_xl_fs) { + // case 0: + // acc_conversion_f = lsm6ds3tr_c_from_fs2g_to_mg; + // break; + + // case 1: + // acc_conversion_f = lsm6ds3tr_c_from_fs16g_to_mg; + // break; + + // case 2: + // acc_conversion_f = lsm6ds3tr_c_from_fs4g_to_mg; + // break; + + // case 3: + // acc_conversion_f = lsm6ds3tr_c_from_fs8g_to_mg; + // break; + + // default: + // acc_conversion_f = lsm6ds3tr_c_from_fs2g_to_mg; + // break; + // } + + // /* Select the function to convert gyro raw data to mdps */ + // switch (sensor_settings.lsm6ds_fs_g) { + // case 0: + // gyro_conversion_f = lsm6ds3tr_c_from_fs250dps_to_mdps; + // break; + + // case 1: + // gyro_conversion_f = lsm6ds3tr_c_from_fs125dps_to_mdps; + // break; + + // case 2: + // gyro_conversion_f = lsm6ds3tr_c_from_fs500dps_to_mdps; + // break; + + // case 4: + // gyro_conversion_f = lsm6ds3tr_c_from_fs1000dps_to_mdps; + // break; + + // case 6: + // gyro_conversion_f = lsm6ds3tr_c_from_fs2000dps_to_mdps; + // break; + + // default: + // gyro_conversion_f = lsm6ds3tr_c_from_fs250dps_to_mdps; + // break; + //} + + // /* Set Output Data Rate for Acc and Gyro */ + // error_code = lsm6ds3tr_c_xl_data_rate_set(&dev_ctx, (lsm6ds3tr_c_odr_xl_t) sensor_settings.lsm6ds_odr_xl); + + // if (error_code != 0) { + // return LSM6DS_ERROR_WRITE_REGISTER; + // } + + // error_code = lsm6ds3tr_c_gy_data_rate_set(&dev_ctx, (lsm6ds3tr_c_odr_g_t) sensor_settings.lsm6ds_odr_g); + + // if (error_code != 0) { + // return LSM6DS_ERROR_WRITE_REGISTER; + // } + + // return LSM6DS_ERROR_NONE; +} + +int8_t LSM6DSO_Proxy::interrupt_init() { + // int32_t error_code; + // /* Enable interrupt generation on DRDY INT1 and INT2 pin */ + // lsm6ds3tr_c_pin_int1_route_get(&dev_ctx, &int_1_reg); + // lsm6ds3tr_c_pin_int2_route_get(&dev_ctx, &int_2_reg); + + // switch(sensor_settings.lsm6ds_xl_interrupt) { + // case(LSM6DS_DRDY_INT_1): { + // int_1_reg.int1_drdy_xl = PROPERTY_ENABLE; + // int_gpio_port_xl = pinout_config.common_pinout.int1_port; + // int_gpio_pin_xl = pinout_config.common_pinout.int1_pin; + // break; + // } + // case(LSM6DS_DRDY_INT_2): { + // int_2_reg.int2_drdy_xl = PROPERTY_ENABLE; + // int_gpio_port_xl = pinout_config.common_pinout.int2_port; + // int_gpio_pin_xl = pinout_config.common_pinout.int2_pin; + // break; + // } + // } + + // switch(sensor_settings.lsm6ds_gy_interrupt) { + // case(LSM6DS_DRDY_INT_1): { + // int_1_reg.int1_drdy_g = PROPERTY_ENABLE; + // int_gpio_port_g = pinout_config.common_pinout.int1_port; + // int_gpio_pin_g = pinout_config.common_pinout.int1_pin; + // break; + // } + // case(LSM6DS_DRDY_INT_2): { + // int_2_reg.int2_drdy_g = PROPERTY_ENABLE; + // int_gpio_port_g = pinout_config.common_pinout.int2_port; + // int_gpio_pin_g = pinout_config.common_pinout.int2_pin; + // break; + // } + // } + + // error_code = lsm6ds3tr_c_pin_int1_route_set(&dev_ctx, int_1_reg); + + // if (error_code != 0) { + // return LSM6DS_ERROR_WRITE_REGISTER; + // } + + // lsm6ds3tr_c_pin_int2_route_set(&dev_ctx, int_2_reg); + + // if (error_code != 0) { + // return LSM6DS_ERROR_WRITE_REGISTER; + // } + + // return LSM6DS_ERROR_NONE; +} + +int8_t LSM6DSO_Proxy::fifo_mode_init() { +// int32_t error_code; +// lsm6ds3tr_c_pin_int1_route_get(&dev_ctx, &int_1_reg); +// lsm6ds3tr_c_pin_int2_route_get(&dev_ctx, &int_2_reg); + +// switch (sensor_settings.lsm6ds_fifo_interrupt) { +// case(LSM6DS_FIFO_FULL_INT_1): { +// int_1_reg.int1_full_flag = PROPERTY_ENABLE; +// error_code = lsm6ds3tr_c_pin_int1_route_set(&dev_ctx, int_1_reg); +// break; +// } +// case(LSM6DS_FIFO_FULL_INT_2): { +// int_2_reg.int2_full_flag = PROPERTY_ENABLE; +// error_code = lsm6ds3tr_c_pin_int2_route_set(&dev_ctx, int_2_reg); +// break; +// } +// } + +// if (error_code != 0) { +// return LSM6DS_ERROR_WRITE_REGISTER; +// } + +// error_code = lsm6ds3tr_c_fifo_stop_on_wtm_set(&dev_ctx, PROPERTY_ENABLE); + +// if (error_code != 0) { +// return LSM6DS_ERROR_WRITE_REGISTER; +// } + +// error_code = lsm6ds3tr_c_fifo_watermark_set(&dev_ctx, sensor_settings.lsm6ds_threshold_fifo); + +// if (error_code != 0) { +// return LSM6DS_ERROR_WRITE_REGISTER; +// } + +// error_code = lsm6ds3tr_c_fifo_gy_batch_set(&dev_ctx, (lsm6ds3tr_c_dec_fifo_gyro_t) sensor_settings.lsm6ds_dec_fifo_gyro); + +// if (error_code != 0) { +// return LSM6DS_ERROR_WRITE_REGISTER; +// } + +// error_code = lsm6ds3tr_c_fifo_xl_batch_set(&dev_ctx, (lsm6ds3tr_c_dec_fifo_xl_t) sensor_settings.lsm6ds_dec_fifo_xl); + +// if (error_code != 0) { +// return LSM6DS_ERROR_WRITE_REGISTER; +// } + +// error_code = lsm6ds3tr_c_fifo_data_rate_set(&dev_ctx, (lsm6ds3tr_c_odr_fifo_t) sensor_settings.lsm6ds_odr_fifo); + +// if (error_code != 0) { +// return LSM6DS_ERROR_WRITE_REGISTER; +// } + +// error_code = lsm6ds3tr_c_fifo_mode_set(&dev_ctx, (lsm6ds3tr_c_fifo_mode_t) sensor_settings.lsm6ds_fifo_md); + +// if (error_code != 0) { +// return LSM6DS_ERROR_WRITE_REGISTER; +// } + +// return LSM6DS_ERROR_NONE; +} diff --git a/src/reg/lsm6ds3_reg.c b/src/reg/lsm6ds3_reg.c new file mode 100644 index 0000000..ac7ea37 --- /dev/null +++ b/src/reg/lsm6ds3_reg.c @@ -0,0 +1,6873 @@ +/** + ****************************************************************************** + * @file lsm6ds3_reg.c + * @author Sensors Software Solution Team + * @brief LSM6DS3 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 "lsm6ds3_reg.h" + +/** + * @defgroup LSM6DS3 + * @brief This file provides a set of functions needed to drive the + * lsm6ds3 enhanced inertial module. + * @{ + * + */ + +/** + * @defgroup LSM6DS3_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 read / write interface definitions(ptr) + * @param reg register to read + * @param data pointer to buffer that store the data read(ptr) + * @param len number of consecutive register to read + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t __weak lsm6ds3_read_reg(stmdev_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 read / write interface definitions(ptr) + * @param reg register to write + * @param data pointer to data to write in register reg(ptr) + * @param len number of consecutive register to write + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t __weak lsm6ds3_write_reg(stmdev_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 LSM6DS3_Sensitivity + * @brief These functions convert raw-data into engineering units. + * @{ + * + */ + +float_t lsm6ds3_from_fs2g_to_mg(int16_t lsb) +{ + return ((float_t)lsb * 61.0f / 1000.0f); +} + +float_t lsm6ds3_from_fs4g_to_mg(int16_t lsb) +{ + return ((float_t)lsb * 122.0f / 1000.0f); +} + +float_t lsm6ds3_from_fs8g_to_mg(int16_t lsb) +{ + return ((float_t)lsb * 244.0f / 1000.0f); +} + +float_t lsm6ds3_from_fs16g_to_mg(int16_t lsb) +{ + return ((float_t)lsb * 488.0f / 1000.0f); +} + +float_t lsm6ds3_from_fs125dps_to_mdps(int16_t lsb) +{ + return ((float_t)lsb * 4375.0f / 1000.0f); +} + +float_t lsm6ds3_from_fs250dps_to_mdps(int16_t lsb) +{ + return ((float_t)lsb * 8750.0f / 1000.0f); +} + +float_t lsm6ds3_from_fs500dps_to_mdps(int16_t lsb) +{ + return ((float_t)lsb * 1750.0f / 100.0f); +} + +float_t lsm6ds3_from_fs1000dps_to_mdps(int16_t lsb) +{ + return ((float_t)lsb * 35.0f); +} + +float_t lsm6ds3_from_fs2000dps_to_mdps(int16_t lsb) +{ + return ((float_t)lsb * 70.0f); +} + +float_t lsm6ds3_from_lsb_to_celsius(int16_t lsb) +{ + return ((float_t)lsb / 16.0f + 25.0f); +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3_Data_generation + * @brief This section groups all the functions concerning + * data generation + * @{ + * + */ + +/** + * @brief Gyroscope directional user-orientation selection.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of orient in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_gy_data_orient_set(stmdev_ctx_t *ctx, + lsm6ds3_gy_orient_t val) +{ + lsm6ds3_orient_cfg_g_t orient_cfg_g; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_ORIENT_CFG_G, + (uint8_t *)&orient_cfg_g, 1); + + if (ret == 0) + { + orient_cfg_g.orient = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_ORIENT_CFG_G, + (uint8_t *)&orient_cfg_g, 1); + } + + return ret; +} + +/** + * @brief Gyroscope directional user-orientation selection.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of orient in reg ORIENT_CFG_G + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_gy_data_orient_get(stmdev_ctx_t *ctx, + lsm6ds3_gy_orient_t *val) +{ + lsm6ds3_orient_cfg_g_t orient_cfg_g; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_ORIENT_CFG_G, + (uint8_t *)&orient_cfg_g, 1); + + switch (orient_cfg_g.orient) + { + case LSM6DS3_GY_ORIENT_XYZ: + *val = LSM6DS3_GY_ORIENT_XYZ; + break; + + case LSM6DS3_GY_ORIENT_XZY: + *val = LSM6DS3_GY_ORIENT_XZY; + break; + + case LSM6DS3_GY_ORIENT_YXZ: + *val = LSM6DS3_GY_ORIENT_YXZ; + break; + + case LSM6DS3_GY_ORIENT_YZX: + *val = LSM6DS3_GY_ORIENT_YZX; + break; + + case LSM6DS3_GY_ORIENT_ZXY: + *val = LSM6DS3_GY_ORIENT_ZXY; + break; + + case LSM6DS3_GY_ORIENT_ZYX: + *val = LSM6DS3_GY_ORIENT_ZYX; + break; + + default: + *val = LSM6DS3_GY_ORIENT_XYZ; + break; + } + + return ret; +} + +/** + * @brief angular rate sign.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of sign_g in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_gy_data_sign_set(stmdev_ctx_t *ctx, + lsm6ds3_gy_sgn_t val) +{ + lsm6ds3_orient_cfg_g_t orient_cfg_g; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_ORIENT_CFG_G, + (uint8_t *)&orient_cfg_g, 1); + + if (ret == 0) + { + orient_cfg_g.sign_g = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_ORIENT_CFG_G, + (uint8_t *)&orient_cfg_g, 1); + } + + return ret; +} + +/** + * @brief angularratesign.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of sign_g in reg ORIENT_CFG_G + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_gy_data_sign_get(stmdev_ctx_t *ctx, + lsm6ds3_gy_sgn_t *val) +{ + lsm6ds3_orient_cfg_g_t orient_cfg_g; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_ORIENT_CFG_G, + (uint8_t *)&orient_cfg_g, 1); + + switch (orient_cfg_g.sign_g) + { + case LSM6DS3_GY_SIGN_PPP: + *val = LSM6DS3_GY_SIGN_PPP; + break; + + case LSM6DS3_GY_SIGN_PPN: + *val = LSM6DS3_GY_SIGN_PPN; + break; + + case LSM6DS3_GY_SIGN_PNP: + *val = LSM6DS3_GY_SIGN_PNP; + break; + + case LSM6DS3_GY_SIGN_NPP: + *val = LSM6DS3_GY_SIGN_NPP; + break; + + case LSM6DS3_GY_SIGN_NNP: + *val = LSM6DS3_GY_SIGN_NNP; + break; + + case LSM6DS3_GY_SIGN_NPN: + *val = LSM6DS3_GY_SIGN_NPN; + break; + + case LSM6DS3_GY_SIGN_PNN: + *val = LSM6DS3_GY_SIGN_PNN; + break; + + case LSM6DS3_GY_SIGN_NNN: + *val = LSM6DS3_GY_SIGN_NNN; + break; + + default: + *val = LSM6DS3_GY_SIGN_PPP; + break; + } + + return ret; +} + +/** + * @brief Accelerometer full-scale selection.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of fs_xl in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_xl_full_scale_set(stmdev_ctx_t *ctx, + lsm6ds3_xl_fs_t val) +{ + lsm6ds3_ctrl1_xl_t ctrl1_xl; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL1_XL, (uint8_t *)&ctrl1_xl, 1); + + if (ret == 0) + { + ctrl1_xl.fs_xl = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL1_XL, (uint8_t *)&ctrl1_xl, 1); + } + + return ret; +} + +/** + * @brief Accelerometer full-scale selection.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of fs_xl in reg CTRL1_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_xl_full_scale_get(stmdev_ctx_t *ctx, + lsm6ds3_xl_fs_t *val) +{ + lsm6ds3_ctrl1_xl_t ctrl1_xl; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL1_XL, (uint8_t *)&ctrl1_xl, 1); + + switch (ctrl1_xl.fs_xl) + { + case LSM6DS3_2g: + *val = LSM6DS3_2g; + break; + + case LSM6DS3_16g: + *val = LSM6DS3_16g; + break; + + case LSM6DS3_4g: + *val = LSM6DS3_4g; + break; + + case LSM6DS3_8g: + *val = LSM6DS3_8g; + break; + + default: + *val = LSM6DS3_2g; + break; + } + + return ret; +} + +/** + * @brief Accelerometer data rate selection.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of odr_xl in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_xl_data_rate_set(stmdev_ctx_t *ctx, + lsm6ds3_odr_xl_t val) +{ + lsm6ds3_ctrl1_xl_t ctrl1_xl; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL1_XL, (uint8_t *)&ctrl1_xl, 1); + + if (ret == 0) + { + ctrl1_xl.odr_xl = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL1_XL, (uint8_t *)&ctrl1_xl, 1); + } + + return ret; +} + +/** + * @brief Accelerometer data rate selection.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of odr_xl in reg CTRL1_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_xl_data_rate_get(stmdev_ctx_t *ctx, + lsm6ds3_odr_xl_t *val) +{ + lsm6ds3_ctrl1_xl_t ctrl1_xl; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL1_XL, (uint8_t *)&ctrl1_xl, 1); + + switch (ctrl1_xl.odr_xl) + { + case LSM6DS3_XL_ODR_OFF: + *val = LSM6DS3_XL_ODR_OFF; + break; + + case LSM6DS3_XL_ODR_12Hz5: + *val = LSM6DS3_XL_ODR_12Hz5; + break; + + case LSM6DS3_XL_ODR_26Hz: + *val = LSM6DS3_XL_ODR_26Hz; + break; + + case LSM6DS3_XL_ODR_52Hz: + *val = LSM6DS3_XL_ODR_52Hz; + break; + + case LSM6DS3_XL_ODR_104Hz: + *val = LSM6DS3_XL_ODR_104Hz; + break; + + case LSM6DS3_XL_ODR_208Hz: + *val = LSM6DS3_XL_ODR_208Hz; + break; + + case LSM6DS3_XL_ODR_416Hz: + *val = LSM6DS3_XL_ODR_416Hz; + break; + + case LSM6DS3_XL_ODR_833Hz: + *val = LSM6DS3_XL_ODR_833Hz; + break; + + case LSM6DS3_XL_ODR_1k66Hz: + *val = LSM6DS3_XL_ODR_1k66Hz; + break; + + case LSM6DS3_XL_ODR_3k33Hz: + *val = LSM6DS3_XL_ODR_3k33Hz; + break; + + case LSM6DS3_XL_ODR_6k66Hz: + *val = LSM6DS3_XL_ODR_6k66Hz; + break; + + default: + *val = LSM6DS3_XL_ODR_OFF; + break; + } + + return ret; +} + +/** + * @brief Gyroscope UI chain full-scale selection.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of fs_g in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_gy_full_scale_set(stmdev_ctx_t *ctx, + lsm6ds3_fs_g_t val) +{ + lsm6ds3_ctrl2_g_t ctrl2_g; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL2_G, (uint8_t *)&ctrl2_g, 1); + + if (ret == 0) + { + ctrl2_g.fs_g = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL2_G, (uint8_t *)&ctrl2_g, 1); + } + + return ret; +} + +/** + * @brief Gyroscope UI chain full-scale selection.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of fs_g in reg CTRL2_G + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_gy_full_scale_get(stmdev_ctx_t *ctx, + lsm6ds3_fs_g_t *val) +{ + lsm6ds3_ctrl2_g_t ctrl2_g; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL2_G, (uint8_t *)&ctrl2_g, 1); + + switch (ctrl2_g.fs_g) + { + case LSM6DS3_250dps: + *val = LSM6DS3_250dps; + break; + + case LSM6DS3_125dps: + *val = LSM6DS3_125dps; + break; + + case LSM6DS3_500dps: + *val = LSM6DS3_500dps; + break; + + case LSM6DS3_1000dps: + *val = LSM6DS3_1000dps; + break; + + case LSM6DS3_2000dps: + *val = LSM6DS3_2000dps; + break; + + default: + *val = LSM6DS3_250dps; + break; + } + + return ret; +} + +/** + * @brief Gyroscope UI data rate selection.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of odr_g in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_gy_data_rate_set(stmdev_ctx_t *ctx, + lsm6ds3_odr_g_t val) +{ + lsm6ds3_ctrl2_g_t ctrl2_g; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL2_G, (uint8_t *)&ctrl2_g, 1); + + if (ret == 0) + { + ctrl2_g.odr_g = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL2_G, (uint8_t *)&ctrl2_g, 1); + } + + return ret; +} + +/** + * @brief Gyroscope UI data rate selection.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of odr_g in reg CTRL2_G + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_gy_data_rate_get(stmdev_ctx_t *ctx, + lsm6ds3_odr_g_t *val) +{ + lsm6ds3_ctrl2_g_t ctrl2_g; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL2_G, (uint8_t *)&ctrl2_g, 1); + + switch (ctrl2_g.odr_g) + { + case LSM6DS3_GY_ODR_OFF: + *val = LSM6DS3_GY_ODR_OFF; + break; + + case LSM6DS3_GY_ODR_12Hz5: + *val = LSM6DS3_GY_ODR_12Hz5; + break; + + case LSM6DS3_GY_ODR_26Hz: + *val = LSM6DS3_GY_ODR_26Hz; + break; + + case LSM6DS3_GY_ODR_52Hz: + *val = LSM6DS3_GY_ODR_52Hz; + break; + + case LSM6DS3_GY_ODR_104Hz: + *val = LSM6DS3_GY_ODR_104Hz; + break; + + case LSM6DS3_GY_ODR_208Hz: + *val = LSM6DS3_GY_ODR_208Hz; + break; + + case LSM6DS3_GY_ODR_416Hz: + *val = LSM6DS3_GY_ODR_416Hz; + break; + + case LSM6DS3_GY_ODR_833Hz: + *val = LSM6DS3_GY_ODR_833Hz; + break; + + case LSM6DS3_GY_ODR_1k66Hz: + *val = LSM6DS3_GY_ODR_1k66Hz; + break; + + default: + *val = LSM6DS3_GY_ODR_OFF; + break; + } + + return ret; +} + +/** + * @brief Blockdataupdate.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of bdu in reg CTRL3_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_block_data_update_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + + if (ret == 0) + { + ctrl3_c.bdu = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + } + + return ret; +} + +/** + * @brief Blockdataupdate.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of bdu in reg CTRL3_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_block_data_update_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + *val = (uint8_t)ctrl3_c.bdu; + + return ret; +} + +/** + * @brief High-performance operating mode for accelerometer.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of xl_hm_mode in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_xl_power_mode_set(stmdev_ctx_t *ctx, + lsm6ds3_xl_hm_mode_t val) +{ + lsm6ds3_ctrl6_c_t ctrl6_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL6_C, (uint8_t *)&ctrl6_c, 1); + + if (ret == 0) + { + ctrl6_c.xl_hm_mode = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL6_C, (uint8_t *)&ctrl6_c, 1); + } + + return ret; +} + +/** + * @brief High-performance operating mode for accelerometer.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of xl_hm_mode in reg CTRL6_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_xl_power_mode_get(stmdev_ctx_t *ctx, + lsm6ds3_xl_hm_mode_t *val) +{ + lsm6ds3_ctrl6_c_t ctrl6_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL6_C, (uint8_t *)&ctrl6_c, 1); + + switch (ctrl6_c.xl_hm_mode) + { + case LSM6DS3_XL_HIGH_PERFORMANCE: + *val = LSM6DS3_XL_HIGH_PERFORMANCE; + break; + + case LSM6DS3_XL_NORMAL: + *val = LSM6DS3_XL_NORMAL; + break; + + default: + *val = LSM6DS3_XL_HIGH_PERFORMANCE; + break; + } + + return ret; +} + +/** + * @brief Source register rounding function on ADD HERE ROUNDING REGISTERS.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of rounding_status in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_rounding_on_status_set(stmdev_ctx_t *ctx, + lsm6ds3_rnd_stat_t val) +{ + lsm6ds3_ctrl7_g_t ctrl7_g; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL7_G, (uint8_t *)&ctrl7_g, 1); + + if (ret == 0) + { + ctrl7_g.rounding_status = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL7_G, (uint8_t *)&ctrl7_g, 1); + } + + return ret; +} + +/** + * @brief Source register rounding function on ADD HERE ROUNDING REGISTERS.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of rounding_status in reg CTRL7_G + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_rounding_on_status_get(stmdev_ctx_t *ctx, + lsm6ds3_rnd_stat_t *val) +{ + lsm6ds3_ctrl7_g_t ctrl7_g; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL7_G, (uint8_t *)&ctrl7_g, 1); + + switch (ctrl7_g.rounding_status) + { + case LSM6DS3_STAT_RND_DISABLE: + *val = LSM6DS3_STAT_RND_DISABLE; + break; + + case LSM6DS3_STAT_RND_ENABLE: + *val = LSM6DS3_STAT_RND_ENABLE; + break; + + default: + *val = LSM6DS3_STAT_RND_DISABLE; + break; + } + + return ret; +} + +/** + * @brief High-performance operating mode disable for gyroscope.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of g_hm_mode in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_gy_power_mode_set(stmdev_ctx_t *ctx, + lsm6ds3_g_hm_mode_t val) +{ + lsm6ds3_ctrl7_g_t ctrl7_g; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL7_G, (uint8_t *)&ctrl7_g, 1); + + if (ret == 0) + { + ctrl7_g.g_hm_mode = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL7_G, (uint8_t *)&ctrl7_g, 1); + } + + return ret; +} + +/** + * @brief High-performance operating mode disable for gyroscope.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of g_hm_mode in reg CTRL7_G + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_gy_power_mode_get(stmdev_ctx_t *ctx, + lsm6ds3_g_hm_mode_t *val) +{ + lsm6ds3_ctrl7_g_t ctrl7_g; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL7_G, (uint8_t *)&ctrl7_g, 1); + + switch (ctrl7_g.g_hm_mode) + { + case LSM6DS3_GY_HIGH_PERFORMANCE: + *val = LSM6DS3_GY_HIGH_PERFORMANCE; + break; + + case LSM6DS3_GY_NORMAL: + *val = LSM6DS3_GY_NORMAL; + break; + + default: + *val = LSM6DS3_GY_HIGH_PERFORMANCE; + break; + } + + return ret; +} + +/** + * @brief Accelerometer X-axis output enable/disable.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of xen_xl in reg CTRL9_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_xl_axis_x_data_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_ctrl9_xl_t ctrl9_xl; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL9_XL, (uint8_t *)&ctrl9_xl, 1); + + if (ret == 0) + { + ctrl9_xl.xen_xl = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL9_XL, (uint8_t *)&ctrl9_xl, 1); + } + + return ret; +} + +/** + * @brief Accelerometer X-axis output enable/disable.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of xen_xl in reg CTRL9_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_xl_axis_x_data_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_ctrl9_xl_t ctrl9_xl; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL9_XL, (uint8_t *)&ctrl9_xl, 1); + *val = (uint8_t)ctrl9_xl.xen_xl; + + return ret; +} + +/** + * @brief Accelerometer Y-axis output enable/disable.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of yen_xl in reg CTRL9_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_xl_axis_y_data_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_ctrl9_xl_t ctrl9_xl; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL9_XL, (uint8_t *)&ctrl9_xl, 1); + + if (ret == 0) + { + ctrl9_xl.yen_xl = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL9_XL, (uint8_t *)&ctrl9_xl, 1); + } + + return ret; +} + +/** + * @brief Accelerometer Y-axis output enable/disable.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of yen_xl in reg CTRL9_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_xl_axis_y_data_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_ctrl9_xl_t ctrl9_xl; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL9_XL, (uint8_t *)&ctrl9_xl, 1); + *val = (uint8_t)ctrl9_xl.yen_xl; + + return ret; +} + +/** + * @brief Accelerometer Z-axis output enable/disable.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of zen_xl in reg CTRL9_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_xl_axis_z_data_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_ctrl9_xl_t ctrl9_xl; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL9_XL, (uint8_t *)&ctrl9_xl, 1); + + if (ret == 0) + { + ctrl9_xl.zen_xl = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL9_XL, (uint8_t *)&ctrl9_xl, 1); + } + + return ret; +} + +/** + * @brief Accelerometer Z-axis output enable/disable.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of zen_xl in reg CTRL9_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_xl_axis_z_data_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_ctrl9_xl_t ctrl9_xl; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL9_XL, (uint8_t *)&ctrl9_xl, 1); + *val = (uint8_t)ctrl9_xl.zen_xl; + + return ret; +} + +/** + * @brief Gyroscope pitch axis (X) output enable/disable.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of xen_g in reg CTRL10_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_gy_axis_x_data_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_ctrl10_c_t ctrl10_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t *)&ctrl10_c, 1); + + if (ret == 0) + { + ctrl10_c.xen_g = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t *)&ctrl10_c, 1); + } + + return ret; +} + +/** + * @brief Gyroscope pitch axis (X) output enable/disable.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of xen_g in reg CTRL10_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_gy_axis_x_data_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_ctrl10_c_t ctrl10_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t *)&ctrl10_c, 1); + *val = (uint8_t)ctrl10_c.xen_g; + + return ret; +} + +/** + * @brief Gyroscope pitch axis (Y) output enable/disable.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of yen_g in reg CTRL10_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_gy_axis_y_data_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_ctrl10_c_t ctrl10_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t *)&ctrl10_c, 1); + + if (ret == 0) + { + ctrl10_c.yen_g = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t *)&ctrl10_c, 1); + } + + return ret; +} + +/** + * @brief Gyroscope pitch axis (Y) output enable/disable.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of yen_g in reg CTRL10_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_gy_axis_y_data_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_ctrl10_c_t ctrl10_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t *)&ctrl10_c, 1); + *val = (uint8_t)ctrl10_c.yen_g; + + return ret; +} + +/** + * @brief Gyroscope pitch axis (Z) output enable/disable.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of zen_g in reg CTRL10_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_gy_axis_z_data_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_ctrl10_c_t ctrl10_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t *)&ctrl10_c, 1); + + if (ret == 0) + { + ctrl10_c.zen_g = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t *)&ctrl10_c, 1); + } + + return ret; +} + +/** + * @brief Gyroscope pitch axis (Z) output enable/disable.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of zen_g in reg CTRL10_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_gy_axis_z_data_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_ctrl10_c_t ctrl10_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t *)&ctrl10_c, 1); + *val = (uint8_t)ctrl10_c.zen_g; + + return ret; +} + +/** + * @brief Read all the interrupt/status flag of the device. ELENCA I REGISTRI[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val Read all the interrupt flag of the device: + * WAKE_UP_SRC, TAP_SRC, D6D_SRC, FUNC_SRC. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_all_sources_get(stmdev_ctx_t *ctx, + lsm6ds3_all_src_t *val) +{ + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_WAKE_UP_SRC, + (uint8_t *) & (val->wake_up_src), 1); + + if (ret == 0) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_SRC, + (uint8_t *) & (val->tap_src), 1); + } + + if (ret == 0) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_D6D_SRC, + (uint8_t *) & (val->d6d_src), 1); + } + + if (ret == 0) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FUNC_SRC, + (uint8_t *) & (val->func_src), 1); + } + + return ret; +} + +/** + * @brief The STATUS_REG register of the device.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val The STATUS_REG register of the device. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_status_reg_get(stmdev_ctx_t *ctx, + lsm6ds3_status_reg_t *val) +{ + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_STATUS_REG, (uint8_t *)val, 1); + + return ret; +} + +/** + * @brief Accelerometer new data available.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of xlda in reg STATUS_REG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_xl_flag_data_ready_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3_status_reg_t status_reg; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_STATUS_REG, + (uint8_t *)&status_reg, 1); + *val = (uint8_t)status_reg.xlda; + + return ret; +} + +/** + * @brief Gyroscope new data available.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of gda in reg STATUS_REG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_gy_flag_data_ready_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3_status_reg_t status_reg; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_STATUS_REG, + (uint8_t *)&status_reg, 1); + *val = (uint8_t)status_reg.gda; + + return ret; +} + +/** + * @brief Temperature new data available.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of tda in reg STATUS_REG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_temp_flag_data_ready_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3_status_reg_t status_reg; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_STATUS_REG, + (uint8_t *)&status_reg, 1); + *val = (uint8_t)status_reg.tda; + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3_Timestamp + * @brief This section groups all the functions that manage the + * timestamp generation. + * @{ + * + */ + +/** + * @brief Timestamp first byte data output register (r). The value is + * expressed as a 24-bit word and the bit resolution is defined + * by setting the value in WAKE_UP_DUR (5Ch).[get] + * + * @param ctx read / write interface definitions(ptr) + * @param buff buffer that stores data read + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_timestamp_raw_get(stmdev_ctx_t *ctx, uint32_t *val) +{ + uint8_t buff[3]; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_TIMESTAMP0_REG, buff, 3); + *val = buff[2]; + *val = (*val * 256U) + buff[1]; + *val = (*val * 256U) + buff[0]; + + return ret; +} + +/** + * @brief Reset the timer.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param buff buffer that stores data to be write + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_timestamp_rst_set(stmdev_ctx_t *ctx) +{ + int32_t ret; + + uint8_t rst_val = 0xAA; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_TIMESTAMP2_REG, &rst_val, 1); + + return ret; +} + +/** + * @brief Timestamp count enable, output data are collected in + * TIMESTAMP0_REG (40h), TIMESTAMP1_REG (41h), + * TIMESTAMP2_REG (42h) register.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of timer_en in reg TAP_CFG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_timestamp_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_tap_cfg_t tap_cfg; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t *)&tap_cfg, 1); + + if (ret == 0) + { + tap_cfg.timer_en = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t *)&tap_cfg, 1); + } + + return ret; +} + +/** + * @brief Timestamp count enable, output data are collected in + * TIMESTAMP0_REG (40h), TIMESTAMP1_REG (41h), + * TIMESTAMP2_REG (42h) register.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of timer_en in reg TAP_CFG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_timestamp_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_tap_cfg_t tap_cfg; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t *)&tap_cfg, 1); + *val = (uint8_t)tap_cfg.timer_en; + + return ret; +} + +/** + * @brief Timestamp register resolution setting.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of timer_hr in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_timestamp_res_set(stmdev_ctx_t *ctx, + lsm6ds3_ts_res_t val) +{ + lsm6ds3_wake_up_dur_t wake_up_dur; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_WAKE_UP_DUR, + (uint8_t *)&wake_up_dur, 1); + + if (ret == 0) + { + wake_up_dur.timer_hr = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_WAKE_UP_DUR, + (uint8_t *)&wake_up_dur, 1); + } + + return ret; +} + +/** + * @brief Timestamp register resolution setting.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of timer_hr in reg WAKE_UP_DUR + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_timestamp_res_get(stmdev_ctx_t *ctx, + lsm6ds3_ts_res_t *val) +{ + lsm6ds3_wake_up_dur_t wake_up_dur; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_WAKE_UP_DUR, + (uint8_t *)&wake_up_dur, 1); + + switch (wake_up_dur.timer_hr) + { + case LSM6DS3_LSB_6ms4: + *val = LSM6DS3_LSB_6ms4; + break; + + case LSM6DS3_LSB_25us: + *val = LSM6DS3_LSB_25us; + break; + + default: + *val = LSM6DS3_LSB_6ms4; + break; + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3_Dataoutput + * @brief This section groups all the data output functions. + * @{ + * + */ + +/** + * @brief Circular burst-mode (rounding) read from output registers + * through the primary interface.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of rounding in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_rounding_mode_set(stmdev_ctx_t *ctx, + lsm6ds3_rounding_t val) +{ + lsm6ds3_ctrl5_c_t ctrl5_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL5_C, (uint8_t *)&ctrl5_c, 1); + + if (ret == 0) + { + ctrl5_c.rounding = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL5_C, (uint8_t *)&ctrl5_c, 1); + } + + return ret; +} + +/** + * @brief Circular burst-mode (rounding) read from output registers + * through the primary interface.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of rounding in reg CTRL5_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_rounding_mode_get(stmdev_ctx_t *ctx, + lsm6ds3_rounding_t *val) +{ + lsm6ds3_ctrl5_c_t ctrl5_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL5_C, (uint8_t *)&ctrl5_c, 1); + + switch (ctrl5_c.rounding) + { + case LSM6DS3_ROUND_DISABLE: + *val = LSM6DS3_ROUND_DISABLE; + break; + + case LSM6DS3_ROUND_XL: + *val = LSM6DS3_ROUND_XL; + break; + + case LSM6DS3_ROUND_GY: + *val = LSM6DS3_ROUND_GY; + break; + + case LSM6DS3_ROUND_GY_XL: + *val = LSM6DS3_ROUND_GY_XL; + break; + + case LSM6DS3_ROUND_SH1_TO_SH6: + *val = LSM6DS3_ROUND_SH1_TO_SH6; + break; + + case LSM6DS3_ROUND_XL_SH1_TO_SH6: + *val = LSM6DS3_ROUND_XL_SH1_TO_SH6; + break; + + case LSM6DS3_ROUND_GY_XL_SH1_TO_SH12: + *val = LSM6DS3_ROUND_GY_XL_SH1_TO_SH12; + break; + + case LSM6DS3_ROUND_GY_XL_SH1_TO_SH6: + *val = LSM6DS3_ROUND_GY_XL_SH1_TO_SH6; + break; + + default: + *val = LSM6DS3_ROUND_DISABLE; + break; + } + + return ret; +} + +/** + * @brief Temperature data output register (r). L and H registers together + * express a 16-bit word in two’s complement.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param buff buffer that stores data read + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_temperature_raw_get(stmdev_ctx_t *ctx, int16_t *val) +{ + uint8_t buff[2]; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_OUT_TEMP_L, buff, 2); + *val = (int16_t)buff[1]; + *val = (*val * 256) + (int16_t)buff[0]; + + return ret; +} + +/** + * @brief Angular rate sensor. The value is expressed as a 16-bit word + * in two’s complement..[get] + * + * @param ctx read / write interface definitions(ptr) + * @param buff buffer that stores data read + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_angular_rate_raw_get(stmdev_ctx_t *ctx, int16_t *val) +{ + uint8_t buff[6]; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_OUTX_L_G, buff, 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 output register. The value is expressed + * as a 16-bit word in two’s complement.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param buff buffer that stores data read + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_acceleration_raw_get(stmdev_ctx_t *ctx, int16_t *val) +{ + uint8_t buff[6]; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_OUTX_L_XL, buff, 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 fifo_raw_data: [get] read data in FIFO. + * + * @param stmdev_ctx_t *ctx: read / write interface definitions + * @param uint8_t *: data buffer to store FIFO data. + * @param uint8_t : number of data to read from FIFO. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_fifo_raw_data_get(stmdev_ctx_t *ctx, uint8_t *buffer, + uint8_t len) +{ + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_DATA_OUT_L, buffer, len); + + return ret; +} + +/** + * @brief Step counter output register..[get] + * + * @param ctx read / write interface definitions(ptr) + * @param buff buffer that stores data read + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_number_of_steps_get(stmdev_ctx_t *ctx, uint16_t *val) +{ + uint8_t buff[2]; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_STEP_COUNTER_L, buff, 2); + *val = buff[1]; + *val = (*val * 256U) + buff[0]; + + return ret; +} + +/** + * @brief magnetometer raw data.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param buff buffer that stores data read + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_mag_calibrated_raw_get(stmdev_ctx_t *ctx, + int16_t *val) +{ + uint8_t buff[6]; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_OUT_MAG_RAW_X_L, buff, 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; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3_Common + * @brief This section groups common useful functions. + * @{ + * + */ + +/** + * @brief Enable access to the embedded functions.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of func_cfg_en in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_mem_bank_set(stmdev_ctx_t *ctx, + lsm6ds3_func_cfg_en_t val) +{ + lsm6ds3_func_cfg_access_t func_cfg_access; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FUNC_CFG_ACCESS, + (uint8_t *)&func_cfg_access, 1); + + if (ret == 0) + { + func_cfg_access.func_cfg_en = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_FUNC_CFG_ACCESS, + (uint8_t *)&func_cfg_access, 1); + } + + return ret; +} + +/** + * @brief Enable access to the embedded functions.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of func_cfg_en in reg FUNC_CFG_ACCESS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_mem_bank_get(stmdev_ctx_t *ctx, + lsm6ds3_func_cfg_en_t *val) +{ + lsm6ds3_func_cfg_access_t func_cfg_access; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FUNC_CFG_ACCESS, + (uint8_t *)&func_cfg_access, 1); + + switch (func_cfg_access.func_cfg_en) + { + case LSM6DS3_USER_BANK: + *val = LSM6DS3_USER_BANK; + break; + + case LSM6DS3_EMBEDDED_FUNC_BANK: + *val = LSM6DS3_EMBEDDED_FUNC_BANK; + break; + + default: + *val = LSM6DS3_USER_BANK; + break; + } + + return ret; +} + +/** + * @brief DeviceWhoamI..[get] + * + * @param ctx read / write interface definitions(ptr) + * @param buff buffer that stores data read + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_device_id_get(stmdev_ctx_t *ctx, uint8_t *buff) +{ + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_WHO_AM_I, buff, 1); + + return ret; +} + +/** + * @brief Software reset. Restore the default values in user registers.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of sw_reset in reg CTRL3_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_reset_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + + if (ret == 0) + { + ctrl3_c.sw_reset = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + } + + return ret; +} + +/** + * @brief Software reset. Restore the default values in user registers.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of sw_reset in reg CTRL3_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_reset_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + *val = (uint8_t)ctrl3_c.sw_reset; + + return ret; +} + +/** + * @brief Big/Little Endian Data selection.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of ble in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_data_format_set(stmdev_ctx_t *ctx, lsm6ds3_ble_t val) +{ + lsm6ds3_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + + if (ret == 0) + { + ctrl3_c.ble = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + } + + return ret; +} + +/** + * @brief Big/Little Endian Data selection.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of ble in reg CTRL3_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_data_format_get(stmdev_ctx_t *ctx, lsm6ds3_ble_t *val) +{ + lsm6ds3_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + + switch (ctrl3_c.ble) + { + case LSM6DS3_LSB_AT_LOW_ADD: + *val = LSM6DS3_LSB_AT_LOW_ADD; + break; + + case LSM6DS3_MSB_AT_LOW_ADD: + *val = LSM6DS3_MSB_AT_LOW_ADD; + break; + + default: + *val = LSM6DS3_LSB_AT_LOW_ADD; + break; + } + + return ret; +} + +/** + * @brief Register address automatically incremented during a multiple + * byte access with a serial interface.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of if_inc in reg CTRL3_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_auto_increment_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + + if (ret == 0) + { + ctrl3_c.if_inc = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + } + + return ret; +} + +/** + * @brief Register address automatically incremented during a multiple + * byte access with a serial interface.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of if_inc in reg CTRL3_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_auto_increment_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + *val = (uint8_t)ctrl3_c.if_inc; + + return ret; +} + +/** + * @brief Reboot memory content. Reload the calibration parameters.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of boot in reg CTRL3_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_boot_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + + if (ret == 0) + { + ctrl3_c.boot = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + } + + return ret; +} + +/** + * @brief Reboot memory content. Reload the calibration parameters.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of boot in reg CTRL3_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_boot_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + *val = (uint8_t)ctrl3_c.boot; + + return ret; +} + +/** + * @brief Linear acceleration sensor self-test enable.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of st_xl in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_xl_self_test_set(stmdev_ctx_t *ctx, + lsm6ds3_st_xl_t val) +{ + lsm6ds3_ctrl5_c_t ctrl5_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL5_C, (uint8_t *)&ctrl5_c, 1); + + if (ret == 0) + { + ctrl5_c.st_xl = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL5_C, (uint8_t *)&ctrl5_c, 1); + } + + return ret; +} + +/** + * @brief Linear acceleration sensor self-test enable.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of st_xl in reg CTRL5_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_xl_self_test_get(stmdev_ctx_t *ctx, + lsm6ds3_st_xl_t *val) +{ + lsm6ds3_ctrl5_c_t ctrl5_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL5_C, (uint8_t *)&ctrl5_c, 1); + + switch (ctrl5_c.st_xl) + { + case LSM6DS3_XL_ST_DISABLE: + *val = LSM6DS3_XL_ST_DISABLE; + break; + + case LSM6DS3_XL_ST_POSITIVE: + *val = LSM6DS3_XL_ST_POSITIVE; + break; + + case LSM6DS3_XL_ST_NEGATIVE: + *val = LSM6DS3_XL_ST_NEGATIVE; + break; + + default: + *val = LSM6DS3_XL_ST_DISABLE; + break; + } + + return ret; +} + +/** + * @brief Angular rate sensor self-test enable.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of st_g in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_gy_self_test_set(stmdev_ctx_t *ctx, + lsm6ds3_st_g_t val) +{ + lsm6ds3_ctrl5_c_t ctrl5_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL5_C, (uint8_t *)&ctrl5_c, 1); + + if (ret == 0) + { + ctrl5_c.st_g = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL5_C, (uint8_t *)&ctrl5_c, 1); + } + + return ret; +} + +/** + * @brief Angular rate sensor self-test enable.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of st_g in reg CTRL5_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_gy_self_test_get(stmdev_ctx_t *ctx, + lsm6ds3_st_g_t *val) +{ + lsm6ds3_ctrl5_c_t ctrl5_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL5_C, (uint8_t *)&ctrl5_c, 1); + + switch (ctrl5_c.st_g) + { + case LSM6DS3_GY_ST_DISABLE: + *val = LSM6DS3_GY_ST_DISABLE; + break; + + case LSM6DS3_GY_ST_POSITIVE: + *val = LSM6DS3_GY_ST_POSITIVE; + break; + + case LSM6DS3_GY_ST_NEGATIVE: + *val = LSM6DS3_GY_ST_NEGATIVE; + break; + + default: + *val = LSM6DS3_GY_ST_DISABLE; + break; + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3_Filters + * @brief This section group all the functions concerning the + * filters configuration + * @{ + * + */ + +/** + * @brief Mask DRDY on pin (both XL & Gyro) until filter settling ends + * (XL and Gyro independently masked).[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of drdy_mask in reg CTRL4_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_filter_settling_mask_set(stmdev_ctx_t *ctx, + uint8_t val) +{ + lsm6ds3_ctrl4_c_t ctrl4_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + + if (ret == 0) + { + ctrl4_c.drdy_mask = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + } + + return ret; +} + +/** + * @brief Mask DRDY on pin (both XL & Gyro) until filter settling ends + * (XL and Gyro independently masked).[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of drdy_mask in reg CTRL4_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_filter_settling_mask_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3_ctrl4_c_t ctrl4_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + *val = (uint8_t)ctrl4_c.drdy_mask; + + return ret; +} + +/** + * @brief Gyroscope high-pass filter cutoff frequency selection.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of hpcf_g in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_gy_hp_bandwidth_set(stmdev_ctx_t *ctx, + lsm6ds3_hpcf_g_t val) +{ + lsm6ds3_ctrl7_g_t ctrl7_g; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL7_G, (uint8_t *)&ctrl7_g, 1); + + if (ret == 0) + { + ctrl7_g.hpcf_g = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL7_G, (uint8_t *)&ctrl7_g, 1); + } + + return ret; +} + +/** + * @brief Gyroscope high-pass filter cutoff frequency selection.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of hpcf_g in reg CTRL7_G + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_gy_hp_bandwidth_get(stmdev_ctx_t *ctx, + lsm6ds3_hpcf_g_t *val) +{ + lsm6ds3_ctrl7_g_t ctrl7_g; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL7_G, (uint8_t *)&ctrl7_g, 1); + + switch (ctrl7_g.hpcf_g) + { + case LSM6DS3_HP_CUT_OFF_8mHz1: + *val = LSM6DS3_HP_CUT_OFF_8mHz1; + break; + + case LSM6DS3_HP_CUT_OFF_32mHz4: + *val = LSM6DS3_HP_CUT_OFF_32mHz4; + break; + + case LSM6DS3_HP_CUT_OFF_2Hz07: + *val = LSM6DS3_HP_CUT_OFF_2Hz07; + break; + + case LSM6DS3_HP_CUT_OFF_16Hz32: + *val = LSM6DS3_HP_CUT_OFF_16Hz32; + break; + + default: + *val = LSM6DS3_HP_CUT_OFF_8mHz1; + break; + } + + return ret; +} + +/** + * @brief Gyro digital HP filter reset.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of hp_g_rst in reg CTRL7_G + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_gy_hp_reset_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_ctrl7_g_t ctrl7_g; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL7_G, (uint8_t *)&ctrl7_g, 1); + + if (ret == 0) + { + ctrl7_g.hp_g_rst = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL7_G, (uint8_t *)&ctrl7_g, 1); + } + + return ret; +} + +/** + * @brief Gyro digital HP filter reset.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of hp_g_rst in reg CTRL7_G + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_gy_hp_reset_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_ctrl7_g_t ctrl7_g; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL7_G, (uint8_t *)&ctrl7_g, 1); + *val = (uint8_t)ctrl7_g.hp_g_rst; + + return ret; +} + +/** + * @brief Accelerometer slope filter and high-pass filter configuration + * and cut-off setting.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of hp_slope_xl_en in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_xl_hp_bandwidth_set(stmdev_ctx_t *ctx, + lsm6ds3_hp_bw_t val) +{ + lsm6ds3_ctrl8_xl_t ctrl8_xl; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL8_XL, (uint8_t *)&ctrl8_xl, 1); + + if (ret == 0) + { + ctrl8_xl.hp_slope_xl_en = PROPERTY_ENABLE; + ctrl8_xl.hpcf_xl = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL8_XL, (uint8_t *)&ctrl8_xl, 1); + } + + return ret; +} + +/** + * @brief Accelerometer slope filter and high-pass filter configuration + * and cut-off setting.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of hp_slope_xl_en in reg CTRL8_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_xl_hp_bandwidth_get(stmdev_ctx_t *ctx, + lsm6ds3_hp_bw_t *val) +{ + lsm6ds3_ctrl8_xl_t ctrl8_xl; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL8_XL, (uint8_t *)&ctrl8_xl, 1); + + switch (ctrl8_xl.hpcf_xl) + { + case LSM6DS3_XL_HP_ODR_DIV_4: + *val = LSM6DS3_XL_HP_ODR_DIV_4; + break; + + case LSM6DS3_XL_HP_ODR_DIV_100: + *val = LSM6DS3_XL_HP_ODR_DIV_100; + break; + + case LSM6DS3_XL_HP_ODR_DIV_9: + *val = LSM6DS3_XL_HP_ODR_DIV_9; + break; + + case LSM6DS3_XL_HP_ODR_DIV_400: + *val = LSM6DS3_XL_HP_ODR_DIV_400; + break; + + default: + *val = LSM6DS3_XL_HP_ODR_DIV_4; + break; + } + + return ret; +} + +/** + * @brief Accelerometer low-pass filter configuration and + * cut-off setting.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of lpf2_xl_en in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_xl_lp2_bandwidth_set(stmdev_ctx_t *ctx, + lsm6ds3_lp_bw_t val) +{ + lsm6ds3_ctrl8_xl_t ctrl8_xl; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL8_XL, (uint8_t *)&ctrl8_xl, 1); + + if (ret == 0) + { + ctrl8_xl.lpf2_xl_en = PROPERTY_ENABLE; + ctrl8_xl.hpcf_xl = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL8_XL, (uint8_t *)&ctrl8_xl, 1); + } + + return ret; +} + +/** + * @brief Accelerometer low-pass filter configuration and cut-off + * setting.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of lpf2_xl_en in reg CTRL8_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_xl_lp2_bandwidth_get(stmdev_ctx_t *ctx, + lsm6ds3_lp_bw_t *val) +{ + lsm6ds3_ctrl8_xl_t ctrl8_xl; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL8_XL, (uint8_t *)&ctrl8_xl, 1); + + switch (ctrl8_xl.hpcf_xl) + { + case LSM6DS3_XL_LP_ODR_DIV_50: + *val = LSM6DS3_XL_LP_ODR_DIV_50; + break; + + case LSM6DS3_XL_LP_ODR_DIV_100: + *val = LSM6DS3_XL_LP_ODR_DIV_100; + break; + + case LSM6DS3_XL_LP_ODR_DIV_9: + *val = LSM6DS3_XL_LP_ODR_DIV_9; + break; + + case LSM6DS3_XL_LP_ODR_DIV_400: + *val = LSM6DS3_XL_LP_ODR_DIV_400; + break; + + default: + *val = LSM6DS3_XL_LP_ODR_DIV_50; + break; + } + + return ret; +} + +/** + * @brief Anti-aliasing filter bandwidth selection.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of bw_xl in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_xl_filter_analog_set(stmdev_ctx_t *ctx, + lsm6ds3_bw_xl_t val) +{ + lsm6ds3_ctrl1_xl_t ctrl1_xl; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL1_XL, (uint8_t *)&ctrl1_xl, 1); + + if (ret == 0) + { + ctrl1_xl.bw_xl = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL1_XL, (uint8_t *)&ctrl1_xl, 1); + } + + return ret; +} + +/** + * @brief Anti-aliasing filter bandwidth selection.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of bw_xl in reg CTRL1_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_xl_filter_analog_get(stmdev_ctx_t *ctx, + lsm6ds3_bw_xl_t *val) +{ + lsm6ds3_ctrl1_xl_t ctrl1_xl; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL1_XL, (uint8_t *)&ctrl1_xl, 1); + + switch (ctrl1_xl.bw_xl) + { + case LSM6DS3_ANTI_ALIASING_400Hz: + *val = LSM6DS3_ANTI_ALIASING_400Hz; + break; + + case LSM6DS3_ANTI_ALIASING_200Hz: + *val = LSM6DS3_ANTI_ALIASING_200Hz; + break; + + case LSM6DS3_ANTI_ALIASING_100Hz: + *val = LSM6DS3_ANTI_ALIASING_100Hz; + break; + + case LSM6DS3_ANTI_ALIASING_50Hz: + *val = LSM6DS3_ANTI_ALIASING_50Hz; + break; + + default: + *val = LSM6DS3_ANTI_ALIASING_400Hz; + break; + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3_Serial_interface + * @brief This section groups all the functions concerning main + * serial interface management (not auxiliary) + * @{ + * + */ + +/** + * @brief SPI Serial Interface Mode selection.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of sim in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_spi_mode_set(stmdev_ctx_t *ctx, lsm6ds3_sim_t val) +{ + lsm6ds3_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + + if (ret == 0) + { + ctrl3_c.sim = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + } + + return ret; +} + +/** + * @brief SPI Serial Interface Mode selection.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of sim in reg CTRL3_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_spi_mode_get(stmdev_ctx_t *ctx, lsm6ds3_sim_t *val) +{ + lsm6ds3_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + + switch (ctrl3_c.sim) + { + case LSM6DS3_SPI_4_WIRE: + *val = LSM6DS3_SPI_4_WIRE; + break; + + case LSM6DS3_SPI_3_WIRE: + *val = LSM6DS3_SPI_3_WIRE; + break; + + default: + *val = LSM6DS3_SPI_4_WIRE; + break; + } + + return ret; +} + +/** + * @brief Disable / Enable I2C interface.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of i2c_disable in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_i2c_interface_set(stmdev_ctx_t *ctx, + lsm6ds3_i2c_dis_t val) +{ + lsm6ds3_ctrl4_c_t ctrl4_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + + if (ret == 0) + { + ctrl4_c.i2c_disable = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + } + + return ret; +} + +/** + * @brief Disable / Enable I2C interface.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of i2c_disable in reg CTRL4_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_i2c_interface_get(stmdev_ctx_t *ctx, + lsm6ds3_i2c_dis_t *val) +{ + lsm6ds3_ctrl4_c_t ctrl4_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + + switch (ctrl4_c.i2c_disable) + { + case LSM6DS3_I2C_ENABLE: + *val = LSM6DS3_I2C_ENABLE; + break; + + case LSM6DS3_I2C_DISABLE: + *val = LSM6DS3_I2C_DISABLE; + break; + + default: + *val = LSM6DS3_I2C_ENABLE; + break; + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3_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 Select the signal that need to route on int1 pad. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_pin_int1_route_set(stmdev_ctx_t *ctx, + lsm6ds3_int1_route_t *val) +{ + lsm6ds3_int1_ctrl_t int1_ctrl; + lsm6ds3_md1_cfg_t md1_cfg; + lsm6ds3_master_config_t master_config; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + + if (ret == 0) + { + int1_ctrl.int1_drdy_xl = val->int1_drdy_xl; + int1_ctrl.int1_drdy_g = val->int1_drdy_g; + int1_ctrl.int1_boot = val->int1_boot; + int1_ctrl.int1_fth = val->int1_fth; + int1_ctrl.int1_fifo_ovr = val->int1_fifo_ovr; + int1_ctrl.int1_full_flag = val->int1_full_flag; + int1_ctrl.int1_sign_mot = val->int1_sign_mot; + int1_ctrl.int1_step_detector = val->int1_step_detector; + md1_cfg.int1_timer = val->int1_timer; + md1_cfg.int1_tilt = val->int1_tilt; + md1_cfg.int1_6d = val->int1_6d; + md1_cfg.int1_double_tap = val->int1_double_tap; + md1_cfg.int1_ff = val->int1_ff; + md1_cfg.int1_wu = val->int1_wu; + md1_cfg.int1_single_tap = val->int1_single_tap; + md1_cfg.int1_inact_state = val->int1_inact_state; + master_config.drdy_on_int1 = val->drdy_on_int1; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_INT1_CTRL, (uint8_t *)&int1_ctrl, 1); + + if (ret == 0) + { + ret = lsm6ds3_write_reg(ctx, LSM6DS3_MD1_CFG, (uint8_t *)&md1_cfg, 1); + } + + if (ret == 0) + { + ret = lsm6ds3_write_reg(ctx, LSM6DS3_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + } + } + + return ret; +} + +/** + * @brief Select the signal that need to route on int1 pad.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val Select the signal that need to route on int1 pad. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_pin_int1_route_get(stmdev_ctx_t *ctx, + lsm6ds3_int1_route_t *val) +{ + lsm6ds3_int1_ctrl_t int1_ctrl; + lsm6ds3_md1_cfg_t md1_cfg; + lsm6ds3_master_config_t master_config; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_INT1_CTRL, (uint8_t *)&int1_ctrl, 1); + + if (ret == 0) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_MD1_CFG, (uint8_t *)&md1_cfg, 1); + } + + if (ret == 0) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + } + + if (ret == 0) + { + val->int1_drdy_xl = int1_ctrl.int1_drdy_xl; + val->int1_drdy_g = int1_ctrl.int1_drdy_g; + val->int1_boot = int1_ctrl.int1_boot; + val->int1_fth = int1_ctrl.int1_fth; + val->int1_fifo_ovr = int1_ctrl.int1_fifo_ovr; + val->int1_full_flag = int1_ctrl.int1_full_flag; + val->int1_sign_mot = int1_ctrl.int1_sign_mot; + val->int1_step_detector = int1_ctrl.int1_step_detector; + val->int1_timer = md1_cfg.int1_timer; + val->int1_tilt = md1_cfg.int1_tilt; + val->int1_6d = md1_cfg.int1_6d; + val->int1_double_tap = md1_cfg.int1_double_tap; + val->int1_ff = md1_cfg.int1_ff; + val->int1_wu = md1_cfg.int1_wu; + val->int1_single_tap = md1_cfg.int1_single_tap; + val->int1_inact_state = md1_cfg.int1_inact_state; + val->drdy_on_int1 = master_config.drdy_on_int1; + } + + return ret; +} + +/** + * @brief Select the signal that need to route on int1 pad.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val Select the signal that need to route on int1 pad. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_pin_int2_route_set(stmdev_ctx_t *ctx, + lsm6ds3_int2_route_t *val) +{ + lsm6ds3_int2_ctrl_t int2_ctrl; + lsm6ds3_md2_cfg_t md2_cfg; + lsm6ds3_master_config_t master_config; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + + if (ret == 0) + { + int2_ctrl.int2_drdy_xl = val->int2_drdy_xl; + int2_ctrl.int2_drdy_g = val->int2_drdy_g; + int2_ctrl.int2_drdy_temp = val->int2_drdy_temp; + int2_ctrl.int2_fth = val->int2_fth; + int2_ctrl.int2_fifo_ovr = val->int2_fifo_ovr; + int2_ctrl.int2_full_flag = val->int2_full_flag; + int2_ctrl.int2_step_count_ov = val->int2_step_count_ov; + int2_ctrl.int2_step_delta = val->int2_step_delta; + md2_cfg.int2_iron = val->int2_iron; + md2_cfg.int2_tilt = val->int2_tilt; + md2_cfg.int2_6d = val->int2_6d; + md2_cfg.int2_double_tap = val->int2_double_tap; + md2_cfg.int2_ff = val->int2_ff; + md2_cfg.int2_wu = val->int2_wu; + md2_cfg.int2_single_tap = val->int2_single_tap; + md2_cfg.int2_inact_state = val->int2_inact_state; + master_config.start_config = val->start_config; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_INT2_CTRL, (uint8_t *)&int2_ctrl, 1); + } + + if (ret == 0) + { + ret = lsm6ds3_write_reg(ctx, LSM6DS3_MD2_CFG, (uint8_t *)&md2_cfg, 1); + } + + if (ret == 0) + { + ret = lsm6ds3_write_reg(ctx, LSM6DS3_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + } + + return ret; +} + +/** + * @brief Select the signal that need to route on int1 pad.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val Select the signal that need to route on int1 pad. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_pin_int2_route_get(stmdev_ctx_t *ctx, + lsm6ds3_int2_route_t *val) +{ + lsm6ds3_int2_ctrl_t int2_ctrl; + lsm6ds3_md2_cfg_t md2_cfg; + lsm6ds3_master_config_t master_config; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_INT2_CTRL, (uint8_t *)&int2_ctrl, 1); + + if (ret == 0) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_MD2_CFG, (uint8_t *)&md2_cfg, 1); + } + + if (ret == 0) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + val->int2_drdy_xl = int2_ctrl.int2_drdy_xl; + val->int2_drdy_g = int2_ctrl.int2_drdy_g; + val->int2_drdy_temp = int2_ctrl.int2_drdy_temp; + val->int2_fth = int2_ctrl.int2_fth; + val->int2_fifo_ovr = int2_ctrl.int2_fifo_ovr; + val->int2_full_flag = int2_ctrl.int2_full_flag; + val->int2_step_count_ov = int2_ctrl.int2_step_count_ov; + val->int2_step_delta = int2_ctrl.int2_step_delta; + val->int2_iron = md2_cfg.int2_iron; + val->int2_tilt = md2_cfg.int2_tilt; + val->int2_6d = md2_cfg.int2_6d; + val->int2_double_tap = md2_cfg.int2_double_tap; + val->int2_ff = md2_cfg.int2_ff; + val->int2_wu = md2_cfg.int2_wu; + val->int2_single_tap = md2_cfg.int2_single_tap; + val->int2_inact_state = md2_cfg.int2_inact_state; + val->start_config = master_config.start_config; + } + + return ret; +} + +/** + * @brief Push-pull/open drain selection on interrupt pads.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of pp_od in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_pin_mode_set(stmdev_ctx_t *ctx, lsm6ds3_pp_od_t val) +{ + lsm6ds3_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + + if (ret == 0) + { + ctrl3_c.pp_od = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + } + + return ret; +} + +/** + * @brief Push-pull/open drain selection on interrupt pads.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of pp_od in reg CTRL3_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_pin_mode_get(stmdev_ctx_t *ctx, lsm6ds3_pp_od_t *val) +{ + lsm6ds3_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + + switch (ctrl3_c.pp_od) + { + case LSM6DS3_PUSH_PULL: + *val = LSM6DS3_PUSH_PULL; + break; + + case LSM6DS3_OPEN_DRAIN: + *val = LSM6DS3_OPEN_DRAIN; + break; + + default: + *val = LSM6DS3_PUSH_PULL; + break; + } + + return ret; +} + +/** + * @brief Interrupt active-high/low.Interrupt active-high/low.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of h_lactive in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_pin_polarity_set(stmdev_ctx_t *ctx, + lsm6ds3_pin_pol_t val) +{ + lsm6ds3_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + + if (ret == 0) + { + ctrl3_c.h_lactive = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + } + + return ret; +} + +/** + * @brief Interrupt active-high/low.Interrupt active-high/low.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of h_lactive in reg CTRL3_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_pin_polarity_get(stmdev_ctx_t *ctx, + lsm6ds3_pin_pol_t *val) +{ + lsm6ds3_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + + switch (ctrl3_c.h_lactive) + { + case LSM6DS3_ACTIVE_HIGH: + *val = LSM6DS3_ACTIVE_HIGH; + break; + + case LSM6DS3_ACTIVE_LOW: + *val = LSM6DS3_ACTIVE_LOW; + break; + + default: + *val = LSM6DS3_ACTIVE_HIGH; + break; + } + + return ret; +} + +/** + * @brief All interrupt signals become available on INT1 pin.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of int2_on_int1 in reg CTRL4_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_all_on_int1_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_ctrl4_c_t ctrl4_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + + if (ret == 0) + { + ctrl4_c.int2_on_int1 = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + } + + return ret; +} + +/** + * @brief All interrupt signals become available on INT1 pin.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of int2_on_int1 in reg CTRL4_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_all_on_int1_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_ctrl4_c_t ctrl4_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + *val = (uint8_t)ctrl4_c.int2_on_int1; + + return ret; +} + +/** + * @brief Latched/pulsed interrupt.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of lir in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_int_notification_set(stmdev_ctx_t *ctx, + lsm6ds3_lir_t val) +{ + lsm6ds3_tap_cfg_t tap_cfg; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t *)&tap_cfg, 1); + + if (ret == 0) + { + tap_cfg.lir = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t *)&tap_cfg, 1); + } + + return ret; +} + +/** + * @brief Latched/pulsed interrupt.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of lir in reg TAP_CFG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_int_notification_get(stmdev_ctx_t *ctx, + lsm6ds3_lir_t *val) +{ + lsm6ds3_tap_cfg_t tap_cfg; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t *)&tap_cfg, 1); + + switch (tap_cfg.lir) + { + case LSM6DS3_INT_PULSED: + *val = LSM6DS3_INT_PULSED; + break; + + case LSM6DS3_INT_LATCHED: + *val = LSM6DS3_INT_LATCHED; + break; + + default: + *val = LSM6DS3_INT_PULSED; + break; + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3_Wake_Up_event + * @brief This section groups all the functions that manage the + * Wake Up event generation. + * @{ + * + */ + +/** + * @brief Read the wake_up_src status flag of the device.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val Read the wake_up_src status flag of the device. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_wkup_src_get(stmdev_ctx_t *ctx, + lsm6ds3_wake_up_src_t *val) +{ + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_WAKE_UP_SRC, (uint8_t *)val, 1); + + return ret; +} + +/** + * @brief Threshold for wakeup (1 LSB = FS_XL / 64).[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of wk_ths in reg WAKE_UP_THS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_wkup_threshold_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_wake_up_ths_t wake_up_ths; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_WAKE_UP_THS, + (uint8_t *)&wake_up_ths, 1); + + if (ret == 0) + { + wake_up_ths.wk_ths = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_WAKE_UP_THS, + (uint8_t *)&wake_up_ths, 1); + } + + return ret; +} + +/** + * @brief Threshold for wakeup (1 LSB = FS_XL / 64).[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of wk_ths in reg WAKE_UP_THS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_wkup_threshold_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_wake_up_ths_t wake_up_ths; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_WAKE_UP_THS, + (uint8_t *)&wake_up_ths, 1); + *val = (uint8_t)wake_up_ths.wk_ths; + + return ret; +} + +/** + * @brief Wake up duration event.1LSb = 1 / ODR[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of wake_dur in reg WAKE_UP_DUR + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_wkup_dur_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_wake_up_dur_t wake_up_dur; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_WAKE_UP_DUR, + (uint8_t *)&wake_up_dur, 1); + + if (ret == 0) + { + wake_up_dur.wake_dur = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_WAKE_UP_DUR, + (uint8_t *)&wake_up_dur, 1); + } + + return ret; +} + +/** + * @brief Wake up duration event.1LSb = 1 / ODR[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of wake_dur in reg WAKE_UP_DUR + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_wkup_dur_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_wake_up_dur_t wake_up_dur; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_WAKE_UP_DUR, + (uint8_t *)&wake_up_dur, 1); + *val = (uint8_t)wake_up_dur.wake_dur; + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3_Activity/Inactivity_detection + * @brief This section groups all the functions concerning + * activity/inactivity detection. + * @{ + * + */ + +/** + * @brief Enables gyroscope Sleep mode.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of sleep_g in reg CTRL4_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_gy_sleep_mode_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_ctrl4_c_t ctrl4_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + + if (ret == 0) + { + ctrl4_c.sleep_g = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + } + + return ret; +} + +/** + * @brief Enables gyroscope Sleep mode.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of sleep_g in reg CTRL4_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_gy_sleep_mode_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_ctrl4_c_t ctrl4_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + *val = (uint8_t)ctrl4_c.sleep_g; + + return ret; +} + +/** + * @brief Enable/Disable inactivity function.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of inactivity in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_act_mode_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_wake_up_ths_t wake_up_ths; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_WAKE_UP_THS, + (uint8_t *)&wake_up_ths, 1); + + if (ret == 0) + { + wake_up_ths.inactivity = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_WAKE_UP_THS, + (uint8_t *)&wake_up_ths, 1); + } + + return ret; +} + +/** + * @brief Enable/Disable inactivity function.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of inactivity in reg WAKE_UP_THS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_act_mode_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_wake_up_ths_t wake_up_ths; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_WAKE_UP_THS, + (uint8_t *)&wake_up_ths, 1); + *val = wake_up_ths.inactivity; + + return ret; +} + +/** + * @brief Duration to go in sleep mode. 1 LSb = 512 / ODR[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of sleep_dur in reg WAKE_UP_DUR + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_act_sleep_dur_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_wake_up_dur_t wake_up_dur; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_WAKE_UP_DUR, + (uint8_t *)&wake_up_dur, 1); + + if (ret == 0) + { + wake_up_dur.sleep_dur = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_WAKE_UP_DUR, + (uint8_t *)&wake_up_dur, 1); + } + + return ret; +} + +/** + * @brief Duration to go in sleep mode. 1 LSb = 512 / ODR[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of sleep_dur in reg WAKE_UP_DUR + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_act_sleep_dur_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_wake_up_dur_t wake_up_dur; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_WAKE_UP_DUR, + (uint8_t *)&wake_up_dur, 1); + *val = (uint8_t)wake_up_dur.sleep_dur; + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3_Tap_generator + * @brief This section groups all the functions that manage the + * tap and double tap event generation. + * @{ + * + */ + +/** + * @brief Read the tap_src status flag of the device.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val Read the tap_src status flag of the device. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_tap_src_get(stmdev_ctx_t *ctx, lsm6ds3_tap_src_t *val) +{ + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_SRC, (uint8_t *)val, 1); + + return ret; +} + +/** + * @brief Enable Z direction in tap recognition.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of tap_z_en in reg TAP_CFG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_tap_detection_on_z_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_tap_cfg_t tap_cfg; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t *)&tap_cfg, 1); + + if (ret == 0) + { + tap_cfg.tap_z_en = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t *)&tap_cfg, 1); + } + + return ret; +} + +/** + * @brief Enable Z direction in tap recognition.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of tap_z_en in reg TAP_CFG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_tap_detection_on_z_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3_tap_cfg_t tap_cfg; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t *)&tap_cfg, 1); + *val = (uint8_t)tap_cfg.tap_z_en; + + return ret; +} + +/** + * @brief Enable Y direction in tap recognition.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of tap_y_en in reg TAP_CFG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_tap_detection_on_y_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_tap_cfg_t tap_cfg; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t *)&tap_cfg, 1); + + if (ret == 0) + { + tap_cfg.tap_y_en = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t *)&tap_cfg, 1); + } + + return ret; +} + +/** + * @brief Enable Y direction in tap recognition.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of tap_y_en in reg TAP_CFG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_tap_detection_on_y_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3_tap_cfg_t tap_cfg; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t *)&tap_cfg, 1); + *val = (uint8_t)tap_cfg.tap_y_en; + + return ret; +} + +/** + * @brief Enable X direction in tap recognition.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of tap_x_en in reg TAP_CFG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_tap_detection_on_x_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_tap_cfg_t tap_cfg; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t *)&tap_cfg, 1); + + if (ret == 0) + { + tap_cfg.tap_x_en = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t *)&tap_cfg, 1); + } + + return ret; +} + +/** + * @brief Enable X direction in tap recognition.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of tap_x_en in reg TAP_CFG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_tap_detection_on_x_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3_tap_cfg_t tap_cfg; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t *)&tap_cfg, 1); + *val = (uint8_t)tap_cfg.tap_x_en; + + return ret; +} + +/** + * @brief Threshold for tap recognition.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of tap_ths in reg TAP_THS_6D + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_tap_threshold_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_tap_ths_6d_t tap_ths_6d; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_THS_6D, + (uint8_t *)&tap_ths_6d, 1); + + if (ret == 0) + { + tap_ths_6d.tap_ths = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_TAP_THS_6D, + (uint8_t *)&tap_ths_6d, 1); + } + + return ret; +} + +/** + * @brief Threshold for tap recognition.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of tap_ths in reg TAP_THS_6D + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_tap_threshold_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_tap_ths_6d_t tap_ths_6d; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_THS_6D, + (uint8_t *)&tap_ths_6d, 1); + *val = (uint8_t)tap_ths_6d.tap_ths; + + return ret; +} + +/** + * @brief 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[1:0] bits are set to a different value, + * 1LSB corresponds to 8*ODR_XL time.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of shock in reg INT_DUR2 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_tap_shock_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_int_dur2_t int_dur2; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_INT_DUR2, (uint8_t *)&int_dur2, 1); + + if (ret == 0) + { + int_dur2.shock = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_INT_DUR2, (uint8_t *)&int_dur2, 1); + } + + return ret; +} + +/** + * @brief 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[1:0] bits are set to a different + * value, 1LSB corresponds to 8*ODR_XL time.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of shock in reg INT_DUR2 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_tap_shock_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_int_dur2_t int_dur2; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_INT_DUR2, (uint8_t *)&int_dur2, 1); + *val = (uint8_t)int_dur2.shock; + + return ret; +} + +/** + * @brief Quiet time is the time after the first detected tap in + * which there must not be any over-threshold event. + * The default value of these bits is 00b which corresponds + * to 2*ODR_XL time. If the QUIET[1:0] bits are set to a + * different value, 1LSB corresponds to 4*ODR_XL time.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of quiet in reg INT_DUR2 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_tap_quiet_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_int_dur2_t int_dur2; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_INT_DUR2, (uint8_t *)&int_dur2, 1); + + if (ret == 0) + { + int_dur2.quiet = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_INT_DUR2, (uint8_t *)&int_dur2, 1); + } + + return ret; +} + +/** + * @brief Quiet time is the time after the first detected tap in which + * there must not be any over-threshold event. The default value + * of these bits is 00b which corresponds to 2*ODR_XL time. + * If the QUIET[1:0] bits are set to a different value, + * 1LSB corresponds to 4*ODR_XL time.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of quiet in reg INT_DUR2 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_tap_quiet_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_int_dur2_t int_dur2; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_INT_DUR2, (uint8_t *)&int_dur2, 1); + *val = (uint8_t)int_dur2.quiet; + + return ret; +} + +/** + * @brief 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(ptr) + * @param val change the values of dur in reg INT_DUR2 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_tap_dur_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_int_dur2_t int_dur2; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_INT_DUR2, (uint8_t *)&int_dur2, 1); + + if (ret == 0) + { + int_dur2.dur = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_INT_DUR2, (uint8_t *)&int_dur2, 1); + } + + return ret; +} + +/** + * @brief 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(ptr) + * @param val get the values of dur in reg INT_DUR2 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_tap_dur_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_int_dur2_t int_dur2; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_INT_DUR2, (uint8_t *)&int_dur2, 1); + *val = (uint8_t)int_dur2.dur; + + return ret; +} + +/** + * @brief Single/double-tap event enable.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of single_double_tap in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_tap_mode_set(stmdev_ctx_t *ctx, lsm6ds3_tap_md_t val) +{ + lsm6ds3_wake_up_ths_t wake_up_ths; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_WAKE_UP_THS, + (uint8_t *)&wake_up_ths, 1); + + if (ret == 0) + { + wake_up_ths.single_double_tap = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_WAKE_UP_THS, + (uint8_t *)&wake_up_ths, 1); + } + + return ret; +} + +/** + * @brief Single/double-tap event enable.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of single_double_tap in reg WAKE_UP_THS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_tap_mode_get(stmdev_ctx_t *ctx, lsm6ds3_tap_md_t *val) +{ + lsm6ds3_wake_up_ths_t wake_up_ths; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_WAKE_UP_THS, + (uint8_t *)&wake_up_ths, 1); + + switch (wake_up_ths.single_double_tap) + { + case LSM6DS3_ONLY_DOUBLE: + *val = LSM6DS3_ONLY_DOUBLE; + break; + + case LSM6DS3_SINGLE_DOUBLE: + *val = LSM6DS3_SINGLE_DOUBLE; + break; + + default: + *val = LSM6DS3_ONLY_DOUBLE; + break; + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3_Six_position_detection(6D/4D) + * @brief This section groups all the functions concerning + * six position detection (6D). + * @{ + * + */ + +/** + * @brief LPF2 feed 6D function selection.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of low_pass_on_6d in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_6d_feed_data_set(stmdev_ctx_t *ctx, + lsm6ds3_low_pass_on_6d_t val) +{ + lsm6ds3_ctrl8_xl_t ctrl8_xl; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL8_XL, (uint8_t *)&ctrl8_xl, 1); + + if (ret == 0) + { + ctrl8_xl.low_pass_on_6d = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL8_XL, (uint8_t *)&ctrl8_xl, 1); + } + + return ret; +} + +/** + * @brief LPF2 feed 6D function selection.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of low_pass_on_6d in reg CTRL8_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_6d_feed_data_get(stmdev_ctx_t *ctx, + lsm6ds3_low_pass_on_6d_t *val) +{ + lsm6ds3_ctrl8_xl_t ctrl8_xl; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL8_XL, (uint8_t *)&ctrl8_xl, 1); + + switch (ctrl8_xl.low_pass_on_6d) + { + case LSM6DS3_ODR_DIV_2_FEED: + *val = LSM6DS3_ODR_DIV_2_FEED; + break; + + case LSM6DS3_LPF2_FEED: + *val = LSM6DS3_LPF2_FEED; + break; + + default: + *val = LSM6DS3_ODR_DIV_2_FEED; + break; + } + + return ret; +} + +/** + * @brief Read the d6d_src status flag of the device.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val Read the d6d_src status flag of the device. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_6d_src_get(stmdev_ctx_t *ctx, lsm6ds3_d6d_src_t *val) +{ + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_D6D_SRC, (uint8_t *)val, 1); + + return ret; +} + +/** + * @brief Threshold for 4D/6D function.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of sixd_ths in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_6d_threshold_set(stmdev_ctx_t *ctx, + lsm6ds3_sixd_ths_t val) +{ + lsm6ds3_tap_ths_6d_t tap_ths_6d; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_THS_6D, + (uint8_t *)&tap_ths_6d, 1); + + if (ret == 0) + { + tap_ths_6d.sixd_ths = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_TAP_THS_6D, + (uint8_t *)&tap_ths_6d, 1); + } + + return ret; +} + +/** + * @brief Threshold for 4D/6D function.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of sixd_ths in reg TAP_THS_6D + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_6d_threshold_get(stmdev_ctx_t *ctx, + lsm6ds3_sixd_ths_t *val) +{ + lsm6ds3_tap_ths_6d_t tap_ths_6d; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_THS_6D, + (uint8_t *)&tap_ths_6d, 1); + + switch (tap_ths_6d.sixd_ths) + { + case LSM6DS3_DEG_80: + *val = LSM6DS3_DEG_80; + break; + + case LSM6DS3_DEG_70: + *val = LSM6DS3_DEG_70; + break; + + case LSM6DS3_DEG_60: + *val = LSM6DS3_DEG_60; + break; + + case LSM6DS3_DEG_50: + *val = LSM6DS3_DEG_50; + break; + + default: + *val = LSM6DS3_DEG_80; + break; + } + + return ret; +} + +/** + * @brief 4D orientation detection enable.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of d4d_en in reg TAP_THS_6D + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_4d_mode_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_tap_ths_6d_t tap_ths_6d; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_THS_6D, + (uint8_t *)&tap_ths_6d, 1); + + if (ret == 0) + { + tap_ths_6d.d4d_en = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_TAP_THS_6D, + (uint8_t *)&tap_ths_6d, 1); + } + + return ret; +} + +/** + * @brief 4D orientation detection enable.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of d4d_en in reg TAP_THS_6D + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_4d_mode_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_tap_ths_6d_t tap_ths_6d; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_THS_6D, + (uint8_t *)&tap_ths_6d, 1); + *val = (uint8_t)tap_ths_6d.d4d_en; + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3_Free_fall + * @brief This section group all the functions concerning the + * free fall detection. + * @{ + * + */ + +/** + * @brief Free fall threshold setting.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of ff_ths in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_ff_threshold_set(stmdev_ctx_t *ctx, + lsm6ds3_ff_ths_t val) +{ + lsm6ds3_free_fall_t free_fall; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FREE_FALL, (uint8_t *)&free_fall, 1); + + if (ret == 0) + { + free_fall.ff_ths = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_FREE_FALL, (uint8_t *)&free_fall, 1); + } + + return ret; +} + +/** + * @brief Free fall threshold setting.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of ff_ths in reg FREE_FALL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_ff_threshold_get(stmdev_ctx_t *ctx, + lsm6ds3_ff_ths_t *val) +{ + lsm6ds3_free_fall_t free_fall; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FREE_FALL, (uint8_t *)&free_fall, 1); + + switch (free_fall.ff_ths) + { + case LSM6DS3_156_mg: + *val = LSM6DS3_156_mg; + break; + + case LSM6DS3_219_mg: + *val = LSM6DS3_219_mg; + break; + + case LSM6DS3_250_mg: + *val = LSM6DS3_250_mg; + break; + + case LSM6DS3_312_mg: + *val = LSM6DS3_312_mg; + break; + + case LSM6DS3_344_mg: + *val = LSM6DS3_344_mg; + break; + + case LSM6DS3_406_mg: + *val = LSM6DS3_406_mg; + break; + + case LSM6DS3_469_mg: + *val = LSM6DS3_469_mg; + break; + + case LSM6DS3_500_mg: + *val = LSM6DS3_500_mg; + break; + + default: + *val = LSM6DS3_156_mg; + break; + } + + return ret; +} + +/** + * @brief Free-fall duration event. 1LSb = 1 / ODR[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of ff_dur in reg FREE_FALL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_ff_dur_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_free_fall_t free_fall; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FREE_FALL, (uint8_t *)&free_fall, 1); + + if (ret == 0) + { + free_fall.ff_dur = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_FREE_FALL, (uint8_t *)&free_fall, 1); + } + + return ret; +} + +/** + * @brief Free-fall duration event. 1LSb = 1 / ODR[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of ff_dur in reg FREE_FALL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_ff_dur_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_free_fall_t free_fall; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FREE_FALL, (uint8_t *)&free_fall, 1); + *val = (uint8_t)free_fall.ff_dur; + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3_Fifo + * @brief This section group all the functions concerning the + * fifo usage + * @{ + * + */ + +/** + * @brief FIFO watermark level selection.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of fth in reg FIFO_CTRL1 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_fifo_watermark_set(stmdev_ctx_t *ctx, uint16_t val) +{ + lsm6ds3_fifo_ctrl1_t fifo_ctrl1; + lsm6ds3_fifo_ctrl2_t fifo_ctrl2; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL1, + (uint8_t *)&fifo_ctrl1, 1); + + if (ret == 0) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL2, + (uint8_t *)&fifo_ctrl2, 1); + } + + if (ret == 0) + { + fifo_ctrl2.fth = (uint8_t)((val & 0x0F00U) >> 8); + ret = lsm6ds3_write_reg(ctx, LSM6DS3_FIFO_CTRL2, + (uint8_t *)&fifo_ctrl2, 1); + } + + if (ret == 0) + { + fifo_ctrl1.fth = (uint8_t)(val & 0x00FFU); + ret = lsm6ds3_write_reg(ctx, LSM6DS3_FIFO_CTRL1, + (uint8_t *)&fifo_ctrl1, 1); + } + + return ret; +} + +/** + * @brief FIFO watermark level selection.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of fth in reg FIFO_CTRL1 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_fifo_watermark_get(stmdev_ctx_t *ctx, uint16_t *val) +{ + lsm6ds3_fifo_ctrl1_t fifo_ctrl1; + lsm6ds3_fifo_ctrl2_t fifo_ctrl2; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL1, + (uint8_t *)&fifo_ctrl1, 1); + + if (ret == 0) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL2, + (uint8_t *)&fifo_ctrl2, 1); + *val = (uint16_t)fifo_ctrl2.fth << 8; + *val |= fifo_ctrl1.fth; + } + + return ret; +} + +/** + * @brief trigger signal for FIFO write operation.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of timer_pedo_fifo_drdy in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_fifo_write_trigger_set(stmdev_ctx_t *ctx, + lsm6ds3_tmr_ped_fifo_drdy_t val) +{ + lsm6ds3_fifo_ctrl2_t fifo_ctrl2; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL2, + (uint8_t *)&fifo_ctrl2, 1); + + if (ret == 0) + { + fifo_ctrl2. timer_pedo_fifo_drdy = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_FIFO_CTRL2, + (uint8_t *)&fifo_ctrl2, 1); + } + + return ret; +} + +/** + * @brief trigger signal for FIFO write operation.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of timer_pedo_fifo_drdy in + * reg FIFO_CTRL2 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_fifo_write_trigger_get(stmdev_ctx_t *ctx, + lsm6ds3_tmr_ped_fifo_drdy_t *val) +{ + lsm6ds3_fifo_ctrl2_t fifo_ctrl2; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL2, + (uint8_t *)&fifo_ctrl2, 1); + + switch (fifo_ctrl2. timer_pedo_fifo_drdy) + { + case LSM6DS3_TRG_XL_GY_DRDY: + *val = LSM6DS3_TRG_XL_GY_DRDY; + break; + + case LSM6DS3_TRG_STEP_DETECT: + *val = LSM6DS3_TRG_STEP_DETECT; + break; + + default: + *val = LSM6DS3_TRG_XL_GY_DRDY; + break; + } + + return ret; +} + +/** + * @brief Pedometer step counter and timestamp as 4th FIFO data set.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of timer_pedo_fifo_en in reg FIFO_CTRL2 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_fifo_pedo_batch_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_fifo_ctrl2_t fifo_ctrl2; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL2, + (uint8_t *)&fifo_ctrl2, 1); + + if (ret == 0) + { + fifo_ctrl2.timer_pedo_fifo_en = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_FIFO_CTRL2, + (uint8_t *)&fifo_ctrl2, 1); + } + + return ret; +} + +/** + * @brief Pedometer step counter and timestamp as 4th FIFO data set.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of timer_pedo_fifo_en in reg FIFO_CTRL2 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_fifo_pedo_batch_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_fifo_ctrl2_t fifo_ctrl2; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL2, + (uint8_t *)&fifo_ctrl2, 1); + *val = (uint8_t)fifo_ctrl2.timer_pedo_fifo_en; + + return ret; +} + +/** + * @brief Selects Batching Data Rate (writing frequency in FIFO) for + * accelerometer data.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of dec_fifo_xl in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_fifo_xl_batch_set(stmdev_ctx_t *ctx, + lsm6ds3_dec_fifo_xl_t val) +{ + lsm6ds3_fifo_ctrl3_t fifo_ctrl3; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL3, + (uint8_t *)&fifo_ctrl3, 1); + + if (ret == 0) + { + fifo_ctrl3.dec_fifo_xl = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_FIFO_CTRL3, + (uint8_t *)&fifo_ctrl3, 1); + } + + return ret; +} + +/** + * @brief Selects Batching Data Rate (writing frequency in FIFO) for + * accelerometer data.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of dec_fifo_xl in reg FIFO_CTRL3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_fifo_xl_batch_get(stmdev_ctx_t *ctx, + lsm6ds3_dec_fifo_xl_t *val) +{ + lsm6ds3_fifo_ctrl3_t fifo_ctrl3; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL3, + (uint8_t *)&fifo_ctrl3, 1); + + switch (fifo_ctrl3.dec_fifo_xl) + { + case LSM6DS3_FIFO_XL_DISABLE: + *val = LSM6DS3_FIFO_XL_DISABLE; + break; + + case LSM6DS3_FIFO_XL_NO_DEC: + *val = LSM6DS3_FIFO_XL_NO_DEC; + break; + + case LSM6DS3_FIFO_XL_DEC_2: + *val = LSM6DS3_FIFO_XL_DEC_2; + break; + + case LSM6DS3_FIFO_XL_DEC_3: + *val = LSM6DS3_FIFO_XL_DEC_3; + break; + + case LSM6DS3_FIFO_XL_DEC_4: + *val = LSM6DS3_FIFO_XL_DEC_4; + break; + + case LSM6DS3_FIFO_XL_DEC_8: + *val = LSM6DS3_FIFO_XL_DEC_8; + break; + + case LSM6DS3_FIFO_XL_DEC_16: + *val = LSM6DS3_FIFO_XL_DEC_16; + break; + + case LSM6DS3_FIFO_XL_DEC_32: + *val = LSM6DS3_FIFO_XL_DEC_32; + break; + + default: + *val = LSM6DS3_FIFO_XL_DISABLE; + break; + } + + return ret; +} + +/** + * @brief Selects Batching Data Rate (writing frequency in FIFO) + * for gyroscope data.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of dec_fifo_gyro in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_fifo_gy_batch_set(stmdev_ctx_t *ctx, + lsm6ds3_dec_fifo_gyro_t val) +{ + lsm6ds3_fifo_ctrl3_t fifo_ctrl3; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL3, + (uint8_t *)&fifo_ctrl3, 1); + + if (ret == 0) + { + fifo_ctrl3.dec_fifo_gyro = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_FIFO_CTRL3, + (uint8_t *)&fifo_ctrl3, 1); + } + + return ret; +} + +/** + * @brief Selects Batching Data Rate (writing frequency in FIFO) + * for gyroscope data.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of dec_fifo_gyro in reg FIFO_CTRL3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_fifo_gy_batch_get(stmdev_ctx_t *ctx, + lsm6ds3_dec_fifo_gyro_t *val) +{ + lsm6ds3_fifo_ctrl3_t fifo_ctrl3; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL3, + (uint8_t *)&fifo_ctrl3, 1); + + switch (fifo_ctrl3.dec_fifo_gyro) + { + case LSM6DS3_FIFO_GY_DISABLE: + *val = LSM6DS3_FIFO_GY_DISABLE; + break; + + case LSM6DS3_FIFO_GY_NO_DEC: + *val = LSM6DS3_FIFO_GY_NO_DEC; + break; + + case LSM6DS3_FIFO_GY_DEC_2: + *val = LSM6DS3_FIFO_GY_DEC_2; + break; + + case LSM6DS3_FIFO_GY_DEC_3: + *val = LSM6DS3_FIFO_GY_DEC_3; + break; + + case LSM6DS3_FIFO_GY_DEC_4: + *val = LSM6DS3_FIFO_GY_DEC_4; + break; + + case LSM6DS3_FIFO_GY_DEC_8: + *val = LSM6DS3_FIFO_GY_DEC_8; + break; + + case LSM6DS3_FIFO_GY_DEC_16: + *val = LSM6DS3_FIFO_GY_DEC_16; + break; + + case LSM6DS3_FIFO_GY_DEC_32: + *val = LSM6DS3_FIFO_GY_DEC_32; + break; + + default: + *val = LSM6DS3_FIFO_GY_DISABLE; + break; + } + + return ret; +} + +/** + * @brief Selects Batching Data Rate (writing frequency in FIFO) + * for third data set.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of dec_ds3_fifo in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_fifo_dataset_3_batch_set(stmdev_ctx_t *ctx, + lsm6ds3_dec_ds3_fifo_t val) +{ + lsm6ds3_fifo_ctrl4_t fifo_ctrl4; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL4, + (uint8_t *)&fifo_ctrl4, 1); + + if (ret == 0) + { + fifo_ctrl4.dec_ds3_fifo = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_FIFO_CTRL4, + (uint8_t *)&fifo_ctrl4, 1); + } + + return ret; +} + +/** + * @brief Selects Batching Data Rate (writing frequency in FIFO) + * for third data set.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of dec_ds3_fifo in reg FIFO_CTRL4 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_fifo_dataset_3_batch_get(stmdev_ctx_t *ctx, + lsm6ds3_dec_ds3_fifo_t *val) +{ + lsm6ds3_fifo_ctrl4_t fifo_ctrl4; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL4, + (uint8_t *)&fifo_ctrl4, 1); + + switch (fifo_ctrl4.dec_ds3_fifo) + { + case LSM6DS3_FIFO_DS3_DISABLE: + *val = LSM6DS3_FIFO_DS3_DISABLE; + break; + + case LSM6DS3_FIFO_DS3_NO_DEC: + *val = LSM6DS3_FIFO_DS3_NO_DEC; + break; + + case LSM6DS3_FIFO_DS3_DEC_2: + *val = LSM6DS3_FIFO_DS3_DEC_2; + break; + + case LSM6DS3_FIFO_DS3_DEC_3: + *val = LSM6DS3_FIFO_DS3_DEC_3; + break; + + case LSM6DS3_FIFO_DS3_DEC_4: + *val = LSM6DS3_FIFO_DS3_DEC_4; + break; + + case LSM6DS3_FIFO_DS3_DEC_8: + *val = LSM6DS3_FIFO_DS3_DEC_8; + break; + + case LSM6DS3_FIFO_DS3_DEC_16: + *val = LSM6DS3_FIFO_DS3_DEC_16; + break; + + case LSM6DS3_FIFO_DS3_DEC_32: + *val = LSM6DS3_FIFO_DS3_DEC_32; + break; + + default: + *val = LSM6DS3_FIFO_DS3_DISABLE; + break; + } + + return ret; +} + +/** + * @brief Selects Batching Data Rate (writing frequency in FIFO) + * for fourth data set.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of dec_ds4_fifo in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_fifo_dataset_4_batch_set(stmdev_ctx_t *ctx, + lsm6ds3_dec_ds4_fifo_t val) +{ + lsm6ds3_fifo_ctrl4_t fifo_ctrl4; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL4, + (uint8_t *)&fifo_ctrl4, 1); + + if (ret == 0) + { + fifo_ctrl4.dec_ds4_fifo = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_FIFO_CTRL4, + (uint8_t *)&fifo_ctrl4, 1); + } + + return ret; +} + +/** + * @brief Selects Batching Data Rate (writing frequency in FIFO) + * for fourth data set.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of dec_ds4_fifo in reg FIFO_CTRL4 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_fifo_dataset_4_batch_get(stmdev_ctx_t *ctx, + lsm6ds3_dec_ds4_fifo_t *val) +{ + lsm6ds3_fifo_ctrl4_t fifo_ctrl4; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL4, + (uint8_t *)&fifo_ctrl4, 1); + + switch (fifo_ctrl4.dec_ds4_fifo) + { + case LSM6DS3_FIFO_DS4_DISABLE: + *val = LSM6DS3_FIFO_DS4_DISABLE; + break; + + case LSM6DS3_FIFO_DS4_NO_DEC: + *val = LSM6DS3_FIFO_DS4_NO_DEC; + break; + + case LSM6DS3_FIFO_DS4_DEC_2: + *val = LSM6DS3_FIFO_DS4_DEC_2; + break; + + case LSM6DS3_FIFO_DS4_DEC_3: + *val = LSM6DS3_FIFO_DS4_DEC_3; + break; + + case LSM6DS3_FIFO_DS4_DEC_4: + *val = LSM6DS3_FIFO_DS4_DEC_4; + break; + + case LSM6DS3_FIFO_DS4_DEC_8: + *val = LSM6DS3_FIFO_DS4_DEC_8; + break; + + case LSM6DS3_FIFO_DS4_DEC_16: + *val = LSM6DS3_FIFO_DS4_DEC_16; + break; + + case LSM6DS3_FIFO_DS4_DEC_32: + *val = LSM6DS3_FIFO_DS4_DEC_32; + break; + + default: + *val = LSM6DS3_FIFO_DS4_DISABLE; + break; + } + + return ret; +} + +/** + * @brief 8-bit data storage in FIFO.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of only_high_data in reg FIFO_CTRL4 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_fifo_xl_gy_8bit_format_set(stmdev_ctx_t *ctx, + uint8_t val) +{ + lsm6ds3_fifo_ctrl4_t fifo_ctrl4; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL4, + (uint8_t *)&fifo_ctrl4, 1); + + if (ret == 0) + { + fifo_ctrl4.only_high_data = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_FIFO_CTRL4, + (uint8_t *)&fifo_ctrl4, 1); + } + + return ret; +} + +/** + * @brief 8-bit data storage in FIFO.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of only_high_data in reg FIFO_CTRL4 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_fifo_xl_gy_8bit_format_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3_fifo_ctrl4_t fifo_ctrl4; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL4, + (uint8_t *)&fifo_ctrl4, 1); + *val = (uint8_t)fifo_ctrl4.only_high_data; + + return ret; +} + +/** + * @brief FIFO mode selection.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of fifo_mode in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_fifo_mode_set(stmdev_ctx_t *ctx, + lsm6ds3_fifo_md_t val) +{ + lsm6ds3_fifo_ctrl5_t fifo_ctrl5; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL5, + (uint8_t *)&fifo_ctrl5, 1); + + if (ret == 0) + { + fifo_ctrl5.fifo_mode = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_FIFO_CTRL5, + (uint8_t *)&fifo_ctrl5, 1); + } + + return ret; +} + +/** + * @brief FIFO mode selection.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of fifo_mode in reg FIFO_CTRL5 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_fifo_mode_get(stmdev_ctx_t *ctx, + lsm6ds3_fifo_md_t *val) +{ + lsm6ds3_fifo_ctrl5_t fifo_ctrl5; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL5, + (uint8_t *)&fifo_ctrl5, 1); + + switch (fifo_ctrl5.fifo_mode) + { + case LSM6DS3_BYPASS_MODE: + *val = LSM6DS3_BYPASS_MODE; + break; + + case LSM6DS3_FIFO_MODE: + *val = LSM6DS3_FIFO_MODE; + break; + + case LSM6DS3_STREAM_TO_FIFO_MODE: + *val = LSM6DS3_STREAM_TO_FIFO_MODE; + break; + + case LSM6DS3_BYPASS_TO_STREAM_MODE: + *val = LSM6DS3_BYPASS_TO_STREAM_MODE; + break; + + default: + *val = LSM6DS3_BYPASS_MODE; + break; + } + + return ret; +} + +/** + * @brief FIFO ODR selection, setting FIFO_MODE also.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of odr_fifo in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_fifo_data_rate_set(stmdev_ctx_t *ctx, + lsm6ds3_odr_fifo_t val) +{ + lsm6ds3_fifo_ctrl5_t fifo_ctrl5; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL5, + (uint8_t *)&fifo_ctrl5, 1); + + if (ret == 0) + { + fifo_ctrl5.odr_fifo = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_FIFO_CTRL5, + (uint8_t *)&fifo_ctrl5, 1); + } + + return ret; +} + +/** + * @brief FIFO ODR selection, setting FIFO_MODE also.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of odr_fifo in reg FIFO_CTRL5 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_fifo_data_rate_get(stmdev_ctx_t *ctx, + lsm6ds3_odr_fifo_t *val) +{ + lsm6ds3_fifo_ctrl5_t fifo_ctrl5; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_CTRL5, + (uint8_t *)&fifo_ctrl5, 1); + + switch (fifo_ctrl5.odr_fifo) + { + case LSM6DS3_FIFO_DISABLE: + *val = LSM6DS3_FIFO_DISABLE; + break; + + case LSM6DS3_FIFO_12Hz5: + *val = LSM6DS3_FIFO_12Hz5; + break; + + case LSM6DS3_FIFO_26Hz: + *val = LSM6DS3_FIFO_26Hz; + break; + + case LSM6DS3_FIFO_52Hz: + *val = LSM6DS3_FIFO_52Hz; + break; + + case LSM6DS3_FIFO_104Hz: + *val = LSM6DS3_FIFO_104Hz; + break; + + case LSM6DS3_FIFO_208Hz: + *val = LSM6DS3_FIFO_208Hz; + break; + + case LSM6DS3_FIFO_416Hz: + *val = LSM6DS3_FIFO_416Hz; + break; + + case LSM6DS3_FIFO_833Hz: + *val = LSM6DS3_FIFO_833Hz; + break; + + case LSM6DS3_FIFO_1k66Hz: + *val = LSM6DS3_FIFO_1k66Hz; + break; + + case LSM6DS3_FIFO_3k33Hz: + *val = LSM6DS3_FIFO_3k33Hz; + break; + + case LSM6DS3_FIFO_6k66Hz: + *val = LSM6DS3_FIFO_6k66Hz; + break; + + default: + *val = LSM6DS3_FIFO_DISABLE; + break; + } + + return ret; +} + +/** + * @brief Sensing chain FIFO stop values memorization at + * threshold level.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of stop_on_fth in reg CTRL4_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_fifo_stop_on_wtm_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_ctrl4_c_t ctrl4_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + + if (ret == 0) + { + ctrl4_c.stop_on_fth = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + } + + return ret; +} + +/** + * @brief Sensing chain FIFO stop values memorization at + * threshold level.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of stop_on_fth in reg CTRL4_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_fifo_stop_on_wtm_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_ctrl4_c_t ctrl4_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + *val = (uint8_t)ctrl4_c.stop_on_fth; + + return ret; +} + +/** + * @brief batching of temperature data.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of fifo_temp_en in reg CTRL4_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_fifo_temp_batch_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_ctrl4_c_t ctrl4_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + + if (ret == 0) + { + ctrl4_c.fifo_temp_en = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + } + + return ret; +} + +/** + * @brief batching of temperature data.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of fifo_temp_en in reg CTRL4_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_fifo_temp_batch_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_ctrl4_c_t ctrl4_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + *val = (uint8_t)ctrl4_c.fifo_temp_en; + + return ret; +} + +/** + * @brief Number of unread words (16-bit axes) stored in FIFO.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of diff_fifo in reg FIFO_STATUS1 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_fifo_data_level_get(stmdev_ctx_t *ctx, uint16_t *val) +{ + lsm6ds3_fifo_status1_t fifo_status1; + lsm6ds3_fifo_status2_t fifo_status2; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_STATUS1, + (uint8_t *)&fifo_status1, 1); + + if (ret == 0) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_STATUS2, + (uint8_t *)&fifo_status2, 1); + *val = (uint16_t)fifo_status2.diff_fifo << 8; + *val |= fifo_status1.diff_fifo; + } + + return ret; +} + +/** + * @brief Smart FIFO full status.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of fifo_empty in reg FIFO_STATUS2 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_fifo_full_flag_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_fifo_status2_t fifo_status2; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_STATUS2, + (uint8_t *)&fifo_status2, 1); + *val = (uint8_t)fifo_status2.fifo_empty; + + return ret; +} + +/** + * @brief FIFO overrun status.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of fifo_full in reg FIFO_STATUS2 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_fifo_ovr_flag_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_fifo_status2_t fifo_status2; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_STATUS2, + (uint8_t *)&fifo_status2, 1); + *val = (uint8_t)fifo_status2.fifo_full; + + return ret; +} + +/** + * @brief FIFO watermark status.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of fth in reg FIFO_STATUS2 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_fifo_wtm_flag_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_fifo_status2_t fifo_status2; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_STATUS2, + (uint8_t *)&fifo_status2, 1); + *val = (uint8_t)fifo_status2.fth; + + return ret; +} + +/** + * @brief Word of recursive pattern read at the next reading.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of fifo_pattern in reg FIFO_STATUS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_fifo_pattern_get(stmdev_ctx_t *ctx, uint16_t *val) +{ + lsm6ds3_fifo_status3_t fifo_status3; + lsm6ds3_fifo_status4_t fifo_status4; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_STATUS3, + (uint8_t *)&fifo_status3, 1); + + if (ret == 0) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FIFO_STATUS4, + (uint8_t *)&fifo_status4, 1); + *val = (uint16_t)fifo_status4.fifo_pattern << 8; + *val |= fifo_status3.fifo_pattern; + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3_DEN_functionality + * @brief This section groups all the functions concerning DEN + * functionality. + * @{ + * + */ + +/** + * @brief DEN functionality marking mode.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of den_mode in reg CTRL6_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_den_mode_set(stmdev_ctx_t *ctx, + lsm6ds3_den_mode_t val) +{ + lsm6ds3_ctrl6_c_t ctrl6_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL6_C, (uint8_t *)&ctrl6_c, 1); + + if (ret == 0) + { + ctrl6_c.den_mode = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL6_C, (uint8_t *)&ctrl6_c, 1); + } + + return ret; +} + +/** + * @brief DEN functionality marking mode.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of den_mode in reg CTRL6_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_den_mode_get(stmdev_ctx_t *ctx, + lsm6ds3_den_mode_t *val) +{ + lsm6ds3_ctrl6_c_t ctrl6_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL6_C, (uint8_t *)&ctrl6_c, 1); + + switch (ctrl6_c.den_mode) + { + case LSM6DS3_DEN_DISABLE: + *val = LSM6DS3_DEN_DISABLE; + break; + + case LSM6DS3_LEVEL_FIFO: + *val = LSM6DS3_LEVEL_FIFO; + break; + + case LSM6DS3_LEVEL_LETCHED: + *val = LSM6DS3_LEVEL_LETCHED; + break; + + case LSM6DS3_LEVEL_TRIGGER: + *val = LSM6DS3_LEVEL_TRIGGER; + break; + + case LSM6DS3_EDGE_TRIGGER: + *val = LSM6DS3_EDGE_TRIGGER; + break; + + default: + *val = LSM6DS3_DEN_DISABLE; + break; + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3_Pedometer + * @brief This section groups all the functions that manage pedometer. + * @{ + * + */ + +/** + * @brief Reset pedometer step counter.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of pedo_rst_step in reg CTRL10_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_pedo_step_reset_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_ctrl10_c_t ctrl10_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t *)&ctrl10_c, 1); + + if (ret == 0) + { + ctrl10_c.pedo_rst_step = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t *)&ctrl10_c, 1); + } + + return ret; +} + +/** + * @brief Reset pedometer step counter.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of pedo_rst_step in reg CTRL10_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_pedo_step_reset_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_ctrl10_c_t ctrl10_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t *)&ctrl10_c, 1); + *val = (uint8_t)ctrl10_c.pedo_rst_step; + + return ret; +} + +/** + * @brief Step counter timestamp information register (r). When a step is + * detected, the value of TIMESTAMP_REG register is copied in + * STEP_TIMESTAMP_L.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param buff buffer that stores data read + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_pedo_timestamp_raw_get(stmdev_ctx_t *ctx, + uint16_t *val) +{ + uint8_t buff[2]; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_STEP_TIMESTAMP_L, buff, 2); + *val = buff[1]; + *val = (*val * 256U) + buff[0]; + + return ret; +} + +/** + * @brief Step detector event detection status + * (0:not detected / 1:detected).[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of step_detected in reg FUNC_SRC + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_pedo_step_detect_flag_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3_func_src_t func_src; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FUNC_SRC, (uint8_t *)&func_src, 1); + *val = (uint8_t)func_src.step_detected; + + return ret; +} + +/** + * @brief Enable pedometer algorithm.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of pedo_en in reg TAP_CFG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_pedo_sens_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_ctrl10_c_t ctrl10_c; + lsm6ds3_tap_cfg_t tap_cfg; + int32_t ret; + + ret = 0; + + if (val == PROPERTY_ENABLE) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t *)&ctrl10_c, 1); + + if (ret == 0) + { + ctrl10_c.func_en = PROPERTY_ENABLE; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t *)&ctrl10_c, 1); + } + } + + if (ret == 0) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t *)&tap_cfg, 1); + } + + if (ret == 0) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t *)&tap_cfg, 1); + } + + if (ret == 0) + { + tap_cfg.pedo_en = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t *)&tap_cfg, 1); + } + + return ret; +} + +/** + * @brief Enable pedometer algorithm.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of pedo_en in reg TAP_CFG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_pedo_sens_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_tap_cfg_t tap_cfg; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t *)&tap_cfg, 1); + *val = (uint8_t)tap_cfg.pedo_en; + + return ret; +} + +/** + * @brief Configurable minimum threshold (PEDO_4G 1LSB = 16 mg , + * PEDO_2G 1LSB = 32 mg).[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of ths_min in reg PEDO_THS_REG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_pedo_threshold_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_pedo_ths_reg_t pedo_ths_reg; + int32_t ret; + + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); + + if (ret == 0) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_PEDO_THS_REG, + (uint8_t *)&pedo_ths_reg, 1); + } + + if (ret == 0) + { + pedo_ths_reg.ths_min = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_PEDO_THS_REG, + (uint8_t *)&pedo_ths_reg, 1); + } + + if (ret == 0) + { + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); + } + + return ret; +} + +/** + * @brief Configurable minimum threshold (PEDO_4G 1LSB = 16 mg, + * PEDO_2G 1LSB = 32 mg).[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of ths_min in reg PEDO_THS_REG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_pedo_threshold_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_pedo_ths_reg_t pedo_ths_reg; + int32_t ret; + + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); + + if (ret == 0) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_PEDO_THS_REG, + (uint8_t *)&pedo_ths_reg, 1); + } + + if (ret == 0) + { + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); + *val = (uint8_t)pedo_ths_reg.ths_min; + } + + return ret; +} + +/** + * @brief This bit sets the internal full scale used in + * pedometer functions.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of pedo_4g in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_pedo_full_scale_set(stmdev_ctx_t *ctx, + lsm6ds3_pedo_fs_t val) +{ + lsm6ds3_pedo_ths_reg_t pedo_ths_reg; + int32_t ret; + + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); + + if (ret == 0) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_PEDO_THS_REG, + (uint8_t *)&pedo_ths_reg, 1); + } + + if (ret == 0) + { + pedo_ths_reg.pedo_4g = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_PEDO_THS_REG, + (uint8_t *)&pedo_ths_reg, 1); + } + + if (ret == 0) + { + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); + } + + return ret; +} + +/** + * @brief This bit sets the internal full scale used in pedometer + * functions.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of pedo_4g in reg PEDO_THS_REG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_pedo_full_scale_get(stmdev_ctx_t *ctx, + lsm6ds3_pedo_fs_t *val) +{ + lsm6ds3_pedo_ths_reg_t pedo_ths_reg; + int32_t ret; + + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); + + if (ret == 0) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_PEDO_THS_REG, + (uint8_t *)&pedo_ths_reg, 1); + + switch (pedo_ths_reg.pedo_4g) + { + case LSM6DS3_PEDO_AT_2g: + *val = LSM6DS3_PEDO_AT_2g; + break; + + case LSM6DS3_PEDO_AT_4g: + *val = LSM6DS3_PEDO_AT_4g; + break; + + default: + *val = LSM6DS3_PEDO_AT_2g; + break; + } + } + + if (ret == 0) + { + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); + } + + return ret; +} + +/** + * @brief Pedometer debounce configuration register (r/w).[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of deb_step in reg PEDO_DEB_REG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_pedo_debounce_steps_set(stmdev_ctx_t *ctx, + uint8_t val) +{ + lsm6ds3_pedo_deb_reg_t pedo_deb_reg; + int32_t ret; + + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); + + if (ret == 0) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_PEDO_DEB_REG, + (uint8_t *)&pedo_deb_reg, 1); + } + + if (ret == 0) + { + pedo_deb_reg.deb_step = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_PEDO_DEB_REG, + (uint8_t *)&pedo_deb_reg, 1); + } + + if (ret == 0) + { + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); + } + + return ret; +} + +/** + * @brief Pedometer debounce configuration register (r/w).[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of deb_step in reg PEDO_DEB_REG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_pedo_debounce_steps_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3_pedo_deb_reg_t pedo_deb_reg; + int32_t ret; + + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); + + if (ret == 0) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_PEDO_DEB_REG, + (uint8_t *)&pedo_deb_reg, 1); + } + + if (ret == 0) + { + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); + *val = (uint8_t)pedo_deb_reg.deb_step; + } + + return ret; +} + +/** + * @brief Debounce time. If the time between two consecutive steps is + * greater than DEB_TIME*80ms, the debounce is reactivated.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of deb_time in reg PEDO_DEB_REG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_pedo_timeout_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_pedo_deb_reg_t pedo_deb_reg; + int32_t ret; + + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); + + if (ret == 0) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_PEDO_DEB_REG, + (uint8_t *)&pedo_deb_reg, 1); + } + + if (ret == 0) + { + pedo_deb_reg.deb_time = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_PEDO_DEB_REG, + (uint8_t *)&pedo_deb_reg, 1); + } + + if (ret == 0) + { + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); + } + + return ret; +} + +/** + * @brief Debounce time. If the time between two consecutive steps is + * greater than DEB_TIME*80ms, the debounce is reactivated.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of deb_time in reg PEDO_DEB_REG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_pedo_timeout_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_pedo_deb_reg_t pedo_deb_reg; + int32_t ret; + + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); + + if (ret == 0) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_PEDO_DEB_REG, + (uint8_t *)&pedo_deb_reg, 1); + } + + if (ret == 0) + { + *val = (uint8_t)pedo_deb_reg.deb_time; + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3_Significant_motion + * @brief This section groups all the functions that manage the + * significant motion detection. + * @{ + * + */ + +/** + * @brief Enable significant motion detection function.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of sign_motion_en in reg CTRL10_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_motion_sens_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_ctrl10_c_t ctrl10_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t *)&ctrl10_c, 1); + + if (ret == 0) + { + ctrl10_c.sign_motion_en = (uint8_t)val; + + if (val == PROPERTY_ENABLE) + { + ctrl10_c.func_en = PROPERTY_ENABLE; + } + + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t *)&ctrl10_c, 1); + } + + return ret; +} + +/** + * @brief Enable significant motion detection function.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of sign_motion_en in reg CTRL10_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_motion_sens_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_ctrl10_c_t ctrl10_c; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t *)&ctrl10_c, 1); + *val = (uint8_t)ctrl10_c.sign_motion_en; + + return ret; +} + +/** + * @brief Significant motion event detection status + * (0:not detected / 1:detected).[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of sign_motion_ia in reg FUNC_SRC + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_motion_event_flag_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_func_src_t func_src; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FUNC_SRC, (uint8_t *)&func_src, 1); + *val = (uint8_t)func_src.sign_motion_ia; + + return ret; +} + +/** + * @brief Significant motion threshold.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of sm_ths in reg SM_THS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_motion_threshold_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_sm_ths_t sm_ths; + int32_t ret; + + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); + + if (ret == 0) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_SM_THS, (uint8_t *)&sm_ths, 1); + } + + if (ret == 0) + { + sm_ths.sm_ths = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_SM_THS, (uint8_t *)&sm_ths, 1); + } + + if (ret == 0) + { + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); + } + + return ret; +} + +/** + * @brief Significant motion threshold.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of sm_ths in reg SM_THS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_motion_threshold_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_sm_ths_t sm_ths; + int32_t ret; + + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); + + if (ret == 0) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_SM_THS, (uint8_t *)&sm_ths, 1); + } + + if (ret == 0) + { + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); + *val = (uint8_t)sm_ths.sm_ths; + } + + return ret; +} + +/** + * @brief Time period register for step detection on delta time + * (1LSB = 1.6384 s).[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of sc_delta in reg STEP_COUNT_DELTA + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_sc_delta_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_step_count_delta_t step_count_delta; + int32_t ret; + + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); + + if (ret == 0) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_STEP_COUNT_DELTA, + (uint8_t *)& step_count_delta, 1); + } + + if (ret == 0) + { + step_count_delta.sc_delta = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_STEP_COUNT_DELTA, + (uint8_t *)& step_count_delta, 1); + } + + if (ret == 0) + { + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); + } + + return ret; +} + +/** + * @brief Time period register for step detection on delta time + * (1LSB = 1.6384 s).[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of sc_delta in reg STEP_COUNT_DELTA + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_sc_delta_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_step_count_delta_t step_count_delta; + int32_t ret; + + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); + + if (ret == 0) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_STEP_COUNT_DELTA, + (uint8_t *)& step_count_delta, 1); + } + + if (ret == 0) + { + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); + *val = (uint8_t) step_count_delta.sc_delta; + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3_Tilt_detection + * @brief This section groups all the functions that manage + * the tilt event detection. + * @{ + * + */ + +/** + * @brief Tilt event detection status(0:not detected / 1:detected).[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of tilt_ia in reg FUNC_SRC + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_tilt_event_flag_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_func_src_t func_src; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FUNC_SRC, (uint8_t *)&func_src, 1); + *val = (uint8_t)func_src.tilt_ia; + + return ret; +} + +/** + * @brief Enable tilt calculation.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of tilt_en in reg TAP_CFG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_tilt_sens_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_ctrl10_c_t ctrl10_c; + lsm6ds3_tap_cfg_t tap_cfg; + int32_t ret; + + ret = 0; + + if (val == PROPERTY_ENABLE) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t *)&ctrl10_c, 1); + + if (ret == 0) + { + ctrl10_c.func_en = PROPERTY_ENABLE; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t *)&ctrl10_c, 1); + } + } + + if (ret == 0) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t *)&tap_cfg, 1); + } + + if (ret == 0) + { + tap_cfg.tilt_en = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t *)&tap_cfg, 1); + } + + return ret; +} + +/** + * @brief Enable tilt calculation.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of tilt_en in reg TAP_CFG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_tilt_sens_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_tap_cfg_t tap_cfg; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t *)&tap_cfg, 1); + *val = (uint8_t)tap_cfg.tilt_en; + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3_Magnetometer_sensor + * @brief This section groups all the functions that manage + * additional magnetometer sensor. + * @{ + * + */ + +/** + * @brief Enable soft-iron correction algorithm for magnetometer.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of soft_en in reg CTRL9_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_mag_soft_iron_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_ctrl10_c_t ctrl10_c; + lsm6ds3_ctrl9_xl_t ctrl9_xl; + int32_t ret; + + ret = 0; + + if (val == PROPERTY_ENABLE) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t *)&ctrl10_c, 1); + + if (ret == 0) + { + ctrl10_c.func_en = PROPERTY_ENABLE; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t *)&ctrl10_c, 1); + } + } + + if (ret == 0) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL9_XL, (uint8_t *)&ctrl9_xl, 1); + } + + if (ret == 0) + { + ctrl9_xl.soft_en = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL9_XL, (uint8_t *)&ctrl9_xl, 1); + } + + return ret; +} + +/** + * @brief Enable soft-iron correction algorithm for magnetometer.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of soft_en in reg CTRL9_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_mag_soft_iron_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_ctrl9_xl_t ctrl9_xl; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL9_XL, (uint8_t *)&ctrl9_xl, 1); + *val = (uint8_t)ctrl9_xl.soft_en; + + return ret; +} + +/** + * @brief Enable hard-iron correction algorithm for magnetometer.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of iron_en in reg MASTER_CONFIG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_mag_hard_iron_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_ctrl10_c_t ctrl10_c; + lsm6ds3_master_config_t master_config; + int32_t ret; + + ret = 0; + + if (val == PROPERTY_ENABLE) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t *)&ctrl10_c, 1); + ctrl10_c.func_en = PROPERTY_ENABLE; + + if (ret == 0) + { + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t *)&ctrl10_c, 1); + } + } + + if (ret == 0) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + } + + if (ret == 0) + { + master_config.iron_en = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + } + + return ret; +} + +/** + * @brief Enable hard-iron correction algorithm for magnetometer.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of iron_en in reg MASTER_CONFIG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_mag_hard_iron_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_master_config_t master_config; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + *val = (uint8_t)master_config.iron_en; + + return ret; +} + +/** + * @brief Hard/soft-iron calculation status (0: on-going / 1: idle).[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of si_end_op in reg FUNC_SRC + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_mag_soft_iron_end_op_flag_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3_func_src_t func_src; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FUNC_SRC, (uint8_t *)&func_src, 1); + *val = (uint8_t)func_src.si_end_op; + + return ret; +} + +/** + * @brief Soft-iron matrix correction registers[set] + * + * @param ctx read / write interface definitions(ptr) + * @param buff buffer that stores data to be write + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_mag_soft_iron_coeff_set(stmdev_ctx_t *ctx, + uint8_t *buff) +{ + int32_t ret; + + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); + + if (ret == 0) + { + ret = lsm6ds3_write_reg(ctx, LSM6DS3_MAG_SI_XX, buff, 9); + } + + if (ret == 0) + { + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); + } + + return ret; +} + +/** + * @brief Soft-iron matrix correction registers[get] + * + * @param ctx read / write interface definitions(ptr) + * @param buff buffer that stores data read + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_mag_soft_iron_coeff_get(stmdev_ctx_t *ctx, + uint8_t *buff) +{ + int32_t ret; + + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); + + if (ret == 0) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_MAG_SI_XX, buff, 9); + } + + if (ret == 0) + { + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); + } + + return ret; +} + +/** + * @brief Offset for hard-iron compensation register (r/w). + * The value is expressed as a 16-bit word in two’s complement.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param buff buffer that stores data to be write + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_mag_offset_set(stmdev_ctx_t *ctx, int16_t *val) +{ + uint8_t buff[6]; + int32_t ret; + + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); + + if (ret == 0) + { + buff[1] = (uint8_t)((uint16_t)val[0] / 256U); + buff[0] = (uint8_t)((uint16_t)val[0] - (buff[1] * 256U)); + buff[3] = (uint8_t)((uint16_t)val[1] / 256U); + buff[2] = (uint8_t)((uint16_t)val[1] - (buff[3] * 256U)); + buff[5] = (uint8_t)((uint16_t)val[2] / 256U); + buff[4] = (uint8_t)((uint16_t)val[2] - (buff[5] * 256U)); + ret = lsm6ds3_write_reg(ctx, LSM6DS3_MAG_OFFX_L, buff, 6); + } + + if (ret == 0) + { + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); + } + + return ret; +} + +/** + * @brief Offset for hard-iron compensation register(r/w). + * The value is expressed as a 16-bit word in two’s complement.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param buff buffer that stores data read + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_mag_offset_get(stmdev_ctx_t *ctx, int16_t *val) +{ + uint8_t buff[6]; + int32_t ret; + + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); + + if (ret == 0) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_MAG_OFFX_L, buff, 6); + } + + if (ret == 0) + { + 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]; + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3_Sensor_hub + * @brief This section groups all the functions that manage the + * sensor hub functionality. + * @{ + * + */ + +/** + * @brief Sensor synchronization time frame with the step of 500 ms + * and full range of 5 s. Unsigned 8-bit.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of tph in reg SENSOR_SYNC_TIME_FRAME + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_sh_sync_sens_frame_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_sensor_sync_time_frame_t sensor_sync_time_frame; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_SENSOR_SYNC_TIME_FRAME, + (uint8_t *)& sensor_sync_time_frame, 1); + + if (ret == 0) + { + sensor_sync_time_frame.tph = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_SENSOR_SYNC_TIME_FRAME, + (uint8_t *)& sensor_sync_time_frame, 1); + } + + return ret; +} + +/** + * @brief Sensor synchronization time frame with the step of 500 ms and + * full range of 5 s. Unsigned 8-bit.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of tph in reg SENSOR_SYNC_TIME_FRAME + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_sh_sync_sens_frame_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3_sensor_sync_time_frame_t sensor_sync_time_frame; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_SENSOR_SYNC_TIME_FRAME, + (uint8_t *)& sensor_sync_time_frame, 1); + *val = (uint8_t) sensor_sync_time_frame.tph; + + return ret; +} +/** + * @brief Sensor hub I2C master enable.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of master_on in reg MASTER_CONFIG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_sh_master_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_ctrl10_c_t ctrl10_c; + lsm6ds3_master_config_t master_config; + int32_t ret; + + ret = 0; + + if (val == PROPERTY_ENABLE) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t *)&ctrl10_c, 1); + + if (ret == 0) + { + ctrl10_c.func_en = PROPERTY_ENABLE; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_CTRL10_C, (uint8_t *)&ctrl10_c, 1); + } + } + + if (ret == 0) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + } + + if (ret == 0) + { + master_config.master_on = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + } + + return ret; +} + +/** + * @brief Sensor hub I2C master enable.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of master_on in reg MASTER_CONFIG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_sh_master_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_master_config_t master_config; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + *val = (uint8_t)master_config.master_on; + + return ret; +} +/** + * @brief I2C interface pass-through.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of pass_through_mode in + * reg MASTER_CONFIG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_sh_pass_through_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3_master_config_t master_config; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + + if (ret == 0) + { + master_config.pass_through_mode = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + } + + return ret; +} + +/** + * @brief I2C interface pass-through.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of pass_through_mode in reg MASTER_CONFIG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_sh_pass_through_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_master_config_t master_config; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + *val = (uint8_t)master_config.pass_through_mode; + + return ret; +} +/** + * @brief Master I2C pull-up enable/disable.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of pull_up_en in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_sh_pin_mode_set(stmdev_ctx_t *ctx, + lsm6ds3_sh_pin_md_t val) +{ + lsm6ds3_master_config_t master_config; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + + if (ret == 0) + { + master_config.pull_up_en = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + } + + return ret; +} + +/** + * @brief Master I2C pull-up enable/disable.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of pull_up_en in reg MASTER_CONFIG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_sh_pin_mode_get(stmdev_ctx_t *ctx, + lsm6ds3_sh_pin_md_t *val) +{ + lsm6ds3_master_config_t master_config; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + + switch (master_config.pull_up_en) + { + case LSM6DS3_EXT_PULL_UP: + *val = LSM6DS3_EXT_PULL_UP; + break; + + case LSM6DS3_INTERNAL_PULL_UP: + *val = LSM6DS3_INTERNAL_PULL_UP; + break; + + default: + *val = LSM6DS3_EXT_PULL_UP; + break; + } + + return ret; +} + +/** + * @brief Sensor hub trigger signal selection.[set] + * + * @param ctx read / write interface definitions(ptr) + * @param val change the values of start_config in reg LSM6DS3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_sh_syncro_mode_set(stmdev_ctx_t *ctx, + lsm6ds3_start_cfg_t val) +{ + lsm6ds3_master_config_t master_config; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + + if (ret == 0) + { + master_config.start_config = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + } + + return ret; +} + +/** + * @brief Sensor hub trigger signal selection.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of start_config in reg MASTER_CONFIG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_sh_syncro_mode_get(stmdev_ctx_t *ctx, + lsm6ds3_start_cfg_t *val) +{ + lsm6ds3_master_config_t master_config; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + + switch (master_config.start_config) + { + case LSM6DS3_XL_GY_DRDY: + *val = LSM6DS3_XL_GY_DRDY; + break; + + case LSM6DS3_EXT_ON_INT2_PIN: + *val = LSM6DS3_EXT_ON_INT2_PIN; + break; + + default: + *val = LSM6DS3_XL_GY_DRDY; + break; + } + + return ret; +} + +/** + * @brief Sensor hub output registers.[get] + * + * @param ctx read / write interface definitions(ptr) + * @param buff buffer that stores data read + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_sh_read_data_raw_get(stmdev_ctx_t *ctx, + lsm6ds3_sh_read_t *buff) +{ + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_SENSORHUB1_REG, + (uint8_t *) & (buff->sh_byte_1), 12); + + if (ret == 0) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_SENSORHUB13_REG, + (uint8_t *) & (buff->sh_byte_13), 6); + } + + return ret; +} + +/** + * @brief sh_cfg_write: Configure slave 0 for perform a write. + * + * @param stmdev_ctx_t *ctx: read / write interface definitions + * @param lsm6ds3_sh_cfg_write_t: 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 lsm6ds3_sh_cfg_write(stmdev_ctx_t *ctx, + lsm6ds3_sh_cfg_write_t *val) +{ + lsm6ds3_slv0_add_t slv0_add; + int32_t ret; + + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); + + if (ret == 0) + { + slv0_add.slave0_add = val->slv0_add >> 1; + slv0_add.rw_0 = 0; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_SLV0_ADD, (uint8_t *)&slv0_add, 1); + } + + if (ret == 0) + { + ret = lsm6ds3_write_reg(ctx, LSM6DS3_SLV0_SUBADD, + &(val->slv0_subadd), 1); + } + + if (ret == 0) + { + ret = lsm6ds3_write_reg(ctx, LSM6DS3_DATAWRITE_SRC_MODE_SUB_SLV0, + &(val->slv0_data), 1); + } + + if (ret == 0) + { + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); + } + + return ret; +} + +/** + * @brief sh_slv0_cfg_read: [get] Configure slave 0 for perform a write/read. + * + * @param stmdev_ctx_t *ctx: read / write interface definitions + * @param lsm6ds3_sh_cfg_read_t: 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_len; num of bit to read + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_sh_slv0_cfg_read(stmdev_ctx_t *ctx, + lsm6ds3_sh_cfg_read_t *val) +{ + lsm6ds3_slv0_add_t slv0_add; + lsm6ds3_slave0_config_t slave0_config; + int32_t ret; + + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); + + if (ret == 0) + { + slv0_add.slave0_add = val->slv_add >> 1; + slv0_add.rw_0 = 1; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_SLV0_ADD, (uint8_t *)&slv0_add, 1); + } + + if (ret == 0) + { + ret = lsm6ds3_write_reg(ctx, LSM6DS3_SLV0_SUBADD, + &(val->slv_subadd), 1); + } + + if (ret == 0) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_SLAVE0_CONFIG, + (uint8_t *)&slave0_config, 1); + } + + if (ret == 0) + { + slave0_config.slave0_numop = val->slv_len; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_SLAVE0_CONFIG, + (uint8_t *)&slave0_config, 1); + } + + if (ret == 0) + { + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); + } + + return ret; +} + +/** + * @brief sh_slv1_cfg_read: [get] Configure slave 0 for perform a write/read. + * + * @param stmdev_ctx_t *ctx: read / write interface definitions + * @param lsm6ds3_sh_cfg_read_t: 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_len; num of bit to read + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_sh_slv1_cfg_read(stmdev_ctx_t *ctx, + lsm6ds3_sh_cfg_read_t *val) +{ + lsm6ds3_slv1_add_t slv1_add; + lsm6ds3_slave1_config_t slave1_config; + int32_t ret; + + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); + + if (ret == 0) + { + slv1_add.slave1_add = val->slv_add >> 1;; + slv1_add.r_1 = 1; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_SLV1_ADD, (uint8_t *)&slv1_add, 1); + } + + if (ret == 0) + { + ret = lsm6ds3_write_reg(ctx, LSM6DS3_SLV1_SUBADD, + &(val->slv_subadd), 1); + } + + if (ret == 0) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_SLAVE1_CONFIG, + (uint8_t *)&slave1_config, 1); + } + + if (ret == 0) + { + slave1_config.slave1_numop = val->slv_len; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_SLAVE1_CONFIG, + (uint8_t *)&slave1_config, 1); + } + + if (ret == 0) + { + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); + } + + return ret; +} + +/** + * @brief sh_slv2_cfg_read: [get] Configure slave 0 for perform a write/read. + * + * @param stmdev_ctx_t *ctx: read / write interface definitions + * @param lsm6ds3_sh_cfg_read_t: a 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 lsm6ds3_sh_slv2_cfg_read(stmdev_ctx_t *ctx, + lsm6ds3_sh_cfg_read_t *val) +{ + lsm6ds3_slv2_add_t slv2_add; + lsm6ds3_slave2_config_t slave2_config; + int32_t ret; + + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); + + if (ret == 0) + { + slv2_add.slave2_add = val->slv_add >> 1; + slv2_add.r_2 = 1; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_SLV2_ADD, + (uint8_t *)&slv2_add, 1); + } + + if (ret == 0) + { + ret = lsm6ds3_write_reg(ctx, LSM6DS3_SLV2_SUBADD, + &(val->slv_subadd), 1); + } + + if (ret == 0) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_SLAVE2_CONFIG, + (uint8_t *)&slave2_config, 1); + } + + if (ret == 0) + { + slave2_config.slave2_numop = val->slv_len; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_SLAVE2_CONFIG, + (uint8_t *)&slave2_config, 1); + } + + if (ret == 0) + { + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); + } + + return ret; +} + +/** + * @brief sh_slv3_cfg_read: [get] Configure slave 0 for perform a write/read. + * + * @param stmdev_ctx_t *ctx: read / write interface definitions + * @param lsm6ds3_sh_cfg_read_t: a 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 lsm6ds3_sh_slv3_cfg_read(stmdev_ctx_t *ctx, + lsm6ds3_sh_cfg_read_t *val) +{ + lsm6ds3_slv3_add_t slv3_add; + lsm6ds3_slave3_config_t slave3_config; + int32_t ret; + + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); + + if (ret == 0) + { + slv3_add.slave3_add = val->slv_add >> 1; + slv3_add.r_3 = 1; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_SLV3_ADD, (uint8_t *)&slv3_add, 1); + } + + if (ret == 0) + { + ret = lsm6ds3_write_reg(ctx, LSM6DS3_SLV3_SUBADD, + &(val->slv_subadd), 1); + } + + if (ret == 0) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_SLAVE3_CONFIG, + (uint8_t *)&slave3_config, 1); + } + + if (ret == 0) + { + slave3_config.slave3_numop = val->slv_len; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_SLAVE3_CONFIG, + (uint8_t *)&slave3_config, 1); + } + + if (ret == 0) + { + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); + } + + return ret; +} + +/** + * @brief Sensor hub communication status (0: on-going / 1: idle).[get] + * + * @param ctx read / write interface definitions(ptr) + * @param val get the values of sensor_hub_end_op in reg FUNC_SRC + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_sh_end_op_flag_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3_func_src_t func_src; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_FUNC_SRC, (uint8_t *)&func_src, 1); + *val = (uint8_t)func_src.sensor_hub_end_op; + + return ret; +} + +/** + * @brief xl_hp_path_internal: [set] HPF or SLOPE filter selection on + * wake-up and Activity/Inactivity + * functions. + * + * @param stmdev_ctx_t *ctx: read / write interface definitions + * @param lsm6ds3_slope_fds_t: change the values of slope_fds in reg TAP_CFG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_xl_hp_path_internal_set(stmdev_ctx_t *ctx, + lsm6ds3_slope_fds_t val) +{ + lsm6ds3_tap_cfg_t tap_cfg; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t *)&tap_cfg, 1); + + if (ret == 0) + { + tap_cfg.slope_fds = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t *)&tap_cfg, 1); + } + + return ret; +} + +/** + * @brief xl_hp_path_internal: [get] HPF or SLOPE filter selection on + * wake-up and Activity/Inactivity + * functions. + * + * @param stmdev_ctx_t *ctx: read / write interface definitions + * @param lsm6ds3_slope_fds_t: Get the values of slope_fds in reg TAP_CFG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_xl_hp_path_internal_get(stmdev_ctx_t *ctx, + lsm6ds3_slope_fds_t *val) +{ + lsm6ds3_tap_cfg_t reg; + int32_t ret; + + ret = lsm6ds3_read_reg(ctx, LSM6DS3_TAP_CFG, (uint8_t *)®, 1); + + switch (reg.slope_fds) + { + case LSM6DS3_USE_SLOPE: + *val = LSM6DS3_USE_SLOPE; + break; + + case LSM6DS3_USE_HPF: + *val = LSM6DS3_USE_HPF; + break; + + default: + *val = LSM6DS3_USE_SLOPE; + break; + } + + return ret; +} + +/** + * @brief sh_num_of_dev_connected: [set] Number of external sensors to + * be read by the sensor hub. + * + * @param stmdev_ctx_t *ctx: read / write interface definitions + * @param lsm6ds3_aux_sens_on_t: change the values of aux_sens_on in + * reg SLAVE0_CONFIG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_sh_num_of_dev_connected_set(stmdev_ctx_t *ctx, + lsm6ds3_aux_sens_on_t val) +{ + lsm6ds3_slave0_config_t reg; + int32_t ret; + + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); + + if (ret == 0) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_SLAVE0_CONFIG, (uint8_t *)®, 1); + } + + if (ret == 0) + { + reg.aux_sens_on = (uint8_t)val; + ret = lsm6ds3_write_reg(ctx, LSM6DS3_SLAVE0_CONFIG, (uint8_t *)®, 1); + } + + if (ret == 0) + { + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); + } + + return ret; +} + +/** + * @brief sh_num_of_dev_connected: [get] Number of external sensors to + * be read by the sensor hub. + * + * @param stmdev_ctx_t *ctx: read / write interface definitions + * @param lsm6ds3_aux_sens_on_t: Get the values of aux_sens_on in + * reg SLAVE0_CONFIG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6ds3_sh_num_of_dev_connected_get(stmdev_ctx_t *ctx, + lsm6ds3_aux_sens_on_t *val) +{ + lsm6ds3_slave0_config_t reg; + int32_t ret; + + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_EMBEDDED_FUNC_BANK); + + if (ret == 0) + { + ret = lsm6ds3_read_reg(ctx, LSM6DS3_SLAVE0_CONFIG, (uint8_t *)®, 1); + + switch (reg.aux_sens_on) + { + case LSM6DS3_SLV_0: + *val = LSM6DS3_SLV_0; + break; + + case LSM6DS3_SLV_0_1: + *val = LSM6DS3_SLV_0_1; + break; + + case LSM6DS3_SLV_0_1_2: + *val = LSM6DS3_SLV_0_1_2; + break; + + case LSM6DS3_SLV_0_1_2_3: + *val = LSM6DS3_SLV_0_1_2_3; + break; + + default: + *val = LSM6DS3_SLV_0; + break; + } + } + + if (ret == 0) + { + ret = lsm6ds3_mem_bank_set(ctx, LSM6DS3_USER_BANK); + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @} + * + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ \ No newline at end of file diff --git a/src/reg/lsm6ds3tr-c_reg.c b/src/reg/lsm6ds3tr-c_reg.c new file mode 100644 index 0000000..d91892d --- /dev/null +++ b/src/reg/lsm6ds3tr-c_reg.c @@ -0,0 +1,8210 @@ +/** + ****************************************************************************** + * @file lsm6ds3tr_c_reg.c + * @author Sensors Software Solution Team + * @brief LSM6DS3TR_C 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 "lsm6ds3tr-c_reg.h" + +/** + * @defgroup LSM6DS3TR_C + * @brief This file provides a set of functions needed to drive the + * lsm6ds3tr_c enanced inertial module. + * @{ + * + */ + +/** + * @defgroup LSM6DS3TR_C_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 read / write interface definitions(ptr) + * @param reg register to read + * @param data pointer to buffer that store the data read(ptr) + * @param len number of consecutive register to read + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t __weak lsm6ds3tr_c_read_reg(stmdev_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 read / write interface definitions(ptr) + * @param reg register to write + * @param data pointer to data to write in register reg(ptr) + * @param len number of consecutive register to write + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t __weak lsm6ds3tr_c_write_reg(stmdev_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 LSM6DS3TR_C_Sensitivity + * @brief These functions convert raw-data into engineering units. + * @{ + * + */ + +float_t lsm6ds3tr_c_from_fs2g_to_mg(int16_t lsb) +{ + return ((float_t)lsb * 0.061f); +} + +float_t lsm6ds3tr_c_from_fs4g_to_mg(int16_t lsb) +{ + return ((float_t)lsb * 0.122f); +} + +float_t lsm6ds3tr_c_from_fs8g_to_mg(int16_t lsb) +{ + return ((float_t)lsb * 0.244f); +} + +float_t lsm6ds3tr_c_from_fs16g_to_mg(int16_t lsb) +{ + return ((float_t)lsb * 0.488f); +} + +float_t lsm6ds3tr_c_from_fs125dps_to_mdps(int16_t lsb) +{ + return ((float_t)lsb * 4.375f); +} + +float_t lsm6ds3tr_c_from_fs250dps_to_mdps(int16_t lsb) +{ + return ((float_t)lsb * 8.750f); +} + +float_t lsm6ds3tr_c_from_fs500dps_to_mdps(int16_t lsb) +{ + return ((float_t)lsb * 17.50f); +} + +float_t lsm6ds3tr_c_from_fs1000dps_to_mdps(int16_t lsb) +{ + return ((float_t)lsb * 35.0f); +} + +float_t lsm6ds3tr_c_from_fs2000dps_to_mdps(int16_t lsb) +{ + return ((float_t)lsb * 70.0f); +} + +float_t lsm6ds3tr_c_from_lsb_to_celsius(int16_t lsb) +{ + return (((float_t)lsb / 256.0f) + 25.0f); +} + +/** + * @} + * + */ + + +/** + * @defgroup LSM6DS3TR_C_data_generation + * @brief This section groups all the functions concerning data + * generation + * @{ + * + */ + +/** + * @brief Accelerometer full-scale selection.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of fs_xl in reg CTRL1_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_xl_full_scale_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_fs_xl_t val) +{ + lsm6ds3tr_c_ctrl1_xl_t ctrl1_xl; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL1_XL, + (uint8_t *)&ctrl1_xl, 1); + + if (ret == 0) + { + ctrl1_xl.fs_xl = (uint8_t) val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL1_XL, + (uint8_t *)&ctrl1_xl, 1); + } + + return ret; +} + +/** + * @brief Accelerometer full-scale selection.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of fs_xl in reg CTRL1_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_xl_full_scale_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_fs_xl_t *val) +{ + lsm6ds3tr_c_ctrl1_xl_t ctrl1_xl; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL1_XL, + (uint8_t *)&ctrl1_xl, 1); + + switch (ctrl1_xl.fs_xl) + { + case LSM6DS3TR_C_2g: + *val = LSM6DS3TR_C_2g; + break; + + case LSM6DS3TR_C_16g: + *val = LSM6DS3TR_C_16g; + break; + + case LSM6DS3TR_C_4g: + *val = LSM6DS3TR_C_4g; + break; + + case LSM6DS3TR_C_8g: + *val = LSM6DS3TR_C_8g; + break; + + default: + *val = LSM6DS3TR_C_XL_FS_ND; + break; + } + + return ret; +} + +/** + * @brief Accelerometer data rate selection.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of odr_xl in reg CTRL1_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_xl_data_rate_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_odr_xl_t val) +{ + lsm6ds3tr_c_ctrl1_xl_t ctrl1_xl; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL1_XL, + (uint8_t *)&ctrl1_xl, 1); + + if (ret == 0) + { + ctrl1_xl.odr_xl = (uint8_t) val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL1_XL, + (uint8_t *)&ctrl1_xl, 1); + } + + return ret; +} + +/** + * @brief Accelerometer data rate selection.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of odr_xl in reg CTRL1_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_xl_data_rate_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_odr_xl_t *val) +{ + lsm6ds3tr_c_ctrl1_xl_t ctrl1_xl; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL1_XL, + (uint8_t *)&ctrl1_xl, 1); + + switch (ctrl1_xl.odr_xl) + { + case LSM6DS3TR_C_XL_ODR_OFF: + *val = LSM6DS3TR_C_XL_ODR_OFF; + break; + + case LSM6DS3TR_C_XL_ODR_12Hz5: + *val = LSM6DS3TR_C_XL_ODR_12Hz5; + break; + + case LSM6DS3TR_C_XL_ODR_26Hz: + *val = LSM6DS3TR_C_XL_ODR_26Hz; + break; + + case LSM6DS3TR_C_XL_ODR_52Hz: + *val = LSM6DS3TR_C_XL_ODR_52Hz; + break; + + case LSM6DS3TR_C_XL_ODR_104Hz: + *val = LSM6DS3TR_C_XL_ODR_104Hz; + break; + + case LSM6DS3TR_C_XL_ODR_208Hz: + *val = LSM6DS3TR_C_XL_ODR_208Hz; + break; + + case LSM6DS3TR_C_XL_ODR_416Hz: + *val = LSM6DS3TR_C_XL_ODR_416Hz; + break; + + case LSM6DS3TR_C_XL_ODR_833Hz: + *val = LSM6DS3TR_C_XL_ODR_833Hz; + break; + + case LSM6DS3TR_C_XL_ODR_1k66Hz: + *val = LSM6DS3TR_C_XL_ODR_1k66Hz; + break; + + case LSM6DS3TR_C_XL_ODR_3k33Hz: + *val = LSM6DS3TR_C_XL_ODR_3k33Hz; + break; + + case LSM6DS3TR_C_XL_ODR_6k66Hz: + *val = LSM6DS3TR_C_XL_ODR_6k66Hz; + break; + + case LSM6DS3TR_C_XL_ODR_1Hz6: + *val = LSM6DS3TR_C_XL_ODR_1Hz6; + break; + + default: + *val = LSM6DS3TR_C_XL_ODR_ND; + break; + } + + return ret; +} + +/** + * @brief Gyroscope chain full-scale selection.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of fs_g in reg CTRL2_G + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_gy_full_scale_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_fs_g_t val) +{ + lsm6ds3tr_c_ctrl2_g_t ctrl2_g; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL2_G, + (uint8_t *)&ctrl2_g, 1); + + if (ret == 0) + { + ctrl2_g.fs_g = (uint8_t) val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL2_G, + (uint8_t *)&ctrl2_g, 1); + } + + return ret; +} + +/** + * @brief Gyroscope chain full-scale selection.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of fs_g in reg CTRL2_G + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_gy_full_scale_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_fs_g_t *val) +{ + lsm6ds3tr_c_ctrl2_g_t ctrl2_g; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL2_G, + (uint8_t *)&ctrl2_g, 1); + + switch (ctrl2_g.fs_g) + { + case LSM6DS3TR_C_250dps: + *val = LSM6DS3TR_C_250dps; + break; + + case LSM6DS3TR_C_125dps: + *val = LSM6DS3TR_C_125dps; + break; + + case LSM6DS3TR_C_500dps: + *val = LSM6DS3TR_C_500dps; + break; + + case LSM6DS3TR_C_1000dps: + *val = LSM6DS3TR_C_1000dps; + break; + + case LSM6DS3TR_C_2000dps: + *val = LSM6DS3TR_C_2000dps; + break; + + default: + *val = LSM6DS3TR_C_GY_FS_ND; + break; + } + + return ret; +} + +/** + * @brief Gyroscope data rate selection.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of odr_g in reg CTRL2_G + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_gy_data_rate_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_odr_g_t val) +{ + lsm6ds3tr_c_ctrl2_g_t ctrl2_g; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL2_G, + (uint8_t *)&ctrl2_g, 1); + + if (ret == 0) + { + ctrl2_g.odr_g = (uint8_t) val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL2_G, + (uint8_t *)&ctrl2_g, 1); + } + + return ret; +} + +/** + * @brief Gyroscope data rate selection.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of odr_g in reg CTRL2_G + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_gy_data_rate_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_odr_g_t *val) +{ + lsm6ds3tr_c_ctrl2_g_t ctrl2_g; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL2_G, + (uint8_t *)&ctrl2_g, 1); + + switch (ctrl2_g.odr_g) + { + case LSM6DS3TR_C_GY_ODR_OFF: + *val = LSM6DS3TR_C_GY_ODR_OFF; + break; + + case LSM6DS3TR_C_GY_ODR_12Hz5: + *val = LSM6DS3TR_C_GY_ODR_12Hz5; + break; + + case LSM6DS3TR_C_GY_ODR_26Hz: + *val = LSM6DS3TR_C_GY_ODR_26Hz; + break; + + case LSM6DS3TR_C_GY_ODR_52Hz: + *val = LSM6DS3TR_C_GY_ODR_52Hz; + break; + + case LSM6DS3TR_C_GY_ODR_104Hz: + *val = LSM6DS3TR_C_GY_ODR_104Hz; + break; + + case LSM6DS3TR_C_GY_ODR_208Hz: + *val = LSM6DS3TR_C_GY_ODR_208Hz; + break; + + case LSM6DS3TR_C_GY_ODR_416Hz: + *val = LSM6DS3TR_C_GY_ODR_416Hz; + break; + + case LSM6DS3TR_C_GY_ODR_833Hz: + *val = LSM6DS3TR_C_GY_ODR_833Hz; + break; + + case LSM6DS3TR_C_GY_ODR_1k66Hz: + *val = LSM6DS3TR_C_GY_ODR_1k66Hz; + break; + + case LSM6DS3TR_C_GY_ODR_3k33Hz: + *val = LSM6DS3TR_C_GY_ODR_3k33Hz; + break; + + case LSM6DS3TR_C_GY_ODR_6k66Hz: + *val = LSM6DS3TR_C_GY_ODR_6k66Hz; + break; + + default: + *val = LSM6DS3TR_C_GY_ODR_ND; + break; + } + + return ret; +} + +/** + * @brief Block data update.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of bdu in reg CTRL3_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_block_data_update_set(stmdev_ctx_t *ctx, + uint8_t val) +{ + lsm6ds3tr_c_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL3_C, + (uint8_t *)&ctrl3_c, 1); + + if (ret == 0) + { + ctrl3_c.bdu = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL3_C, + (uint8_t *)&ctrl3_c, 1); + } + + return ret; +} + +/** + * @brief Block data update.[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of bdu in reg CTRL3_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_block_data_update_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3tr_c_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL3_C, + (uint8_t *)&ctrl3_c, 1); + *val = ctrl3_c.bdu; + + return ret; +} + +/** + * @brief Weight of XL user offset bits of registers + * X_OFS_USR(73h), Y_OFS_USR(74h), Z_OFS_USR(75h).[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of usr_off_w in reg CTRL6_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_xl_offset_weight_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_usr_off_w_t val) +{ + lsm6ds3tr_c_ctrl6_c_t ctrl6_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL6_C, + (uint8_t *)&ctrl6_c, 1); + + if (ret == 0) + { + ctrl6_c.usr_off_w = (uint8_t) val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL6_C, + (uint8_t *)&ctrl6_c, 1); + } + + return ret; +} + +/** + * @brief Weight of XL user offset bits of registers + * X_OFS_USR(73h), Y_OFS_USR(74h), Z_OFS_USR(75h).[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of usr_off_w in reg CTRL6_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_xl_offset_weight_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_usr_off_w_t *val) +{ + lsm6ds3tr_c_ctrl6_c_t ctrl6_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL6_C, + (uint8_t *)&ctrl6_c, 1); + + switch (ctrl6_c.usr_off_w) + { + case LSM6DS3TR_C_LSb_1mg: + *val = LSM6DS3TR_C_LSb_1mg; + break; + + case LSM6DS3TR_C_LSb_16mg: + *val = LSM6DS3TR_C_LSb_16mg; + break; + + default: + *val = LSM6DS3TR_C_WEIGHT_ND; + break; + } + + return ret; +} + +/** + * @brief High-performance operating mode for accelerometer[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of xl_hm_mode in reg CTRL6_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_xl_power_mode_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_xl_hm_mode_t val) +{ + lsm6ds3tr_c_ctrl6_c_t ctrl6_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL6_C, + (uint8_t *)&ctrl6_c, 1); + + if (ret == 0) + { + ctrl6_c.xl_hm_mode = (uint8_t) val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL6_C, + (uint8_t *)&ctrl6_c, 1); + } + + return ret; +} + +/** + * @brief High-performance operating mode for accelerometer.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of xl_hm_mode in reg CTRL6_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_xl_power_mode_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_xl_hm_mode_t *val) +{ + lsm6ds3tr_c_ctrl6_c_t ctrl6_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL6_C, + (uint8_t *)&ctrl6_c, 1); + + switch (ctrl6_c.xl_hm_mode) + { + case LSM6DS3TR_C_XL_HIGH_PERFORMANCE: + *val = LSM6DS3TR_C_XL_HIGH_PERFORMANCE; + break; + + case LSM6DS3TR_C_XL_NORMAL: + *val = LSM6DS3TR_C_XL_NORMAL; + break; + + default: + *val = LSM6DS3TR_C_XL_PW_MODE_ND; + break; + } + + return ret; +} + +/** + * @brief Source register rounding function on WAKE_UP_SRC (1Bh), + * TAP_SRC (1Ch), D6D_SRC (1Dh), STATUS_REG (1Eh) and + * FUNC_SRC1 (53h) registers in the primary interface.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of rounding_status in reg CTRL7_G + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_rounding_on_status_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_rounding_status_t val) +{ + lsm6ds3tr_c_ctrl7_g_t ctrl7_g; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL7_G, + (uint8_t *)&ctrl7_g, 1); + + if (ret == 0) + { + ctrl7_g.rounding_status = (uint8_t) val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL7_G, + (uint8_t *)&ctrl7_g, 1); + } + + return ret; +} + +/** + * @brief Source register rounding function on WAKE_UP_SRC (1Bh), + * TAP_SRC (1Ch), D6D_SRC (1Dh), STATUS_REG (1Eh) and + * FUNC_SRC1 (53h) registers in the primary interface.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of rounding_status in reg CTRL7_G + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_rounding_on_status_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_rounding_status_t *val) +{ + lsm6ds3tr_c_ctrl7_g_t ctrl7_g; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL7_G, + (uint8_t *)&ctrl7_g, 1); + + switch (ctrl7_g.rounding_status) + { + case LSM6DS3TR_C_STAT_RND_DISABLE: + *val = LSM6DS3TR_C_STAT_RND_DISABLE; + break; + + case LSM6DS3TR_C_STAT_RND_ENABLE: + *val = LSM6DS3TR_C_STAT_RND_ENABLE; + break; + + default: + *val = LSM6DS3TR_C_STAT_RND_ND; + break; + } + + return ret; +} + +/** + * @brief High-performance operating mode disable for gyroscope.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of g_hm_mode in reg CTRL7_G + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_gy_power_mode_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_g_hm_mode_t val) +{ + lsm6ds3tr_c_ctrl7_g_t ctrl7_g; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL7_G, + (uint8_t *)&ctrl7_g, 1); + + if (ret == 0) + { + ctrl7_g.g_hm_mode = (uint8_t) val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL7_G, + (uint8_t *)&ctrl7_g, 1); + } + + return ret; +} + +/** + * @brief High-performance operating mode disable for gyroscope.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of g_hm_mode in reg CTRL7_G + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_gy_power_mode_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_g_hm_mode_t *val) +{ + lsm6ds3tr_c_ctrl7_g_t ctrl7_g; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL7_G, + (uint8_t *)&ctrl7_g, 1); + + switch (ctrl7_g.g_hm_mode) + { + case LSM6DS3TR_C_GY_HIGH_PERFORMANCE: + *val = LSM6DS3TR_C_GY_HIGH_PERFORMANCE; + break; + + case LSM6DS3TR_C_GY_NORMAL: + *val = LSM6DS3TR_C_GY_NORMAL; + break; + + default: + *val = LSM6DS3TR_C_GY_PW_MODE_ND; + break; + } + + return ret; +} + +/** + * @brief Read all the interrupt/status flag of the device.[get] + * + * @param ctx Read / write interface definitions + * @param val WAKE_UP_SRC, TAP_SRC, D6D_SRC, STATUS_REG, + * FUNC_SRC1, FUNC_SRC2, WRIST_TILT_IA, A_WRIST_TILT_Mask + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_all_sources_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_all_sources_t *val) +{ + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_WAKE_UP_SRC, + (uint8_t *) & (val->wake_up_src), 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_TAP_SRC, + (uint8_t *) & (val->tap_src), 1); + } + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_D6D_SRC, + (uint8_t *) & (val->d6d_src), 1); + } + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_STATUS_REG, + (uint8_t *) & (val->status_reg), 1); + } + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FUNC_SRC1, + (uint8_t *) & (val->func_src1), 1); + } + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FUNC_SRC2, + (uint8_t *) & (val->func_src2), 1); + } + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_WRIST_TILT_IA, + (uint8_t *) & (val->wrist_tilt_ia), 1); + } + + if (ret == 0) + { + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_B); + } + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_A_WRIST_TILT_MASK, + (uint8_t *) & (val->a_wrist_tilt_mask), 1); + } + + if (ret == 0) + { + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + + return ret; +} +/** + * @brief The STATUS_REG register is read by the primary interface[get] + * + * @param ctx Read / write interface definitions + * @param val Registers STATUS_REG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_status_reg_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_status_reg_t *val) +{ + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_STATUS_REG, + (uint8_t *) val, 1); + + return ret; +} + +/** + * @brief Accelerometer new data available.[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of xlda in reg STATUS_REG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_xl_flag_data_ready_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3tr_c_status_reg_t status_reg; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_STATUS_REG, + (uint8_t *)&status_reg, 1); + *val = status_reg.xlda; + + return ret; +} + +/** + * @brief Gyroscope new data available.[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of gda in reg STATUS_REG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_gy_flag_data_ready_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3tr_c_status_reg_t status_reg; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_STATUS_REG, + (uint8_t *)&status_reg, 1); + *val = status_reg.gda; + + return ret; +} + +/** + * @brief Temperature new data available.[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of tda in reg STATUS_REG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_temp_flag_data_ready_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3tr_c_status_reg_t status_reg; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_STATUS_REG, + (uint8_t *)&status_reg, 1); + *val = status_reg.tda; + + return ret; +} + +/** + * @brief Accelerometer axis user offset correction expressed in two’s + * complement, weight depends on USR_OFF_W in CTRL6_C. + * The value must be in the range [-127 127].[set] + * + * @param ctx Read / write interface definitions + * @param buff Buffer that contains data to write + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_xl_usr_offset_set(stmdev_ctx_t *ctx, + uint8_t *buff) +{ + int32_t ret; + + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_X_OFS_USR, buff, 3); + + return ret; +} + +/** + * @brief Accelerometer axis user offset correction xpressed in two’s + * complement, weight depends on USR_OFF_W in CTRL6_C. + * The value must be in the range [-127 127].[get] + * + * @param ctx Read / write interface definitions + * @param buff Buffer that stores data read + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_xl_usr_offset_get(stmdev_ctx_t *ctx, + uint8_t *buff) +{ + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_X_OFS_USR, buff, 3); + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3TR_C_Timestamp + * @brief This section groups all the functions that manage the + * timestamp generation. + * @{ + * + */ + +/** + * @brief Enable timestamp count. The count is saved in TIMESTAMP0_REG (40h), + * TIMESTAMP1_REG (41h) and TIMESTAMP2_REG (42h).[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of timer_en in reg CTRL10_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_timestamp_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3tr_c_ctrl10_c_t ctrl10_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL10_C, + (uint8_t *)&ctrl10_c, 1); + + if (ret == 0) + { + ctrl10_c.timer_en = val; + + if (val != 0x00U) + { + ctrl10_c.func_en = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL10_C, + (uint8_t *)&ctrl10_c, 1); + } + } + + return ret; +} + +/** + * @brief Enable timestamp count. The count is saved in TIMESTAMP0_REG (40h), + * TIMESTAMP1_REG (41h) and TIMESTAMP2_REG (42h).[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of timer_en in reg CTRL10_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_timestamp_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3tr_c_ctrl10_c_t ctrl10_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL10_C, + (uint8_t *)&ctrl10_c, 1); + *val = ctrl10_c.timer_en; + + return ret; +} + +/** + * @brief Timestamp register resolution setting. + * Configuration of this bit affects + * TIMESTAMP0_REG(40h), TIMESTAMP1_REG(41h), + * TIMESTAMP2_REG(42h), STEP_TIMESTAMP_L(49h), + * STEP_TIMESTAMP_H(4Ah) and + * STEP_COUNT_DELTA(15h) registers.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of timer_hr in reg WAKE_UP_DUR + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_timestamp_res_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_timer_hr_t val) +{ + lsm6ds3tr_c_wake_up_dur_t wake_up_dur; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_WAKE_UP_DUR, + (uint8_t *)&wake_up_dur, 1); + + if (ret == 0) + { + wake_up_dur.timer_hr = (uint8_t) val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_WAKE_UP_DUR, + (uint8_t *)&wake_up_dur, 1); + } + + return ret; +} + +/** + * @brief Timestamp register resolution setting. + * Configuration of this bit affects + * TIMESTAMP0_REG(40h), TIMESTAMP1_REG(41h), + * TIMESTAMP2_REG(42h), STEP_TIMESTAMP_L(49h), + * STEP_TIMESTAMP_H(4Ah) and + * STEP_COUNT_DELTA(15h) registers.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of timer_hr in reg WAKE_UP_DUR + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_timestamp_res_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_timer_hr_t *val) +{ + lsm6ds3tr_c_wake_up_dur_t wake_up_dur; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_WAKE_UP_DUR, + (uint8_t *)&wake_up_dur, 1); + + switch (wake_up_dur.timer_hr) + { + case LSM6DS3TR_C_LSB_6ms4: + *val = LSM6DS3TR_C_LSB_6ms4; + break; + + case LSM6DS3TR_C_LSB_25us: + *val = LSM6DS3TR_C_LSB_25us; + break; + + default: + *val = LSM6DS3TR_C_TS_RES_ND; + break; + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3TR_C_Dataoutput + * @brief This section groups all the data output functions. + * @{ + * + */ + +/** + * @brief Circular burst-mode (rounding) read from output registers + * through the primary interface.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of rounding in reg CTRL5_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_rounding_mode_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_rounding_t val) +{ + lsm6ds3tr_c_ctrl5_c_t ctrl5_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL5_C, + (uint8_t *)&ctrl5_c, 1); + + if (ret == 0) + { + ctrl5_c.rounding = (uint8_t) val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL5_C, + (uint8_t *)&ctrl5_c, 1); + } + + return ret; +} + +/** + * @brief Circular burst-mode (rounding) read from output registers + * through the primary interface.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of rounding in reg CTRL5_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_rounding_mode_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_rounding_t *val) +{ + lsm6ds3tr_c_ctrl5_c_t ctrl5_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL5_C, + (uint8_t *)&ctrl5_c, 1); + + switch (ctrl5_c.rounding) + { + case LSM6DS3TR_C_ROUND_DISABLE: + *val = LSM6DS3TR_C_ROUND_DISABLE; + break; + + case LSM6DS3TR_C_ROUND_XL: + *val = LSM6DS3TR_C_ROUND_XL; + break; + + case LSM6DS3TR_C_ROUND_GY: + *val = LSM6DS3TR_C_ROUND_GY; + break; + + case LSM6DS3TR_C_ROUND_GY_XL: + *val = LSM6DS3TR_C_ROUND_GY_XL; + break; + + case LSM6DS3TR_C_ROUND_SH1_TO_SH6: + *val = LSM6DS3TR_C_ROUND_SH1_TO_SH6; + break; + + case LSM6DS3TR_C_ROUND_XL_SH1_TO_SH6: + *val = LSM6DS3TR_C_ROUND_XL_SH1_TO_SH6; + break; + + case LSM6DS3TR_C_ROUND_GY_XL_SH1_TO_SH12: + *val = LSM6DS3TR_C_ROUND_GY_XL_SH1_TO_SH12; + break; + + case LSM6DS3TR_C_ROUND_GY_XL_SH1_TO_SH6: + *val = LSM6DS3TR_C_ROUND_GY_XL_SH1_TO_SH6; + break; + + default: + *val = LSM6DS3TR_C_ROUND_OUT_ND; + break; + } + + return ret; +} + +/** + * @brief Temperature data output register (r). L and H registers together + * express a 16-bit word in two’s complement.[get] + * + * @param ctx Read / write interface definitions + * @param buff Buffer that stores data read + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_temperature_raw_get(stmdev_ctx_t *ctx, + int16_t *val) +{ + uint8_t buff[2]; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_OUT_TEMP_L, buff, 2); + *val = (int16_t)buff[1]; + *val = (*val * 256) + (int16_t)buff[0]; + + return ret; +} + +/** + * @brief Angular rate sensor. The value is expressed as a 16-bit word in + * two’s complement.[get] + * + * @param ctx Read / write interface definitions + * @param buff Buffer that stores data read + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_angular_rate_raw_get(stmdev_ctx_t *ctx, + int16_t *val) +{ + uint8_t buff[6]; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_OUTX_L_G, buff, 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 output register. The value is expressed + * as a 16-bit word in two’s complement.[get] + * + * @param ctx Read / write interface definitions + * @param buff Buffer that stores data read + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_acceleration_raw_get(stmdev_ctx_t *ctx, + int16_t *val) +{ + uint8_t buff[6]; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_OUTX_L_XL, buff, 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 External magnetometer raw data.[get] + * + * @param ctx Read / write interface definitions + * @param buff Buffer that stores data read + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_mag_calibrated_raw_get(stmdev_ctx_t *ctx, + int16_t *val) +{ + uint8_t buff[6]; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_OUT_MAG_RAW_X_L, buff, 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 Read data in FIFO.[get] + * + * @param ctx Read / write interface definitions + * @param buffer Data buffer to store FIFO data. + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_fifo_raw_data_get(stmdev_ctx_t *ctx, + uint8_t *buffer) +{ + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FIFO_DATA_OUT_L, buffer, 1); + + if (ret == 0) { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FIFO_DATA_OUT_H, buffer + 1, 1); + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3TR_C_common + * @brief This section groups common useful functions. + * @{ + * + */ + +/** + * @brief Enable access to the embedded functions/sensor hub + * configuration registers[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of func_cfg_en in reg FUNC_CFG_ACCESS + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_mem_bank_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_func_cfg_en_t val) +{ + lsm6ds3tr_c_func_cfg_access_t func_cfg_access; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FUNC_CFG_ACCESS, + (uint8_t *)&func_cfg_access, 1); + + if (ret == 0) + { + func_cfg_access.func_cfg_en = (uint8_t) val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_FUNC_CFG_ACCESS, + (uint8_t *)&func_cfg_access, 1); + } + + return ret; +} + +/** + * @brief Enable access to the embedded functions/sensor hub configuration + * registers[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of func_cfg_en in reg FUNC_CFG_ACCESS + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_mem_bank_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_func_cfg_en_t *val) +{ + lsm6ds3tr_c_func_cfg_access_t func_cfg_access; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FUNC_CFG_ACCESS, + (uint8_t *)&func_cfg_access, 1); + + switch (func_cfg_access.func_cfg_en) + { + case LSM6DS3TR_C_USER_BANK: + *val = LSM6DS3TR_C_USER_BANK; + break; + + case LSM6DS3TR_C_BANK_B: + *val = LSM6DS3TR_C_BANK_B; + break; + + default: + *val = LSM6DS3TR_C_BANK_ND; + break; + } + + return ret; +} + +/** + * @brief Data-ready pulsed / letched mode[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of drdy_pulsed in reg DRDY_PULSE_CFG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_data_ready_mode_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_drdy_pulsed_g_t val) +{ + lsm6ds3tr_c_drdy_pulse_cfg_g_t drdy_pulse_cfg_g; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_DRDY_PULSE_CFG_G, + (uint8_t *)&drdy_pulse_cfg_g, 1); + + if (ret == 0) + { + drdy_pulse_cfg_g.drdy_pulsed = (uint8_t) val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_DRDY_PULSE_CFG_G, + (uint8_t *)&drdy_pulse_cfg_g, 1); + } + + return ret; +} + +/** + * @brief Data-ready pulsed / letched mode[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of drdy_pulsed in reg DRDY_PULSE_CFG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_data_ready_mode_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_drdy_pulsed_g_t *val) +{ + lsm6ds3tr_c_drdy_pulse_cfg_g_t drdy_pulse_cfg_g; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_DRDY_PULSE_CFG_G, + (uint8_t *)&drdy_pulse_cfg_g, 1); + + switch (drdy_pulse_cfg_g.drdy_pulsed) + { + case LSM6DS3TR_C_DRDY_LATCHED: + *val = LSM6DS3TR_C_DRDY_LATCHED; + break; + + case LSM6DS3TR_C_DRDY_PULSED: + *val = LSM6DS3TR_C_DRDY_PULSED; + break; + + default: + *val = LSM6DS3TR_C_DRDY_ND; + break; + } + + return ret; +} + +/** + * @brief DeviceWhoamI.[get] + * + * @param ctx Read / write interface definitions + * @param buff Buffer that stores data read + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_device_id_get(stmdev_ctx_t *ctx, uint8_t *buff) +{ + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_WHO_AM_I, buff, 1); + + return ret; +} + +/** + * @brief Software reset. Restore the default values in user registers[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of sw_reset in reg CTRL3_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_reset_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3tr_c_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL3_C, + (uint8_t *)&ctrl3_c, 1); + + if (ret == 0) + { + ctrl3_c.sw_reset = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL3_C, + (uint8_t *)&ctrl3_c, 1); + } + + return ret; +} + +/** + * @brief Software reset. Restore the default values in user registers[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of sw_reset in reg CTRL3_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_reset_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3tr_c_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL3_C, + (uint8_t *)&ctrl3_c, 1); + *val = ctrl3_c.sw_reset; + + return ret; +} + +/** + * @brief Big/Little Endian Data selection.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of ble in reg CTRL3_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_data_format_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_ble_t val) +{ + lsm6ds3tr_c_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL3_C, + (uint8_t *)&ctrl3_c, 1); + + if (ret == 0) + { + ctrl3_c.ble = (uint8_t) val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL3_C, + (uint8_t *)&ctrl3_c, 1); + } + + return ret; +} + +/** + * @brief Big/Little Endian Data selection.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of ble in reg CTRL3_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_data_format_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_ble_t *val) +{ + lsm6ds3tr_c_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL3_C, + (uint8_t *)&ctrl3_c, 1); + + switch (ctrl3_c.ble) + { + case LSM6DS3TR_C_LSB_AT_LOW_ADD: + *val = LSM6DS3TR_C_LSB_AT_LOW_ADD; + break; + + case LSM6DS3TR_C_MSB_AT_LOW_ADD: + *val = LSM6DS3TR_C_MSB_AT_LOW_ADD; + break; + + default: + *val = LSM6DS3TR_C_DATA_FMT_ND; + break; + } + + return ret; +} + +/** + * @brief Register address automatically incremented during a multiple byte + * access with a serial interface.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of if_inc in reg CTRL3_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_auto_increment_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3tr_c_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL3_C, + (uint8_t *)&ctrl3_c, 1); + + if (ret == 0) + { + ctrl3_c.if_inc = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL3_C, + (uint8_t *)&ctrl3_c, 1); + } + + return ret; +} + +/** + * @brief Register address automatically incremented during a multiple byte + * access with a serial interface.[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of if_inc in reg CTRL3_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_auto_increment_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3tr_c_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL3_C, + (uint8_t *)&ctrl3_c, 1); + *val = ctrl3_c.if_inc; + + return ret; +} + +/** + * @brief Reboot memory content. Reload the calibration parameters.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of boot in reg CTRL3_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_boot_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3tr_c_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL3_C, + (uint8_t *)&ctrl3_c, 1); + + if (ret == 0) + { + ctrl3_c.boot = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL3_C, + (uint8_t *)&ctrl3_c, 1); + } + + return ret; +} + +/** + * @brief Reboot memory content. Reload the calibration parameters.[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of boot in reg CTRL3_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_boot_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3tr_c_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL3_C, + (uint8_t *)&ctrl3_c, 1); + *val = ctrl3_c.boot; + + return ret; +} + +/** + * @brief Linear acceleration sensor self-test enable.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of st_xl in reg CTRL5_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_xl_self_test_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_st_xl_t val) +{ + lsm6ds3tr_c_ctrl5_c_t ctrl5_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL5_C, + (uint8_t *)&ctrl5_c, 1); + + if (ret == 0) + { + ctrl5_c.st_xl = (uint8_t) val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL5_C, + (uint8_t *)&ctrl5_c, 1); + } + + return ret; +} + +/** + * @brief Linear acceleration sensor self-test enable.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of st_xl in reg CTRL5_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_xl_self_test_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_st_xl_t *val) +{ + lsm6ds3tr_c_ctrl5_c_t ctrl5_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL5_C, + (uint8_t *)&ctrl5_c, 1); + + switch (ctrl5_c.st_xl) + { + case LSM6DS3TR_C_XL_ST_DISABLE: + *val = LSM6DS3TR_C_XL_ST_DISABLE; + break; + + case LSM6DS3TR_C_XL_ST_POSITIVE: + *val = LSM6DS3TR_C_XL_ST_POSITIVE; + break; + + case LSM6DS3TR_C_XL_ST_NEGATIVE: + *val = LSM6DS3TR_C_XL_ST_NEGATIVE; + break; + + default: + *val = LSM6DS3TR_C_XL_ST_ND; + break; + } + + return ret; +} + +/** + * @brief Angular rate sensor self-test enable.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of st_g in reg CTRL5_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_gy_self_test_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_st_g_t val) +{ + lsm6ds3tr_c_ctrl5_c_t ctrl5_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL5_C, + (uint8_t *)&ctrl5_c, 1); + + if (ret == 0) + { + ctrl5_c.st_g = (uint8_t) val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL5_C, + (uint8_t *)&ctrl5_c, 1); + } + + return ret; +} + +/** + * @brief Angular rate sensor self-test enable.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of st_g in reg CTRL5_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_gy_self_test_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_st_g_t *val) +{ + lsm6ds3tr_c_ctrl5_c_t ctrl5_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL5_C, + (uint8_t *)&ctrl5_c, 1); + + switch (ctrl5_c.st_g) + { + case LSM6DS3TR_C_GY_ST_DISABLE: + *val = LSM6DS3TR_C_GY_ST_DISABLE; + break; + + case LSM6DS3TR_C_GY_ST_POSITIVE: + *val = LSM6DS3TR_C_GY_ST_POSITIVE; + break; + + case LSM6DS3TR_C_GY_ST_NEGATIVE: + *val = LSM6DS3TR_C_GY_ST_NEGATIVE; + break; + + default: + *val = LSM6DS3TR_C_GY_ST_ND; + break; + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3TR_C_filters + * @brief This section group all the functions concerning the filters + * configuration that impact both accelerometer and gyro. + * @{ + * + */ + +/** + * @brief Mask DRDY on pin (both XL & Gyro) until filter settling ends + * (XL and Gyro independently masked).[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of drdy_mask in reg CTRL4_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_filter_settling_mask_set(stmdev_ctx_t *ctx, + uint8_t val) +{ + lsm6ds3tr_c_ctrl4_c_t ctrl4_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL4_C, + (uint8_t *)&ctrl4_c, 1); + + if (ret == 0) + { + ctrl4_c.drdy_mask = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL4_C, + (uint8_t *)&ctrl4_c, 1); + } + + return ret; +} + +/** + * @brief Mask DRDY on pin (both XL & Gyro) until filter settling ends + * (XL and Gyro independently masked).[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of drdy_mask in reg CTRL4_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_filter_settling_mask_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3tr_c_ctrl4_c_t ctrl4_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL4_C, + (uint8_t *)&ctrl4_c, 1); + *val = ctrl4_c.drdy_mask; + + return ret; +} + +/** + * @brief HPF or SLOPE filter selection on wake-up and Activity/Inactivity + * functions.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of slope_fds in reg TAP_CFG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_xl_hp_path_internal_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_slope_fds_t val) +{ + lsm6ds3tr_c_tap_cfg_t tap_cfg; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_TAP_CFG, + (uint8_t *)&tap_cfg, 1); + + if (ret == 0) + { + tap_cfg.slope_fds = (uint8_t) val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_TAP_CFG, + (uint8_t *)&tap_cfg, 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 Get the values of slope_fds in reg TAP_CFG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_xl_hp_path_internal_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_slope_fds_t *val) +{ + lsm6ds3tr_c_tap_cfg_t tap_cfg; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_TAP_CFG, + (uint8_t *)&tap_cfg, 1); + + switch (tap_cfg.slope_fds) + { + case LSM6DS3TR_C_USE_SLOPE: + *val = LSM6DS3TR_C_USE_SLOPE; + break; + + case LSM6DS3TR_C_USE_HPF: + *val = LSM6DS3TR_C_USE_HPF; + break; + + default: + *val = LSM6DS3TR_C_HP_PATH_ND; + break; + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3TR_C_accelerometer_filters + * @brief This section group all the functions concerning the filters + * configuration that impact accelerometer in every mode. + * @{ + * + */ + +/** + * @brief Accelerometer analog chain bandwidth selection (only for + * accelerometer ODR ≥ 1.67 kHz).[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of bw0_xl in reg CTRL1_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_xl_filter_analog_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_bw0_xl_t val) +{ + lsm6ds3tr_c_ctrl1_xl_t ctrl1_xl; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL1_XL, + (uint8_t *)&ctrl1_xl, 1); + + if (ret == 0) + { + ctrl1_xl.bw0_xl = (uint8_t) val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL1_XL, + (uint8_t *)&ctrl1_xl, 1); + } + + return ret; +} + +/** + * @brief Accelerometer analog chain bandwidth selection (only for + * accelerometer ODR ≥ 1.67 kHz).[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of bw0_xl in reg CTRL1_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_xl_filter_analog_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_bw0_xl_t *val) +{ + lsm6ds3tr_c_ctrl1_xl_t ctrl1_xl; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL1_XL, + (uint8_t *)&ctrl1_xl, 1); + + switch (ctrl1_xl.bw0_xl) + { + case LSM6DS3TR_C_XL_ANA_BW_1k5Hz: + *val = LSM6DS3TR_C_XL_ANA_BW_1k5Hz; + break; + + case LSM6DS3TR_C_XL_ANA_BW_400Hz: + *val = LSM6DS3TR_C_XL_ANA_BW_400Hz; + break; + + default: + *val = LSM6DS3TR_C_XL_ANA_BW_ND; + break; + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3TR_C_accelerometer_filters + * @brief This section group all the functions concerning the filters + * configuration that impact accelerometer. + * @{ + * + */ + +/** + * @brief Accelerometer digital LPF (LPF1) bandwidth selection LPF2 is + * not used.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of lpf1_bw_sel in reg CTRL1_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_xl_lp1_bandwidth_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_lpf1_bw_sel_t val) +{ + lsm6ds3tr_c_ctrl1_xl_t ctrl1_xl; + lsm6ds3tr_c_ctrl8_xl_t ctrl8_xl; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL1_XL, + (uint8_t *)&ctrl1_xl, 1); + + if (ret == 0) + { + ctrl1_xl.lpf1_bw_sel = (uint8_t) val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL1_XL, + (uint8_t *)&ctrl1_xl, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL8_XL, + (uint8_t *)&ctrl8_xl, 1); + + if (ret == 0) + { + ctrl8_xl.lpf2_xl_en = 0; + ctrl8_xl.hp_slope_xl_en = 0; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL8_XL, + (uint8_t *)&ctrl8_xl, 1); + } + } + } + + return ret; +} + +/** + * @brief Accelerometer digital LPF (LPF1) bandwidth selection LPF2 + * is not used.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of lpf1_bw_sel in reg CTRL1_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_xl_lp1_bandwidth_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_lpf1_bw_sel_t *val) +{ + lsm6ds3tr_c_ctrl1_xl_t ctrl1_xl; + lsm6ds3tr_c_ctrl8_xl_t ctrl8_xl; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL8_XL, + (uint8_t *)&ctrl8_xl, 1); + + if (ret == 0) + { + if ((ctrl8_xl.lpf2_xl_en != 0x00U) || + (ctrl8_xl.hp_slope_xl_en != 0x00U)) + { + *val = LSM6DS3TR_C_XL_LP1_NA; + } + + else + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL1_XL, + (uint8_t *)&ctrl1_xl, 1); + + switch (ctrl1_xl.lpf1_bw_sel) + { + case LSM6DS3TR_C_XL_LP1_ODR_DIV_2: + *val = LSM6DS3TR_C_XL_LP1_ODR_DIV_2; + break; + + case LSM6DS3TR_C_XL_LP1_ODR_DIV_4: + *val = LSM6DS3TR_C_XL_LP1_ODR_DIV_4; + break; + + default: + *val = LSM6DS3TR_C_XL_LP1_NA; + break; + } + } + } + + return ret; +} + +/** + * @brief LPF2 on outputs[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of input_composite in reg CTRL8_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_xl_lp2_bandwidth_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_input_composite_t val) +{ + lsm6ds3tr_c_ctrl8_xl_t ctrl8_xl; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL8_XL, + (uint8_t *)&ctrl8_xl, 1); + + if (ret == 0) + { + ctrl8_xl.input_composite = ((uint8_t) val & 0x10U) >> 4; + ctrl8_xl.hpcf_xl = (uint8_t) val & 0x03U; + ctrl8_xl.lpf2_xl_en = 1; + ctrl8_xl.hp_slope_xl_en = 0; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL8_XL, + (uint8_t *)&ctrl8_xl, 1); + } + + return ret; +} + +/** + * @brief LPF2 on outputs[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of input_composite in reg CTRL8_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_xl_lp2_bandwidth_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_input_composite_t *val) +{ + lsm6ds3tr_c_ctrl8_xl_t ctrl8_xl; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL8_XL, + (uint8_t *)&ctrl8_xl, 1); + + if (ret == 0) + { + if ((ctrl8_xl.lpf2_xl_en == 0x00U) || + (ctrl8_xl.hp_slope_xl_en != 0x00U)) + { + *val = LSM6DS3TR_C_XL_LP_NA; + } + + else + { + switch ((ctrl8_xl.input_composite << 4) + ctrl8_xl.hpcf_xl) + { + case LSM6DS3TR_C_XL_LOW_LAT_LP_ODR_DIV_50: + *val = LSM6DS3TR_C_XL_LOW_LAT_LP_ODR_DIV_50; + break; + + case LSM6DS3TR_C_XL_LOW_LAT_LP_ODR_DIV_100: + *val = LSM6DS3TR_C_XL_LOW_LAT_LP_ODR_DIV_100; + break; + + case LSM6DS3TR_C_XL_LOW_LAT_LP_ODR_DIV_9: + *val = LSM6DS3TR_C_XL_LOW_LAT_LP_ODR_DIV_9; + break; + + case LSM6DS3TR_C_XL_LOW_LAT_LP_ODR_DIV_400: + *val = LSM6DS3TR_C_XL_LOW_LAT_LP_ODR_DIV_400; + break; + + case LSM6DS3TR_C_XL_LOW_NOISE_LP_ODR_DIV_50: + *val = LSM6DS3TR_C_XL_LOW_NOISE_LP_ODR_DIV_50; + break; + + case LSM6DS3TR_C_XL_LOW_NOISE_LP_ODR_DIV_100: + *val = LSM6DS3TR_C_XL_LOW_NOISE_LP_ODR_DIV_100; + break; + + case LSM6DS3TR_C_XL_LOW_NOISE_LP_ODR_DIV_9: + *val = LSM6DS3TR_C_XL_LOW_NOISE_LP_ODR_DIV_9; + break; + + case LSM6DS3TR_C_XL_LOW_NOISE_LP_ODR_DIV_400: + *val = LSM6DS3TR_C_XL_LOW_NOISE_LP_ODR_DIV_400; + break; + + default: + *val = LSM6DS3TR_C_XL_LP_NA; + break; + } + } + } + + return ret; +} + +/** + * @brief Enable HP filter reference mode.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of hp_ref_mode in reg CTRL8_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_xl_reference_mode_set(stmdev_ctx_t *ctx, + uint8_t val) +{ + lsm6ds3tr_c_ctrl8_xl_t ctrl8_xl; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL8_XL, + (uint8_t *)&ctrl8_xl, 1); + + if (ret == 0) + { + ctrl8_xl.hp_ref_mode = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL8_XL, + (uint8_t *)&ctrl8_xl, 1); + } + + return ret; +} + +/** + * @brief Enable HP filter reference mode.[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of hp_ref_mode in reg CTRL8_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_xl_reference_mode_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3tr_c_ctrl8_xl_t ctrl8_xl; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL8_XL, + (uint8_t *)&ctrl8_xl, 1); + *val = ctrl8_xl.hp_ref_mode; + + return ret; +} + +/** + * @brief High pass/Slope on outputs.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of hpcf_xl in reg CTRL8_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_xl_hp_bandwidth_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_hpcf_xl_t val) +{ + lsm6ds3tr_c_ctrl8_xl_t ctrl8_xl; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL8_XL, + (uint8_t *)&ctrl8_xl, 1); + + if (ret == 0) + { + ctrl8_xl.input_composite = 0; + ctrl8_xl.hpcf_xl = (uint8_t)val & 0x03U; + ctrl8_xl.hp_slope_xl_en = 1; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL8_XL, + (uint8_t *)&ctrl8_xl, 1); + } + + return ret; +} + +/** + * @brief High pass/Slope on outputs.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of hpcf_xl in reg CTRL8_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_xl_hp_bandwidth_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_hpcf_xl_t *val) +{ + lsm6ds3tr_c_ctrl8_xl_t ctrl8_xl; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL8_XL, + (uint8_t *)&ctrl8_xl, 1); + + if (ctrl8_xl.hp_slope_xl_en == 0x00U) + { + *val = LSM6DS3TR_C_XL_HP_NA; + } + + switch (ctrl8_xl.hpcf_xl) + { + case LSM6DS3TR_C_XL_HP_ODR_DIV_4: + *val = LSM6DS3TR_C_XL_HP_ODR_DIV_4; + break; + + case LSM6DS3TR_C_XL_HP_ODR_DIV_100: + *val = LSM6DS3TR_C_XL_HP_ODR_DIV_100; + break; + + case LSM6DS3TR_C_XL_HP_ODR_DIV_9: + *val = LSM6DS3TR_C_XL_HP_ODR_DIV_9; + break; + + case LSM6DS3TR_C_XL_HP_ODR_DIV_400: + *val = LSM6DS3TR_C_XL_HP_ODR_DIV_400; + break; + + default: + *val = LSM6DS3TR_C_XL_HP_NA; + break; + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3TR_C_gyroscope_filters + * @brief This section group all the functions concerning the filters + * configuration that impact gyroscope. + * @{ + * + */ + +/** + * @brief Gyroscope low pass path bandwidth.[set] + * + * @param ctx Read / write interface definitions + * @param val gyroscope filtering chain configuration. + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_gy_band_pass_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_lpf1_sel_g_t val) +{ + lsm6ds3tr_c_ctrl4_c_t ctrl4_c; + lsm6ds3tr_c_ctrl6_c_t ctrl6_c; + lsm6ds3tr_c_ctrl7_g_t ctrl7_g; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL7_G, + (uint8_t *)&ctrl7_g, 1); + + if (ret == 0) + { + ctrl7_g.hpm_g = ((uint8_t)val & 0x30U) >> 4; + ctrl7_g.hp_en_g = ((uint8_t)val & 0x80U) >> 7; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL7_G, + (uint8_t *)&ctrl7_g, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL6_C, + (uint8_t *)&ctrl6_c, 1); + + if (ret == 0) + { + ctrl6_c.ftype = (uint8_t)val & 0x03U; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL6_C, + (uint8_t *)&ctrl6_c, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL4_C, + (uint8_t *)&ctrl4_c, 1); + + if (ret == 0) + { + ctrl4_c.lpf1_sel_g = ((uint8_t)val & 0x08U) >> 3; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL4_C, + (uint8_t *)&ctrl4_c, 1); + } + } + } + } + } + + return ret; +} + +/** + * @brief Gyroscope low pass path bandwidth.[get] + * + * @param ctx Read / write interface definitions + * @param val gyroscope filtering chain + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_gy_band_pass_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_lpf1_sel_g_t *val) +{ + lsm6ds3tr_c_ctrl4_c_t ctrl4_c; + lsm6ds3tr_c_ctrl6_c_t ctrl6_c; + lsm6ds3tr_c_ctrl7_g_t ctrl7_g; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL6_C, + (uint8_t *)&ctrl6_c, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL4_C, + (uint8_t *)&ctrl4_c, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL7_G, + (uint8_t *)&ctrl7_g, 1); + + switch ((ctrl7_g.hp_en_g << 7) + (ctrl7_g.hpm_g << 4) + + (ctrl4_c.lpf1_sel_g << 3) + ctrl6_c.ftype) + { + case LSM6DS3TR_C_HP_16mHz_LP2: + *val = LSM6DS3TR_C_HP_16mHz_LP2; + break; + + case LSM6DS3TR_C_HP_65mHz_LP2: + *val = LSM6DS3TR_C_HP_65mHz_LP2; + break; + + case LSM6DS3TR_C_HP_260mHz_LP2: + *val = LSM6DS3TR_C_HP_260mHz_LP2; + break; + + case LSM6DS3TR_C_HP_1Hz04_LP2: + *val = LSM6DS3TR_C_HP_1Hz04_LP2; + break; + + case LSM6DS3TR_C_HP_DISABLE_LP1_LIGHT: + *val = LSM6DS3TR_C_HP_DISABLE_LP1_LIGHT; + break; + + case LSM6DS3TR_C_HP_DISABLE_LP1_NORMAL: + *val = LSM6DS3TR_C_HP_DISABLE_LP1_NORMAL; + break; + + case LSM6DS3TR_C_HP_DISABLE_LP_STRONG: + *val = LSM6DS3TR_C_HP_DISABLE_LP_STRONG; + break; + + case LSM6DS3TR_C_HP_DISABLE_LP1_AGGRESSIVE: + *val = LSM6DS3TR_C_HP_DISABLE_LP1_AGGRESSIVE; + break; + + case LSM6DS3TR_C_HP_16mHz_LP1_LIGHT: + *val = LSM6DS3TR_C_HP_16mHz_LP1_LIGHT; + break; + + case LSM6DS3TR_C_HP_65mHz_LP1_NORMAL: + *val = LSM6DS3TR_C_HP_65mHz_LP1_NORMAL; + break; + + case LSM6DS3TR_C_HP_260mHz_LP1_STRONG: + *val = LSM6DS3TR_C_HP_260mHz_LP1_STRONG; + break; + + case LSM6DS3TR_C_HP_1Hz04_LP1_AGGRESSIVE: + *val = LSM6DS3TR_C_HP_1Hz04_LP1_AGGRESSIVE; + break; + + default: + *val = LSM6DS3TR_C_HP_GY_BAND_NA; + break; + } + } + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3TR_C_serial_interface + * @brief This section groups all the functions concerning serial + * interface management + * @{ + * + */ + +/** + * @brief SPI Serial Interface Mode selection.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of sim in reg CTRL3_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_spi_mode_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_sim_t val) +{ + lsm6ds3tr_c_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL3_C, + (uint8_t *)&ctrl3_c, 1); + + if (ret == 0) + { + ctrl3_c.sim = (uint8_t) val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL3_C, + (uint8_t *)&ctrl3_c, 1); + } + + return ret; +} + +/** + * @brief SPI Serial Interface Mode selection.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of sim in reg CTRL3_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_spi_mode_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_sim_t *val) +{ + lsm6ds3tr_c_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL3_C, + (uint8_t *)&ctrl3_c, 1); + + switch (ctrl3_c.sim) + { + case LSM6DS3TR_C_SPI_4_WIRE: + *val = LSM6DS3TR_C_SPI_4_WIRE; + break; + + case LSM6DS3TR_C_SPI_3_WIRE: + *val = LSM6DS3TR_C_SPI_3_WIRE; + break; + + default: + *val = LSM6DS3TR_C_SPI_MODE_ND; + break; + } + + return ret; +} + +/** + * @brief Disable / Enable I2C interface.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of i2c_disable in reg CTRL4_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_i2c_interface_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_i2c_disable_t val) +{ + lsm6ds3tr_c_ctrl4_c_t ctrl4_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL4_C, + (uint8_t *)&ctrl4_c, 1); + + if (ret == 0) + { + ctrl4_c.i2c_disable = (uint8_t)val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL4_C, + (uint8_t *)&ctrl4_c, 1); + } + + return ret; +} + +/** + * @brief Disable / Enable I2C interface.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of i2c_disable in reg CTRL4_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_i2c_interface_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_i2c_disable_t *val) +{ + lsm6ds3tr_c_ctrl4_c_t ctrl4_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL4_C, + (uint8_t *)&ctrl4_c, 1); + + switch (ctrl4_c.i2c_disable) + { + case LSM6DS3TR_C_I2C_ENABLE: + *val = LSM6DS3TR_C_I2C_ENABLE; + break; + + case LSM6DS3TR_C_I2C_DISABLE: + *val = LSM6DS3TR_C_I2C_DISABLE; + break; + + default: + *val = LSM6DS3TR_C_I2C_MODE_ND; + break; + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3TR_C_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 + * @param val configure INT1_CTRL, MD1_CFG, CTRL4_C(den_drdy_int1), + * MASTER_CONFIG(drdy_on_int1) + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_pin_int1_route_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_int1_route_t val) +{ + lsm6ds3tr_c_master_config_t master_config; + lsm6ds3tr_c_int1_ctrl_t int1_ctrl; + lsm6ds3tr_c_md1_cfg_t md1_cfg; + lsm6ds3tr_c_md2_cfg_t md2_cfg; + lsm6ds3tr_c_ctrl4_c_t ctrl4_c; + lsm6ds3tr_c_tap_cfg_t tap_cfg; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_INT1_CTRL, + (uint8_t *)&int1_ctrl, 1); + + if (ret == 0) + { + int1_ctrl.int1_drdy_xl = val.int1_drdy_xl; + int1_ctrl.int1_drdy_g = val.int1_drdy_g; + int1_ctrl.int1_boot = val.int1_boot; + int1_ctrl.int1_fth = val.int1_fth; + int1_ctrl.int1_fifo_ovr = val.int1_fifo_ovr; + int1_ctrl.int1_full_flag = val.int1_full_flag; + int1_ctrl.int1_sign_mot = val.int1_sign_mot; + int1_ctrl.int1_step_detector = val.int1_step_detector; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_INT1_CTRL, + (uint8_t *)&int1_ctrl, 1); + } + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_MD1_CFG, + (uint8_t *)&md1_cfg, 1); + } + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_MD2_CFG, + (uint8_t *)&md2_cfg, 1); + } + + if (ret == 0) + { + md1_cfg.int1_timer = val.int1_timer; + md1_cfg.int1_tilt = val.int1_tilt; + md1_cfg.int1_6d = val.int1_6d; + md1_cfg.int1_double_tap = val.int1_double_tap; + md1_cfg.int1_ff = val.int1_ff; + md1_cfg.int1_wu = val.int1_wu; + md1_cfg.int1_single_tap = val.int1_single_tap; + md1_cfg.int1_inact_state = val.int1_inact_state; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_MD1_CFG, + (uint8_t *)&md1_cfg, 1); + } + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL4_C, + (uint8_t *)&ctrl4_c, 1); + } + + if (ret == 0) + { + ctrl4_c.den_drdy_int1 = val.den_drdy_int1; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL4_C, + (uint8_t *)&ctrl4_c, 1); + } + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + } + + if (ret == 0) + { + master_config.drdy_on_int1 = val.den_drdy_int1; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + } + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_TAP_CFG, + (uint8_t *)&tap_cfg, 1); + + if ((val.int1_6d != 0x00U) || + (val.int1_ff != 0x00U) || + (val.int1_wu != 0x00U) || + (val.int1_single_tap != 0x00U) || + (val.int1_double_tap != 0x00U) || + (val.int1_inact_state != 0x00U) || + (md2_cfg.int2_6d != 0x00U) || + (md2_cfg.int2_ff != 0x00U) || + (md2_cfg.int2_wu != 0x00U) || + (md2_cfg.int2_single_tap != 0x00U) || + (md2_cfg.int2_double_tap != 0x00U) || + (md2_cfg.int2_inact_state != 0x00U)) + { + tap_cfg.interrupts_enable = PROPERTY_ENABLE; + } + + else + { + tap_cfg.interrupts_enable = PROPERTY_DISABLE; + } + } + + if (ret == 0) + { + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_TAP_CFG, + (uint8_t *)&tap_cfg, 1); + } + + return ret; +} + +/** + * @brief Select the signal that need to route on int1 pad[get] + * + * @param ctx Read / write interface definitions + * @param val read INT1_CTRL, MD1_CFG, CTRL4_C(den_drdy_int1), + * MASTER_CONFIG(drdy_on_int1) + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_pin_int1_route_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_int1_route_t *val) +{ + lsm6ds3tr_c_master_config_t master_config; + lsm6ds3tr_c_int1_ctrl_t int1_ctrl; + lsm6ds3tr_c_md1_cfg_t md1_cfg; + lsm6ds3tr_c_ctrl4_c_t ctrl4_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_INT1_CTRL, + (uint8_t *)&int1_ctrl, 1); + + if (ret == 0) + { + val->int1_drdy_xl = int1_ctrl.int1_drdy_xl; + val->int1_drdy_g = int1_ctrl.int1_drdy_g; + val->int1_boot = int1_ctrl.int1_boot; + val->int1_fth = int1_ctrl.int1_fth; + val->int1_fifo_ovr = int1_ctrl.int1_fifo_ovr; + val->int1_full_flag = int1_ctrl.int1_full_flag; + val->int1_sign_mot = int1_ctrl.int1_sign_mot; + val->int1_step_detector = int1_ctrl.int1_step_detector ; + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_MD1_CFG, + (uint8_t *)&md1_cfg, 1); + + if (ret == 0) + { + val->int1_timer = md1_cfg.int1_timer; + val->int1_tilt = md1_cfg.int1_tilt; + val->int1_6d = md1_cfg.int1_6d; + val->int1_double_tap = md1_cfg.int1_double_tap; + val->int1_ff = md1_cfg.int1_ff; + val->int1_wu = md1_cfg.int1_wu; + val->int1_single_tap = md1_cfg.int1_single_tap; + val->int1_inact_state = md1_cfg.int1_inact_state; + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL4_C, + (uint8_t *)&ctrl4_c, 1); + + if (ret == 0) + { + val->den_drdy_int1 = ctrl4_c.den_drdy_int1; + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + val->den_drdy_int1 = master_config.drdy_on_int1; + } + } + } + + return ret; +} + +/** + * @brief Select the signal that need to route on int2 pad[set] + * + * @param ctx Read / write interface definitions + * @param val INT2_CTRL, DRDY_PULSE_CFG(int2_wrist_tilt), MD2_CFG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_pin_int2_route_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_int2_route_t val) +{ + lsm6ds3tr_c_int2_ctrl_t int2_ctrl; + lsm6ds3tr_c_md1_cfg_t md1_cfg; + lsm6ds3tr_c_md2_cfg_t md2_cfg; + lsm6ds3tr_c_drdy_pulse_cfg_g_t drdy_pulse_cfg_g; + lsm6ds3tr_c_tap_cfg_t tap_cfg; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_INT2_CTRL, + (uint8_t *)&int2_ctrl, 1); + + if (ret == 0) + { + int2_ctrl.int2_drdy_xl = val.int2_drdy_xl; + int2_ctrl.int2_drdy_g = val.int2_drdy_g; + int2_ctrl.int2_drdy_temp = val.int2_drdy_temp; + int2_ctrl.int2_fth = val.int2_fth; + int2_ctrl.int2_fifo_ovr = val.int2_fifo_ovr; + int2_ctrl.int2_full_flag = val.int2_full_flag; + int2_ctrl.int2_step_count_ov = val.int2_step_count_ov; + int2_ctrl.int2_step_delta = val.int2_step_delta; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_INT2_CTRL, + (uint8_t *)&int2_ctrl, 1); + } + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_MD1_CFG, + (uint8_t *)&md1_cfg, 1); + } + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_MD2_CFG, + (uint8_t *)&md2_cfg, 1); + } + + if (ret == 0) + { + md2_cfg.int2_iron = val.int2_iron; + md2_cfg.int2_tilt = val.int2_tilt; + md2_cfg.int2_6d = val.int2_6d; + md2_cfg.int2_double_tap = val.int2_double_tap; + md2_cfg.int2_ff = val.int2_ff; + md2_cfg.int2_wu = val.int2_wu; + md2_cfg.int2_single_tap = val.int2_single_tap; + md2_cfg.int2_inact_state = val.int2_inact_state; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_MD2_CFG, + (uint8_t *)&md2_cfg, 1); + } + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_DRDY_PULSE_CFG_G, + (uint8_t *)&drdy_pulse_cfg_g, 1); + } + + if (ret == 0) + { + drdy_pulse_cfg_g.int2_wrist_tilt = val.int2_wrist_tilt; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_DRDY_PULSE_CFG_G, + (uint8_t *)&drdy_pulse_cfg_g, 1); + } + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_TAP_CFG, + (uint8_t *)&tap_cfg, 1); + + if ((md1_cfg.int1_6d != 0x00U) || + (md1_cfg.int1_ff != 0x00U) || + (md1_cfg.int1_wu != 0x00U) || + (md1_cfg.int1_single_tap != 0x00U) || + (md1_cfg.int1_double_tap != 0x00U) || + (md1_cfg.int1_inact_state != 0x00U) || + (val.int2_6d != 0x00U) || + (val.int2_ff != 0x00U) || + (val.int2_wu != 0x00U) || + (val.int2_single_tap != 0x00U) || + (val.int2_double_tap != 0x00U) || + (val.int2_inact_state != 0x00U)) + { + tap_cfg.interrupts_enable = PROPERTY_ENABLE; + } + + else + { + tap_cfg.interrupts_enable = PROPERTY_DISABLE; + } + } + + if (ret == 0) + { + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_TAP_CFG, + (uint8_t *)&tap_cfg, 1); + } + + return ret; +} + +/** + * @brief Select the signal that need to route on int2 pad[get] + * + * @param ctx Read / write interface definitions + * @param val INT2_CTRL, DRDY_PULSE_CFG(int2_wrist_tilt), MD2_CFG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_pin_int2_route_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_int2_route_t *val) +{ + lsm6ds3tr_c_int2_ctrl_t int2_ctrl; + lsm6ds3tr_c_md2_cfg_t md2_cfg; + lsm6ds3tr_c_drdy_pulse_cfg_g_t drdy_pulse_cfg_g; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_INT2_CTRL, + (uint8_t *)&int2_ctrl, 1); + + if (ret == 0) + { + val->int2_drdy_xl = int2_ctrl.int2_drdy_xl; + val->int2_drdy_g = int2_ctrl.int2_drdy_g; + val->int2_drdy_temp = int2_ctrl.int2_drdy_temp; + val->int2_fth = int2_ctrl.int2_fth; + val->int2_fifo_ovr = int2_ctrl.int2_fifo_ovr; + val->int2_full_flag = int2_ctrl.int2_full_flag; + val->int2_step_count_ov = int2_ctrl.int2_step_count_ov; + val->int2_step_delta = int2_ctrl.int2_step_delta; + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_MD2_CFG, + (uint8_t *)&md2_cfg, 1); + + if (ret == 0) + { + val->int2_iron = md2_cfg.int2_iron; + val->int2_tilt = md2_cfg.int2_tilt; + val->int2_6d = md2_cfg.int2_6d; + val->int2_double_tap = md2_cfg.int2_double_tap; + val->int2_ff = md2_cfg.int2_ff; + val->int2_wu = md2_cfg.int2_wu; + val->int2_single_tap = md2_cfg.int2_single_tap; + val->int2_inact_state = md2_cfg.int2_inact_state; + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_DRDY_PULSE_CFG_G, + (uint8_t *)&drdy_pulse_cfg_g, 1); + val->int2_wrist_tilt = drdy_pulse_cfg_g.int2_wrist_tilt; + } + } + + return ret; +} + +/** + * @brief Push-pull/open drain selection on interrupt pads.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of pp_od in reg CTRL3_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_pin_mode_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_pp_od_t val) +{ + lsm6ds3tr_c_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL3_C, + (uint8_t *)&ctrl3_c, 1); + + if (ret == 0) + { + ctrl3_c.pp_od = (uint8_t) val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL3_C, + (uint8_t *)&ctrl3_c, 1); + } + + return ret; +} + +/** + * @brief Push-pull/open drain selection on interrupt pads.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of pp_od in reg CTRL3_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_pin_mode_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_pp_od_t *val) +{ + lsm6ds3tr_c_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL3_C, + (uint8_t *)&ctrl3_c, 1); + + switch (ctrl3_c.pp_od) + { + case LSM6DS3TR_C_PUSH_PULL: + *val = LSM6DS3TR_C_PUSH_PULL; + break; + + case LSM6DS3TR_C_OPEN_DRAIN: + *val = LSM6DS3TR_C_OPEN_DRAIN; + break; + + default: + *val = LSM6DS3TR_C_PIN_MODE_ND; + break; + } + + return ret; +} + +/** + * @brief Interrupt active-high/low.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of h_lactive in reg CTRL3_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_pin_polarity_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_h_lactive_t val) +{ + lsm6ds3tr_c_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL3_C, + (uint8_t *)&ctrl3_c, 1); + + if (ret == 0) + { + ctrl3_c.h_lactive = (uint8_t) val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL3_C, + (uint8_t *)&ctrl3_c, 1); + } + + return ret; +} + +/** + * @brief Interrupt active-high/low.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of h_lactive in reg CTRL3_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_pin_polarity_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_h_lactive_t *val) +{ + lsm6ds3tr_c_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL3_C, + (uint8_t *)&ctrl3_c, 1); + + switch (ctrl3_c.h_lactive) + { + case LSM6DS3TR_C_ACTIVE_HIGH: + *val = LSM6DS3TR_C_ACTIVE_HIGH; + break; + + case LSM6DS3TR_C_ACTIVE_LOW: + *val = LSM6DS3TR_C_ACTIVE_LOW; + break; + + default: + *val = LSM6DS3TR_C_POLARITY_ND; + break; + } + + return ret; +} + +/** + * @brief All interrupt signals become available on INT1 pin.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of int2_on_int1 in reg CTRL4_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_all_on_int1_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3tr_c_ctrl4_c_t ctrl4_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL4_C, + (uint8_t *)&ctrl4_c, 1); + + if (ret == 0) + { + ctrl4_c.int2_on_int1 = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL4_C, + (uint8_t *)&ctrl4_c, 1); + } + + return ret; +} + +/** + * @brief All interrupt signals become available on INT1 pin.[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of int2_on_int1 in reg CTRL4_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_all_on_int1_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3tr_c_ctrl4_c_t ctrl4_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL4_C, + (uint8_t *)&ctrl4_c, 1); + *val = ctrl4_c.int2_on_int1; + + return ret; +} + +/** + * @brief Latched/pulsed interrupt.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of lir in reg TAP_CFG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_int_notification_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_lir_t val) +{ + lsm6ds3tr_c_tap_cfg_t tap_cfg; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_TAP_CFG, + (uint8_t *)&tap_cfg, 1); + + if (ret == 0) + { + tap_cfg.lir = (uint8_t) val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_TAP_CFG, + (uint8_t *)&tap_cfg, 1); + } + + return ret; +} + +/** + * @brief Latched/pulsed interrupt.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of lir in reg TAP_CFG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_int_notification_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_lir_t *val) +{ + lsm6ds3tr_c_tap_cfg_t tap_cfg; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_TAP_CFG, + (uint8_t *)&tap_cfg, 1); + + switch (tap_cfg.lir) + { + case LSM6DS3TR_C_INT_PULSED: + *val = LSM6DS3TR_C_INT_PULSED; + break; + + case LSM6DS3TR_C_INT_LATCHED: + *val = LSM6DS3TR_C_INT_LATCHED; + break; + + default: + *val = LSM6DS3TR_C_INT_MODE; + break; + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3TR_C_Wake_Up_event + * @brief This section groups all the functions that manage the + * Wake Up event generation. + * @{ + * + */ + +/** + * @brief Threshold for wakeup.1 LSB = FS_XL / 64.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of wk_ths in reg WAKE_UP_THS + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_wkup_threshold_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3tr_c_wake_up_ths_t wake_up_ths; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_WAKE_UP_THS, + (uint8_t *)&wake_up_ths, 1); + + if (ret == 0) + { + wake_up_ths.wk_ths = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_WAKE_UP_THS, + (uint8_t *)&wake_up_ths, 1); + } + + return ret; +} + +/** + * @brief Threshold for wakeup.1 LSB = FS_XL / 64.[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of wk_ths in reg WAKE_UP_THS + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_wkup_threshold_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3tr_c_wake_up_ths_t wake_up_ths; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_WAKE_UP_THS, + (uint8_t *)&wake_up_ths, 1); + *val = wake_up_ths.wk_ths; + + return ret; +} + +/** + * @brief Wake up duration event.1LSb = 1 / ODR[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of wake_dur in reg WAKE_UP_DUR + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_wkup_dur_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3tr_c_wake_up_dur_t wake_up_dur; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_WAKE_UP_DUR, + (uint8_t *)&wake_up_dur, 1); + + if (ret == 0) + { + wake_up_dur.wake_dur = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_WAKE_UP_DUR, + (uint8_t *)&wake_up_dur, 1); + } + + return ret; +} + +/** + * @brief Wake up duration event.1LSb = 1 / ODR[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of wake_dur in reg WAKE_UP_DUR + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_wkup_dur_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3tr_c_wake_up_dur_t wake_up_dur; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_WAKE_UP_DUR, + (uint8_t *)&wake_up_dur, 1); + *val = wake_up_dur.wake_dur; + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3TR_C_Activity/Inactivity_detection + * @brief This section groups all the functions concerning + * activity/inactivity detection. + * @{ + * + */ + +/** + * @brief Enables gyroscope Sleep mode.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of sleep in reg CTRL4_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_gy_sleep_mode_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3tr_c_ctrl4_c_t ctrl4_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL4_C, + (uint8_t *)&ctrl4_c, 1); + + if (ret == 0) + { + ctrl4_c.sleep = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL4_C, + (uint8_t *)&ctrl4_c, 1); + } + + return ret; +} + +/** + * @brief Enables gyroscope Sleep mode.[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of sleep in reg CTRL4_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_gy_sleep_mode_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3tr_c_ctrl4_c_t ctrl4_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL4_C, + (uint8_t *)&ctrl4_c, 1); + *val = ctrl4_c.sleep; + + return ret; +} + +/** + * @brief Enable inactivity function.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of inact_en in reg TAP_CFG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_act_mode_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_inact_en_t val) +{ + lsm6ds3tr_c_tap_cfg_t tap_cfg; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_TAP_CFG, + (uint8_t *)&tap_cfg, 1); + + if (ret == 0) + { + tap_cfg.inact_en = (uint8_t) val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_TAP_CFG, + (uint8_t *)&tap_cfg, 1); + } + + return ret; +} + +/** + * @brief Enable inactivity function.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of inact_en in reg TAP_CFG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_act_mode_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_inact_en_t *val) +{ + lsm6ds3tr_c_tap_cfg_t tap_cfg; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_TAP_CFG, + (uint8_t *)&tap_cfg, 1); + + switch (tap_cfg.inact_en) + { + case LSM6DS3TR_C_PROPERTY_DISABLE: + *val = LSM6DS3TR_C_PROPERTY_DISABLE; + break; + + case LSM6DS3TR_C_XL_12Hz5_GY_NOT_AFFECTED: + *val = LSM6DS3TR_C_XL_12Hz5_GY_NOT_AFFECTED; + break; + + case LSM6DS3TR_C_XL_12Hz5_GY_SLEEP: + *val = LSM6DS3TR_C_XL_12Hz5_GY_SLEEP; + break; + + case LSM6DS3TR_C_XL_12Hz5_GY_PD: + *val = LSM6DS3TR_C_XL_12Hz5_GY_PD; + break; + + default: + *val = LSM6DS3TR_C_ACT_MODE_ND; + break; + } + + return ret; +} + +/** + * @brief Duration to go in sleep mode.1 LSb = 512 / ODR[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of sleep_dur in reg WAKE_UP_DUR + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_act_sleep_dur_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3tr_c_wake_up_dur_t wake_up_dur; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_WAKE_UP_DUR, + (uint8_t *)&wake_up_dur, 1); + + if (ret == 0) + { + wake_up_dur.sleep_dur = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_WAKE_UP_DUR, + (uint8_t *)&wake_up_dur, 1); + } + + return ret; +} + +/** + * @brief Duration to go in sleep mode. 1 LSb = 512 / ODR[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of sleep_dur in reg WAKE_UP_DUR + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_act_sleep_dur_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3tr_c_wake_up_dur_t wake_up_dur; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_WAKE_UP_DUR, + (uint8_t *)&wake_up_dur, 1); + *val = wake_up_dur.sleep_dur; + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3TR_C_tap_generator + * @brief This section groups all the functions that manage the + * tap and double tap event generation. + * @{ + * + */ + +/** + * @brief Read the tap / double tap source register.[get] + * + * @param ctx Read / write interface definitions + * @param val Structure of registers from TAP_SRC + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_tap_src_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_tap_src_t *val) +{ + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_TAP_SRC, (uint8_t *) val, 1); + + return ret; +} + +/** + * @brief Enable Z direction in tap recognition.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of tap_z_en in reg TAP_CFG + * + */ +int32_t lsm6ds3tr_c_tap_detection_on_z_set(stmdev_ctx_t *ctx, + uint8_t val) +{ + lsm6ds3tr_c_tap_cfg_t tap_cfg; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_TAP_CFG, + (uint8_t *)&tap_cfg, 1); + + if (ret == 0) + { + tap_cfg.tap_z_en = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_TAP_CFG, + (uint8_t *)&tap_cfg, 1); + } + + return ret; +} + +/** + * @brief Enable Z direction in tap recognition.[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of tap_z_en in reg TAP_CFG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_tap_detection_on_z_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3tr_c_tap_cfg_t tap_cfg; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_TAP_CFG, + (uint8_t *)&tap_cfg, 1); + *val = tap_cfg.tap_z_en; + + return ret; +} + +/** + * @brief Enable Y direction in tap recognition.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of tap_y_en in reg TAP_CFG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_tap_detection_on_y_set(stmdev_ctx_t *ctx, + uint8_t val) +{ + lsm6ds3tr_c_tap_cfg_t tap_cfg; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_TAP_CFG, + (uint8_t *)&tap_cfg, 1); + + if (ret == 0) + { + tap_cfg.tap_y_en = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_TAP_CFG, + (uint8_t *)&tap_cfg, 1); + } + + return ret; +} + +/** + * @brief Enable Y direction in tap recognition.[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of tap_y_en in reg TAP_CFG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_tap_detection_on_y_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3tr_c_tap_cfg_t tap_cfg; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_TAP_CFG, + (uint8_t *)&tap_cfg, 1); + *val = tap_cfg.tap_y_en; + + return ret; +} + +/** + * @brief Enable X direction in tap recognition.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of tap_x_en in reg TAP_CFG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_tap_detection_on_x_set(stmdev_ctx_t *ctx, + uint8_t val) +{ + lsm6ds3tr_c_tap_cfg_t tap_cfg; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_TAP_CFG, + (uint8_t *)&tap_cfg, 1); + + if (ret == 0) + { + tap_cfg.tap_x_en = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_TAP_CFG, + (uint8_t *)&tap_cfg, 1); + } + + return ret; +} + +/** + * @brief Enable X direction in tap recognition.[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of tap_x_en in reg TAP_CFG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_tap_detection_on_x_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3tr_c_tap_cfg_t tap_cfg; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_TAP_CFG, + (uint8_t *)&tap_cfg, 1); + *val = tap_cfg.tap_x_en; + + return ret; +} + +/** + * @brief Threshold for tap recognition.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of tap_ths in reg TAP_THS_6D + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_tap_threshold_x_set(stmdev_ctx_t *ctx, + uint8_t val) +{ + lsm6ds3tr_c_tap_ths_6d_t tap_ths_6d; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_TAP_THS_6D, + (uint8_t *)&tap_ths_6d, 1); + + if (ret == 0) + { + tap_ths_6d.tap_ths = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_TAP_THS_6D, + (uint8_t *)&tap_ths_6d, 1); + } + + return ret; +} + +/** + * @brief Threshold for tap recognition.[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of tap_ths in reg TAP_THS_6D + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_tap_threshold_x_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3tr_c_tap_ths_6d_t tap_ths_6d; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_TAP_THS_6D, + (uint8_t *)&tap_ths_6d, 1); + *val = tap_ths_6d.tap_ths; + + return ret; +} + +/** + * @brief 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[1:0] bits are set to a different + * value, 1LSB corresponds to 8*ODR_XL time.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of shock in reg INT_DUR2 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_tap_shock_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3tr_c_int_dur2_t int_dur2; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_INT_DUR2, + (uint8_t *)&int_dur2, 1); + + if (ret == 0) + { + int_dur2.shock = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_INT_DUR2, + (uint8_t *)&int_dur2, 1); + } + + return ret; +} + +/** + * @brief 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[1:0] bits are set to a different value, 1LSB + * corresponds to 8*ODR_XL time.[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of shock in reg INT_DUR2 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_tap_shock_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3tr_c_int_dur2_t int_dur2; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_INT_DUR2, + (uint8_t *)&int_dur2, 1); + *val = int_dur2.shock; + + return ret; +} + +/** + * @brief 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[1:0] bits are set to a different value, 1LSB + * corresponds to 4*ODR_XL time.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of quiet in reg INT_DUR2 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_tap_quiet_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3tr_c_int_dur2_t int_dur2; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_INT_DUR2, + (uint8_t *)&int_dur2, 1); + + if (ret == 0) + { + int_dur2.quiet = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_INT_DUR2, + (uint8_t *)&int_dur2, 1); + } + + return ret; +} + +/** + * @brief 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[1:0] bits are set to a different value, 1LSB + * corresponds to 4*ODR_XL time.[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of quiet in reg INT_DUR2 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_tap_quiet_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3tr_c_int_dur2_t int_dur2; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_INT_DUR2, + (uint8_t *)&int_dur2, 1); + *val = int_dur2.quiet; + + return ret; +} + +/** + * @brief 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 Change the values of dur in reg INT_DUR2 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_tap_dur_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3tr_c_int_dur2_t int_dur2; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_INT_DUR2, + (uint8_t *)&int_dur2, 1); + + if (ret == 0) + { + int_dur2.dur = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_INT_DUR2, + (uint8_t *)&int_dur2, 1); + } + + return ret; +} + +/** + * @brief 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 Change the values of dur in reg INT_DUR2 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_tap_dur_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3tr_c_int_dur2_t int_dur2; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_INT_DUR2, + (uint8_t *)&int_dur2, 1); + *val = int_dur2.dur; + + return ret; +} + +/** + * @brief Single/double-tap event enable/disable.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of + * single_double_tap in reg WAKE_UP_THS + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_tap_mode_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_single_double_tap_t val) +{ + lsm6ds3tr_c_wake_up_ths_t wake_up_ths; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_WAKE_UP_THS, + (uint8_t *)&wake_up_ths, 1); + + if (ret == 0) + { + wake_up_ths.single_double_tap = (uint8_t) val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_WAKE_UP_THS, + (uint8_t *)&wake_up_ths, 1); + } + + return ret; +} + +/** + * @brief Single/double-tap event enable/disable.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of single_double_tap + * in reg WAKE_UP_THS + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_tap_mode_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_single_double_tap_t *val) +{ + lsm6ds3tr_c_wake_up_ths_t wake_up_ths; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_WAKE_UP_THS, + (uint8_t *)&wake_up_ths, 1); + + switch (wake_up_ths.single_double_tap) + { + case LSM6DS3TR_C_ONLY_SINGLE: + *val = LSM6DS3TR_C_ONLY_SINGLE; + break; + + case LSM6DS3TR_C_BOTH_SINGLE_DOUBLE: + *val = LSM6DS3TR_C_BOTH_SINGLE_DOUBLE; + break; + + default: + *val = LSM6DS3TR_C_TAP_MODE_ND; + break; + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3TR_C_ Six_position_detection(6D/4D) + * @brief This section groups all the functions concerning six + * position detection (6D). + * @{ + * + */ + +/** + * @brief LPF2 feed 6D function selection.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of low_pass_on_6d in + * reg CTRL8_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_6d_feed_data_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_low_pass_on_6d_t val) +{ + lsm6ds3tr_c_ctrl8_xl_t ctrl8_xl; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL8_XL, + (uint8_t *)&ctrl8_xl, 1); + + if (ret == 0) + { + ctrl8_xl.low_pass_on_6d = (uint8_t) val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL8_XL, + (uint8_t *)&ctrl8_xl, 1); + } + + return ret; +} + +/** + * @brief LPF2 feed 6D function selection.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of low_pass_on_6d in reg CTRL8_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_6d_feed_data_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_low_pass_on_6d_t *val) +{ + lsm6ds3tr_c_ctrl8_xl_t ctrl8_xl; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL8_XL, + (uint8_t *)&ctrl8_xl, 1); + + switch (ctrl8_xl.low_pass_on_6d) + { + case LSM6DS3TR_C_ODR_DIV_2_FEED: + *val = LSM6DS3TR_C_ODR_DIV_2_FEED; + break; + + case LSM6DS3TR_C_LPF2_FEED: + *val = LSM6DS3TR_C_LPF2_FEED; + break; + + default: + *val = LSM6DS3TR_C_6D_FEED_ND; + break; + } + + return ret; +} + +/** + * @brief Threshold for 4D/6D function.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of sixd_ths in reg TAP_THS_6D + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_6d_threshold_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_sixd_ths_t val) +{ + lsm6ds3tr_c_tap_ths_6d_t tap_ths_6d; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_TAP_THS_6D, + (uint8_t *)&tap_ths_6d, 1); + + if (ret == 0) + { + tap_ths_6d.sixd_ths = (uint8_t) val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_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 Get the values of sixd_ths in reg TAP_THS_6D + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_6d_threshold_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_sixd_ths_t *val) +{ + lsm6ds3tr_c_tap_ths_6d_t tap_ths_6d; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_TAP_THS_6D, + (uint8_t *)&tap_ths_6d, 1); + + switch (tap_ths_6d.sixd_ths) + { + case LSM6DS3TR_C_DEG_80: + *val = LSM6DS3TR_C_DEG_80; + break; + + case LSM6DS3TR_C_DEG_70: + *val = LSM6DS3TR_C_DEG_70; + break; + + case LSM6DS3TR_C_DEG_60: + *val = LSM6DS3TR_C_DEG_60; + break; + + case LSM6DS3TR_C_DEG_50: + *val = LSM6DS3TR_C_DEG_50; + break; + + default: + *val = LSM6DS3TR_C_6D_TH_ND; + break; + } + + return ret; +} + +/** + * @brief 4D orientation detection enable.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of d4d_en in reg TAP_THS_6D + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_4d_mode_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3tr_c_tap_ths_6d_t tap_ths_6d; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_TAP_THS_6D, + (uint8_t *)&tap_ths_6d, 1); + + if (ret == 0) + { + tap_ths_6d.d4d_en = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_TAP_THS_6D, + (uint8_t *)&tap_ths_6d, 1); + } + + return ret; +} + +/** + * @brief 4D orientation detection enable.[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of d4d_en in reg TAP_THS_6D + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_4d_mode_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3tr_c_tap_ths_6d_t tap_ths_6d; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_TAP_THS_6D, + (uint8_t *)&tap_ths_6d, 1); + *val = tap_ths_6d.d4d_en; + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3TR_C_free_fall + * @brief This section group all the functions concerning the free + * fall detection. + * @{ + * + */ + +/** + * @brief Free-fall duration event. 1LSb = 1 / ODR[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of ff_dur in reg WAKE_UP_DUR + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_ff_dur_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3tr_c_wake_up_dur_t wake_up_dur; + lsm6ds3tr_c_free_fall_t free_fall; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FREE_FALL, + (uint8_t *)&free_fall, 1); + + if (ret == 0) + { + free_fall.ff_dur = (val & 0x1FU); + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_FREE_FALL, + (uint8_t *)&free_fall, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_WAKE_UP_DUR, + (uint8_t *)&wake_up_dur, 1); + + if (ret == 0) + { + wake_up_dur.ff_dur = (val & 0x20U) >> 5; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_WAKE_UP_DUR, + (uint8_t *)&wake_up_dur, 1); + } + } + } + + return ret; +} + +/** + * @brief Free-fall duration event. 1LSb = 1 / ODR[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of ff_dur in reg WAKE_UP_DUR + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_ff_dur_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3tr_c_wake_up_dur_t wake_up_dur; + lsm6ds3tr_c_free_fall_t free_fall; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_WAKE_UP_DUR, + (uint8_t *)&wake_up_dur, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_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 Change the values of ff_ths in reg FREE_FALL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_ff_threshold_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_ff_ths_t val) +{ + lsm6ds3tr_c_free_fall_t free_fall; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FREE_FALL, + (uint8_t *)&free_fall, 1); + + if (ret == 0) + { + free_fall.ff_ths = (uint8_t) val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_FREE_FALL, + (uint8_t *)&free_fall, 1); + } + + return ret; +} + +/** + * @brief Free fall threshold setting.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of ff_ths in reg FREE_FALL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_ff_threshold_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_ff_ths_t *val) +{ + lsm6ds3tr_c_free_fall_t free_fall; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FREE_FALL, + (uint8_t *)&free_fall, 1); + + switch (free_fall.ff_ths) + { + case LSM6DS3TR_C_FF_TSH_156mg: + *val = LSM6DS3TR_C_FF_TSH_156mg; + break; + + case LSM6DS3TR_C_FF_TSH_219mg: + *val = LSM6DS3TR_C_FF_TSH_219mg; + break; + + case LSM6DS3TR_C_FF_TSH_250mg: + *val = LSM6DS3TR_C_FF_TSH_250mg; + break; + + case LSM6DS3TR_C_FF_TSH_312mg: + *val = LSM6DS3TR_C_FF_TSH_312mg; + break; + + case LSM6DS3TR_C_FF_TSH_344mg: + *val = LSM6DS3TR_C_FF_TSH_344mg; + break; + + case LSM6DS3TR_C_FF_TSH_406mg: + *val = LSM6DS3TR_C_FF_TSH_406mg; + break; + + case LSM6DS3TR_C_FF_TSH_469mg: + *val = LSM6DS3TR_C_FF_TSH_469mg; + break; + + case LSM6DS3TR_C_FF_TSH_500mg: + *val = LSM6DS3TR_C_FF_TSH_500mg; + break; + + default: + *val = LSM6DS3TR_C_FF_TSH_ND; + break; + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3TR_C_fifo + * @brief This section group all the functions concerning the + * fifo usage + * @{ + * + */ + +/** + * @brief FIFO watermark level selection.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of fth in reg FIFO_CTRL1 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_fifo_watermark_set(stmdev_ctx_t *ctx, + uint16_t val) +{ + lsm6ds3tr_c_fifo_ctrl1_t fifo_ctrl1; + lsm6ds3tr_c_fifo_ctrl2_t fifo_ctrl2; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FIFO_CTRL2, + (uint8_t *)&fifo_ctrl2, 1); + + if (ret == 0) + { + fifo_ctrl1.fth = (uint8_t)(0x00FFU & val); + fifo_ctrl2.fth = (uint8_t)((0x0700U & val) >> 8); + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_FIFO_CTRL1, + (uint8_t *)&fifo_ctrl1, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_FIFO_CTRL2, + (uint8_t *)&fifo_ctrl2, 1); + } + } + + return ret; +} + +/** + * @brief FIFO watermark level selection.[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of fth in reg FIFO_CTRL1 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_fifo_watermark_get(stmdev_ctx_t *ctx, + uint16_t *val) +{ + lsm6ds3tr_c_fifo_ctrl1_t fifo_ctrl1; + lsm6ds3tr_c_fifo_ctrl2_t fifo_ctrl2; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FIFO_CTRL1, + (uint8_t *)&fifo_ctrl1, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FIFO_CTRL2, + (uint8_t *)&fifo_ctrl2, 1); + } + + *val = ((uint16_t)fifo_ctrl2.fth << 8) + (uint16_t)fifo_ctrl1.fth; + + return ret; +} + +/** + * @brief FIFO data level.[get] + * + * @param ctx Read / write interface definitions + * @param val get the values of diff_fifo in reg FIFO_STATUS1 and + * FIFO_STATUS2(diff_fifo), it is recommended to set the + * BDU bit. + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_fifo_data_level_get(stmdev_ctx_t *ctx, + uint16_t *val) +{ + lsm6ds3tr_c_fifo_status1_t fifo_status1; + lsm6ds3tr_c_fifo_status2_t fifo_status2; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FIFO_STATUS1, + (uint8_t *)&fifo_status1, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FIFO_STATUS2, + (uint8_t *)&fifo_status2, 1); + *val = ((uint16_t) fifo_status2.diff_fifo << 8) + + (uint16_t) fifo_status1.diff_fifo; + } + + return ret; +} + +/** + * @brief FIFO watermark.[get] + * + * @param ctx Read / write interface definitions + * @param val get the values of watermark in reg FIFO_STATUS2 and + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_fifo_wtm_flag_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3tr_c_fifo_status2_t fifo_status2; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FIFO_STATUS2, + (uint8_t *)&fifo_status2, 1); + *val = fifo_status2.waterm; + + return ret; +} + +/** + * @brief FIFO pattern.[get] + * + * @param ctx Read / write interface definitions + * @param val get the values of fifo_pattern in reg FIFO_STATUS3 and + * FIFO_STATUS4, it is recommended to set the BDU bit + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_fifo_pattern_get(stmdev_ctx_t *ctx, uint16_t *val) +{ + lsm6ds3tr_c_fifo_status3_t fifo_status3; + lsm6ds3tr_c_fifo_status4_t fifo_status4; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FIFO_STATUS3, + (uint8_t *)&fifo_status3, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FIFO_STATUS4, + (uint8_t *)&fifo_status4, 1); + *val = ((uint16_t)fifo_status4.fifo_pattern << 8) + + fifo_status3.fifo_pattern; + } + + return ret; +} + +/** + * @brief Batching of temperature data[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of fifo_temp_en in reg FIFO_CTRL2 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_fifo_temp_batch_set(stmdev_ctx_t *ctx, + uint8_t val) +{ + lsm6ds3tr_c_fifo_ctrl2_t fifo_ctrl2; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FIFO_CTRL2, + (uint8_t *)&fifo_ctrl2, 1); + + if (ret == 0) + { + fifo_ctrl2.fifo_temp_en = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_FIFO_CTRL2, + (uint8_t *)&fifo_ctrl2, 1); + } + + return ret; +} + +/** + * @brief Batching of temperature data[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of fifo_temp_en in reg FIFO_CTRL2 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_fifo_temp_batch_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3tr_c_fifo_ctrl2_t fifo_ctrl2; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FIFO_CTRL2, + (uint8_t *)&fifo_ctrl2, 1); + *val = fifo_ctrl2.fifo_temp_en; + + return ret; +} + +/** + * @brief Trigger signal for FIFO write operation.[set] + * + * @param ctx Read / write interface definitions + * @param val act on FIFO_CTRL2(timer_pedo_fifo_drdy) + * and MASTER_CONFIG(data_valid_sel_fifo) + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_fifo_write_trigger_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_trigger_fifo_t val) +{ + lsm6ds3tr_c_fifo_ctrl2_t fifo_ctrl2; + lsm6ds3tr_c_master_config_t master_config; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FIFO_CTRL2, + (uint8_t *)&fifo_ctrl2, 1); + + if (ret == 0) + { + fifo_ctrl2.timer_pedo_fifo_drdy = (uint8_t)val & 0x01U; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_FIFO_CTRL2, + (uint8_t *)&fifo_ctrl2, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + + if (ret == 0) + { + master_config.data_valid_sel_fifo = (((uint8_t)val & 0x02U) >> 1); + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + } + } + } + + return ret; +} + +/** + * @brief Trigger signal for FIFO write operation.[get] + * + * @param ctx Read / write interface definitions + * @param val act on FIFO_CTRL2(timer_pedo_fifo_drdy) + * and MASTER_CONFIG(data_valid_sel_fifo) + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_fifo_write_trigger_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_trigger_fifo_t *val) +{ + lsm6ds3tr_c_fifo_ctrl2_t fifo_ctrl2; + lsm6ds3tr_c_master_config_t master_config; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FIFO_CTRL2, + (uint8_t *)&fifo_ctrl2, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + + switch ((fifo_ctrl2.timer_pedo_fifo_drdy << 1) + + fifo_ctrl2. timer_pedo_fifo_drdy) + { + case LSM6DS3TR_C_TRG_XL_GY_DRDY: + *val = LSM6DS3TR_C_TRG_XL_GY_DRDY; + break; + + case LSM6DS3TR_C_TRG_STEP_DETECT: + *val = LSM6DS3TR_C_TRG_STEP_DETECT; + break; + + case LSM6DS3TR_C_TRG_SH_DRDY: + *val = LSM6DS3TR_C_TRG_SH_DRDY; + break; + + default: + *val = LSM6DS3TR_C_TRG_SH_ND; + break; + } + } + + return ret; +} + +/** + * @brief Enable pedometer step counter and timestamp as 4th + * FIFO data set.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of timer_pedo_fifo_en in reg FIFO_CTRL2 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_fifo_pedo_and_timestamp_batch_set( + stmdev_ctx_t *ctx, + uint8_t val) +{ + lsm6ds3tr_c_fifo_ctrl2_t fifo_ctrl2; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FIFO_CTRL2, + (uint8_t *)&fifo_ctrl2, 1); + + if (ret == 0) + { + fifo_ctrl2.timer_pedo_fifo_en = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_FIFO_CTRL2, + (uint8_t *)&fifo_ctrl2, 1); + } + + return ret; +} + +/** + * @brief Enable pedometer step counter and timestamp as 4th + * FIFO data set.[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of timer_pedo_fifo_en in reg FIFO_CTRL2 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_fifo_pedo_and_timestamp_batch_get( + stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3tr_c_fifo_ctrl2_t fifo_ctrl2; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FIFO_CTRL2, + (uint8_t *)&fifo_ctrl2, 1); + *val = fifo_ctrl2.timer_pedo_fifo_en; + + return ret; +} + +/** + * @brief Selects Batching Data Rate (writing frequency in FIFO) for + * accelerometer data.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of dec_fifo_xl in reg FIFO_CTRL3 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_fifo_xl_batch_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_dec_fifo_xl_t val) +{ + lsm6ds3tr_c_fifo_ctrl3_t fifo_ctrl3; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FIFO_CTRL3, + (uint8_t *)&fifo_ctrl3, 1); + + if (ret == 0) + { + fifo_ctrl3.dec_fifo_xl = (uint8_t)val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_FIFO_CTRL3, + (uint8_t *)&fifo_ctrl3, 1); + } + + return ret; +} + +/** + * @brief Selects Batching Data Rate (writing frequency in FIFO) for + * accelerometer data.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of dec_fifo_xl in reg FIFO_CTRL3 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_fifo_xl_batch_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_dec_fifo_xl_t *val) +{ + lsm6ds3tr_c_fifo_ctrl3_t fifo_ctrl3; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FIFO_CTRL3, + (uint8_t *)&fifo_ctrl3, 1); + + switch (fifo_ctrl3.dec_fifo_xl) + { + case LSM6DS3TR_C_FIFO_XL_DISABLE: + *val = LSM6DS3TR_C_FIFO_XL_DISABLE; + break; + + case LSM6DS3TR_C_FIFO_XL_NO_DEC: + *val = LSM6DS3TR_C_FIFO_XL_NO_DEC; + break; + + case LSM6DS3TR_C_FIFO_XL_DEC_2: + *val = LSM6DS3TR_C_FIFO_XL_DEC_2; + break; + + case LSM6DS3TR_C_FIFO_XL_DEC_3: + *val = LSM6DS3TR_C_FIFO_XL_DEC_3; + break; + + case LSM6DS3TR_C_FIFO_XL_DEC_4: + *val = LSM6DS3TR_C_FIFO_XL_DEC_4; + break; + + case LSM6DS3TR_C_FIFO_XL_DEC_8: + *val = LSM6DS3TR_C_FIFO_XL_DEC_8; + break; + + case LSM6DS3TR_C_FIFO_XL_DEC_16: + *val = LSM6DS3TR_C_FIFO_XL_DEC_16; + break; + + case LSM6DS3TR_C_FIFO_XL_DEC_32: + *val = LSM6DS3TR_C_FIFO_XL_DEC_32; + break; + + default: + *val = LSM6DS3TR_C_FIFO_XL_DEC_ND; + break; + } + + return ret; +} + +/** + * @brief Selects Batching Data Rate (writing frequency in FIFO) + * for gyroscope data.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of dec_fifo_gyro in reg FIFO_CTRL3 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_fifo_gy_batch_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_dec_fifo_gyro_t val) +{ + lsm6ds3tr_c_fifo_ctrl3_t fifo_ctrl3; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FIFO_CTRL3, + (uint8_t *)&fifo_ctrl3, 1); + + if (ret == 0) + { + fifo_ctrl3.dec_fifo_gyro = (uint8_t)val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_FIFO_CTRL3, + (uint8_t *)&fifo_ctrl3, 1); + } + + return ret; +} + +/** + * @brief Selects Batching Data Rate (writing frequency in FIFO) + * for gyroscope data.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of dec_fifo_gyro in reg FIFO_CTRL3 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_fifo_gy_batch_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_dec_fifo_gyro_t *val) +{ + lsm6ds3tr_c_fifo_ctrl3_t fifo_ctrl3; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FIFO_CTRL3, + (uint8_t *)&fifo_ctrl3, 1); + + switch (fifo_ctrl3.dec_fifo_gyro) + { + case LSM6DS3TR_C_FIFO_GY_DISABLE: + *val = LSM6DS3TR_C_FIFO_GY_DISABLE; + break; + + case LSM6DS3TR_C_FIFO_GY_NO_DEC: + *val = LSM6DS3TR_C_FIFO_GY_NO_DEC; + break; + + case LSM6DS3TR_C_FIFO_GY_DEC_2: + *val = LSM6DS3TR_C_FIFO_GY_DEC_2; + break; + + case LSM6DS3TR_C_FIFO_GY_DEC_3: + *val = LSM6DS3TR_C_FIFO_GY_DEC_3; + break; + + case LSM6DS3TR_C_FIFO_GY_DEC_4: + *val = LSM6DS3TR_C_FIFO_GY_DEC_4; + break; + + case LSM6DS3TR_C_FIFO_GY_DEC_8: + *val = LSM6DS3TR_C_FIFO_GY_DEC_8; + break; + + case LSM6DS3TR_C_FIFO_GY_DEC_16: + *val = LSM6DS3TR_C_FIFO_GY_DEC_16; + break; + + case LSM6DS3TR_C_FIFO_GY_DEC_32: + *val = LSM6DS3TR_C_FIFO_GY_DEC_32; + break; + + default: + *val = LSM6DS3TR_C_FIFO_GY_DEC_ND; + break; + } + + return ret; +} + +/** + * @brief Selects Batching Data Rate (writing frequency in FIFO) + * for third data set.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of dec_ds3_fifo in reg FIFO_CTRL4 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_fifo_dataset_3_batch_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_dec_ds3_fifo_t val) +{ + lsm6ds3tr_c_fifo_ctrl4_t fifo_ctrl4; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FIFO_CTRL4, + (uint8_t *)&fifo_ctrl4, 1); + + if (ret == 0) + { + fifo_ctrl4.dec_ds3_fifo = (uint8_t)val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_FIFO_CTRL4, + (uint8_t *)&fifo_ctrl4, 1); + } + + return ret; +} + +/** + * @brief Selects Batching Data Rate (writing frequency in FIFO) + * for third data set.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of dec_ds3_fifo in reg FIFO_CTRL4 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_fifo_dataset_3_batch_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_dec_ds3_fifo_t *val) +{ + lsm6ds3tr_c_fifo_ctrl4_t fifo_ctrl4; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FIFO_CTRL4, + (uint8_t *)&fifo_ctrl4, 1); + + switch (fifo_ctrl4.dec_ds3_fifo) + { + case LSM6DS3TR_C_FIFO_DS3_DISABLE: + *val = LSM6DS3TR_C_FIFO_DS3_DISABLE; + break; + + case LSM6DS3TR_C_FIFO_DS3_NO_DEC: + *val = LSM6DS3TR_C_FIFO_DS3_NO_DEC; + break; + + case LSM6DS3TR_C_FIFO_DS3_DEC_2: + *val = LSM6DS3TR_C_FIFO_DS3_DEC_2; + break; + + case LSM6DS3TR_C_FIFO_DS3_DEC_3: + *val = LSM6DS3TR_C_FIFO_DS3_DEC_3; + break; + + case LSM6DS3TR_C_FIFO_DS3_DEC_4: + *val = LSM6DS3TR_C_FIFO_DS3_DEC_4; + break; + + case LSM6DS3TR_C_FIFO_DS3_DEC_8: + *val = LSM6DS3TR_C_FIFO_DS3_DEC_8; + break; + + case LSM6DS3TR_C_FIFO_DS3_DEC_16: + *val = LSM6DS3TR_C_FIFO_DS3_DEC_16; + break; + + case LSM6DS3TR_C_FIFO_DS3_DEC_32: + *val = LSM6DS3TR_C_FIFO_DS3_DEC_32; + break; + + default: + *val = LSM6DS3TR_C_FIFO_DS3_DEC_ND; + break; + } + + return ret; +} + +/** + * @brief Selects Batching Data Rate (writing frequency in FIFO) + * for fourth data set.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of dec_ds4_fifo in reg FIFO_CTRL4 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_fifo_dataset_4_batch_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_dec_ds4_fifo_t val) +{ + lsm6ds3tr_c_fifo_ctrl4_t fifo_ctrl4; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FIFO_CTRL4, + (uint8_t *)&fifo_ctrl4, 1); + + if (ret == 0) + { + fifo_ctrl4.dec_ds4_fifo = (uint8_t)val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_FIFO_CTRL4, + (uint8_t *)&fifo_ctrl4, 1); + } + + return ret; +} + +/** + * @brief Selects Batching Data Rate (writing frequency in FIFO) for + * fourth data set.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of dec_ds4_fifo in reg FIFO_CTRL4 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_fifo_dataset_4_batch_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_dec_ds4_fifo_t *val) +{ + lsm6ds3tr_c_fifo_ctrl4_t fifo_ctrl4; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FIFO_CTRL4, + (uint8_t *)&fifo_ctrl4, 1); + + switch (fifo_ctrl4.dec_ds4_fifo) + { + case LSM6DS3TR_C_FIFO_DS4_DISABLE: + *val = LSM6DS3TR_C_FIFO_DS4_DISABLE; + break; + + case LSM6DS3TR_C_FIFO_DS4_NO_DEC: + *val = LSM6DS3TR_C_FIFO_DS4_NO_DEC; + break; + + case LSM6DS3TR_C_FIFO_DS4_DEC_2: + *val = LSM6DS3TR_C_FIFO_DS4_DEC_2; + break; + + case LSM6DS3TR_C_FIFO_DS4_DEC_3: + *val = LSM6DS3TR_C_FIFO_DS4_DEC_3; + break; + + case LSM6DS3TR_C_FIFO_DS4_DEC_4: + *val = LSM6DS3TR_C_FIFO_DS4_DEC_4; + break; + + case LSM6DS3TR_C_FIFO_DS4_DEC_8: + *val = LSM6DS3TR_C_FIFO_DS4_DEC_8; + break; + + case LSM6DS3TR_C_FIFO_DS4_DEC_16: + *val = LSM6DS3TR_C_FIFO_DS4_DEC_16; + break; + + case LSM6DS3TR_C_FIFO_DS4_DEC_32: + *val = LSM6DS3TR_C_FIFO_DS4_DEC_32; + break; + + default: + *val = LSM6DS3TR_C_FIFO_DS4_DEC_ND; + break; + } + + return ret; +} + +/** + * @brief 8-bit data storage in FIFO.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of only_high_data in reg FIFO_CTRL4 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_fifo_xl_gy_8bit_format_set(stmdev_ctx_t *ctx, + uint8_t val) +{ + lsm6ds3tr_c_fifo_ctrl4_t fifo_ctrl4; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FIFO_CTRL4, + (uint8_t *)&fifo_ctrl4, 1); + + if (ret == 0) + { + fifo_ctrl4.only_high_data = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_FIFO_CTRL4, + (uint8_t *)&fifo_ctrl4, 1); + } + + return ret; +} + +/** + * @brief 8-bit data storage in FIFO.[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of only_high_data in reg FIFO_CTRL4 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_fifo_xl_gy_8bit_format_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3tr_c_fifo_ctrl4_t fifo_ctrl4; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FIFO_CTRL4, + (uint8_t *)&fifo_ctrl4, 1); + *val = fifo_ctrl4.only_high_data; + + return ret; +} + +/** + * @brief Sensing chain FIFO stop values memorization at threshold + * level.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of stop_on_fth in reg FIFO_CTRL4 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_fifo_stop_on_wtm_set(stmdev_ctx_t *ctx, + uint8_t val) +{ + lsm6ds3tr_c_fifo_ctrl4_t fifo_ctrl4; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FIFO_CTRL4, + (uint8_t *)&fifo_ctrl4, 1); + + if (ret == 0) + { + fifo_ctrl4.stop_on_fth = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_FIFO_CTRL4, + (uint8_t *)&fifo_ctrl4, 1); + } + + return ret; +} + +/** + * @brief Sensing chain FIFO stop values memorization at threshold + * level.[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of stop_on_fth in reg FIFO_CTRL4 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_fifo_stop_on_wtm_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3tr_c_fifo_ctrl4_t fifo_ctrl4; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FIFO_CTRL4, + (uint8_t *)&fifo_ctrl4, 1); + *val = fifo_ctrl4.stop_on_fth; + + return ret; +} + +/** + * @brief FIFO mode selection.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of fifo_mode in reg FIFO_CTRL5 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_fifo_mode_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_fifo_mode_t val) +{ + lsm6ds3tr_c_fifo_ctrl5_t fifo_ctrl5; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FIFO_CTRL5, + (uint8_t *)&fifo_ctrl5, 1); + + if (ret == 0) + { + fifo_ctrl5.fifo_mode = (uint8_t)val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_FIFO_CTRL5, + (uint8_t *)&fifo_ctrl5, 1); + } + + return ret; +} + +/** + * @brief FIFO mode selection.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of fifo_mode in reg FIFO_CTRL5 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_fifo_mode_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_fifo_mode_t *val) +{ + lsm6ds3tr_c_fifo_ctrl5_t fifo_ctrl5; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FIFO_CTRL5, + (uint8_t *)&fifo_ctrl5, 1); + + switch (fifo_ctrl5.fifo_mode) + { + case LSM6DS3TR_C_BYPASS_MODE: + *val = LSM6DS3TR_C_BYPASS_MODE; + break; + + case LSM6DS3TR_C_FIFO_MODE: + *val = LSM6DS3TR_C_FIFO_MODE; + break; + + case LSM6DS3TR_C_STREAM_TO_FIFO_MODE: + *val = LSM6DS3TR_C_STREAM_TO_FIFO_MODE; + break; + + case LSM6DS3TR_C_BYPASS_TO_STREAM_MODE: + *val = LSM6DS3TR_C_BYPASS_TO_STREAM_MODE; + break; + + case LSM6DS3TR_C_STREAM_MODE: + *val = LSM6DS3TR_C_STREAM_MODE; + break; + + default: + *val = LSM6DS3TR_C_FIFO_MODE_ND; + break; + } + + return ret; +} + +/** + * @brief FIFO ODR selection, setting FIFO_MODE also.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of odr_fifo in reg FIFO_CTRL5 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_fifo_data_rate_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_odr_fifo_t val) +{ + lsm6ds3tr_c_fifo_ctrl5_t fifo_ctrl5; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FIFO_CTRL5, + (uint8_t *)&fifo_ctrl5, 1); + + if (ret == 0) + { + fifo_ctrl5.odr_fifo = (uint8_t)val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_FIFO_CTRL5, + (uint8_t *)&fifo_ctrl5, 1); + } + + return ret; +} + +/** + * @brief FIFO ODR selection, setting FIFO_MODE also.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of odr_fifo in reg FIFO_CTRL5 + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_fifo_data_rate_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_odr_fifo_t *val) +{ + lsm6ds3tr_c_fifo_ctrl5_t fifo_ctrl5; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_FIFO_CTRL5, + (uint8_t *)&fifo_ctrl5, 1); + + switch (fifo_ctrl5.odr_fifo) + { + case LSM6DS3TR_C_FIFO_DISABLE: + *val = LSM6DS3TR_C_FIFO_DISABLE; + break; + + case LSM6DS3TR_C_FIFO_12Hz5: + *val = LSM6DS3TR_C_FIFO_12Hz5; + break; + + case LSM6DS3TR_C_FIFO_26Hz: + *val = LSM6DS3TR_C_FIFO_26Hz; + break; + + case LSM6DS3TR_C_FIFO_52Hz: + *val = LSM6DS3TR_C_FIFO_52Hz; + break; + + case LSM6DS3TR_C_FIFO_104Hz: + *val = LSM6DS3TR_C_FIFO_104Hz; + break; + + case LSM6DS3TR_C_FIFO_208Hz: + *val = LSM6DS3TR_C_FIFO_208Hz; + break; + + case LSM6DS3TR_C_FIFO_416Hz: + *val = LSM6DS3TR_C_FIFO_416Hz; + break; + + case LSM6DS3TR_C_FIFO_833Hz: + *val = LSM6DS3TR_C_FIFO_833Hz; + break; + + case LSM6DS3TR_C_FIFO_1k66Hz: + *val = LSM6DS3TR_C_FIFO_1k66Hz; + break; + + case LSM6DS3TR_C_FIFO_3k33Hz: + *val = LSM6DS3TR_C_FIFO_3k33Hz; + break; + + case LSM6DS3TR_C_FIFO_6k66Hz: + *val = LSM6DS3TR_C_FIFO_6k66Hz; + break; + + default: + *val = LSM6DS3TR_C_FIFO_RATE_ND; + break; + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3TR_C_DEN_functionality + * @brief This section groups all the functions concerning DEN + * functionality. + * @{ + * + */ + +/** + * @brief DEN active level configuration.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of den_lh in reg CTRL5_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_den_polarity_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_den_lh_t val) +{ + lsm6ds3tr_c_ctrl5_c_t ctrl5_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL5_C, + (uint8_t *)&ctrl5_c, 1); + + if (ret == 0) + { + ctrl5_c.den_lh = (uint8_t)val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL5_C, + (uint8_t *)&ctrl5_c, 1); + } + + return ret; +} + +/** + * @brief DEN active level configuration.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of den_lh in reg CTRL5_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_den_polarity_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_den_lh_t *val) +{ + lsm6ds3tr_c_ctrl5_c_t ctrl5_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL5_C, + (uint8_t *)&ctrl5_c, 1); + + switch (ctrl5_c.den_lh) + { + case LSM6DS3TR_C_DEN_ACT_LOW: + *val = LSM6DS3TR_C_DEN_ACT_LOW; + break; + + case LSM6DS3TR_C_DEN_ACT_HIGH: + *val = LSM6DS3TR_C_DEN_ACT_HIGH; + break; + + default: + *val = LSM6DS3TR_C_DEN_POL_ND; + break; + } + + return ret; +} + +/** + * @brief DEN functionality marking mode[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of den_mode in reg CTRL6_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_den_mode_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_den_mode_t val) +{ + lsm6ds3tr_c_ctrl6_c_t ctrl6_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL6_C, + (uint8_t *)&ctrl6_c, 1); + + if (ret == 0) + { + ctrl6_c.den_mode = (uint8_t)val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL6_C, + (uint8_t *)&ctrl6_c, 1); + } + + return ret; +} + +/** + * @brief DEN functionality marking mode[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of den_mode in reg CTRL6_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_den_mode_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_den_mode_t *val) +{ + lsm6ds3tr_c_ctrl6_c_t ctrl6_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL6_C, + (uint8_t *)&ctrl6_c, 1); + + switch (ctrl6_c.den_mode) + { + case LSM6DS3TR_C_DEN_DISABLE: + *val = LSM6DS3TR_C_DEN_DISABLE; + break; + + case LSM6DS3TR_C_LEVEL_LETCHED: + *val = LSM6DS3TR_C_LEVEL_LETCHED; + break; + + case LSM6DS3TR_C_LEVEL_TRIGGER: + *val = LSM6DS3TR_C_LEVEL_TRIGGER; + break; + + case LSM6DS3TR_C_EDGE_TRIGGER: + *val = LSM6DS3TR_C_EDGE_TRIGGER; + break; + + default: + *val = LSM6DS3TR_C_DEN_MODE_ND; + break; + } + + return ret; +} + +/** + * @brief Extend DEN functionality to accelerometer sensor.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of den_xl_g in reg CTRL9_XL + * and den_xl_en in CTRL4_C. + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_den_enable_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_den_xl_en_t val) +{ + lsm6ds3tr_c_ctrl4_c_t ctrl4_c; + lsm6ds3tr_c_ctrl9_xl_t ctrl9_xl; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL9_XL, + (uint8_t *)&ctrl9_xl, 1); + + if (ret == 0) + { + ctrl9_xl.den_xl_g = (uint8_t)val & 0x01U; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL9_XL, + (uint8_t *)&ctrl9_xl, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL4_C, + (uint8_t *)&ctrl4_c, 1); + + if (ret == 0) + { + ctrl4_c.den_xl_en = (uint8_t)val & 0x02U; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL4_C, + (uint8_t *)&ctrl4_c, 1); + } + } + } + + return ret; +} + +/** + * @brief Extend DEN functionality to accelerometer sensor. [get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of den_xl_g in reg CTRL9_XL + * and den_xl_en in CTRL4_C. + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_den_enable_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_den_xl_en_t *val) +{ + lsm6ds3tr_c_ctrl4_c_t ctrl4_c; + lsm6ds3tr_c_ctrl9_xl_t ctrl9_xl; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL4_C, + (uint8_t *)&ctrl4_c, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL9_XL, + (uint8_t *)&ctrl9_xl, 1); + + switch ((ctrl4_c.den_xl_en << 1) + ctrl9_xl.den_xl_g) + { + case LSM6DS3TR_C_STAMP_IN_GY_DATA: + *val = LSM6DS3TR_C_STAMP_IN_GY_DATA; + break; + + case LSM6DS3TR_C_STAMP_IN_XL_DATA: + *val = LSM6DS3TR_C_STAMP_IN_XL_DATA; + break; + + case LSM6DS3TR_C_STAMP_IN_GY_XL_DATA: + *val = LSM6DS3TR_C_STAMP_IN_GY_XL_DATA; + break; + + default: + *val = LSM6DS3TR_C_DEN_STAMP_ND; + break; + } + } + + return ret; +} + +/** + * @brief DEN value stored in LSB of Z-axis.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of den_z in reg CTRL9_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_den_mark_axis_z_set(stmdev_ctx_t *ctx, + uint8_t val) +{ + lsm6ds3tr_c_ctrl9_xl_t ctrl9_xl; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL9_XL, + (uint8_t *)&ctrl9_xl, 1); + + if (ret == 0) + { + ctrl9_xl.den_z = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL9_XL, + (uint8_t *)&ctrl9_xl, 1); + } + + return ret; +} + +/** + * @brief DEN value stored in LSB of Z-axis.[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of den_z in reg CTRL9_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_den_mark_axis_z_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3tr_c_ctrl9_xl_t ctrl9_xl; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL9_XL, + (uint8_t *)&ctrl9_xl, 1); + *val = ctrl9_xl.den_z; + + return ret; +} + +/** + * @brief DEN value stored in LSB of Y-axis.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of den_y in reg CTRL9_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_den_mark_axis_y_set(stmdev_ctx_t *ctx, + uint8_t val) +{ + lsm6ds3tr_c_ctrl9_xl_t ctrl9_xl; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL9_XL, + (uint8_t *)&ctrl9_xl, 1); + + if (ret == 0) + { + ctrl9_xl.den_y = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL9_XL, + (uint8_t *)&ctrl9_xl, 1); + } + + return ret; +} + +/** + * @brief DEN value stored in LSB of Y-axis.[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of den_y in reg CTRL9_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_den_mark_axis_y_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3tr_c_ctrl9_xl_t ctrl9_xl; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL9_XL, + (uint8_t *)&ctrl9_xl, 1); + *val = ctrl9_xl.den_y; + + return ret; +} + +/** + * @brief DEN value stored in LSB of X-axis.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of den_x in reg CTRL9_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_den_mark_axis_x_set(stmdev_ctx_t *ctx, + uint8_t val) +{ + lsm6ds3tr_c_ctrl9_xl_t ctrl9_xl; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL9_XL, + (uint8_t *)&ctrl9_xl, 1); + + if (ret == 0) + { + ctrl9_xl.den_x = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL9_XL, + (uint8_t *)&ctrl9_xl, 1); + } + + return ret; +} + +/** + * @brief DEN value stored in LSB of X-axis.[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of den_x in reg CTRL9_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_den_mark_axis_x_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3tr_c_ctrl9_xl_t ctrl9_xl; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL9_XL, + (uint8_t *)&ctrl9_xl, 1); + *val = ctrl9_xl.den_x; + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3TR_C_Pedometer + * @brief This section groups all the functions that manage pedometer. + * @{ + * + */ + +/** + * @brief Reset pedometer step counter.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of pedo_rst_step in reg CTRL10_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_pedo_step_reset_set(stmdev_ctx_t *ctx, + uint8_t val) +{ + lsm6ds3tr_c_ctrl10_c_t ctrl10_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL10_C, + (uint8_t *)&ctrl10_c, 1); + + if (ret == 0) + { + ctrl10_c.pedo_rst_step = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL10_C, + (uint8_t *)&ctrl10_c, 1); + } + + return ret; +} + +/** + * @brief Reset pedometer step counter.[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of pedo_rst_step in reg CTRL10_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_pedo_step_reset_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3tr_c_ctrl10_c_t ctrl10_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL10_C, + (uint8_t *)&ctrl10_c, 1); + *val = ctrl10_c.pedo_rst_step; + + return ret; +} + +/** + * @brief Enable pedometer algorithm.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of pedo_en in reg CTRL10_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_pedo_sens_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3tr_c_ctrl10_c_t ctrl10_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL10_C, + (uint8_t *)&ctrl10_c, 1); + + if (ret == 0) + { + ctrl10_c.pedo_en = val; + + if (val != 0x00U) + { + ctrl10_c.func_en = val; + } + + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL10_C, + (uint8_t *)&ctrl10_c, 1); + } + + return ret; +} + +/** + * @brief pedo_sens: Enable pedometer algorithm.[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of pedo_en in reg CTRL10_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_pedo_sens_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3tr_c_ctrl10_c_t ctrl10_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL10_C, + (uint8_t *)&ctrl10_c, 1); + *val = ctrl10_c.pedo_en; + + return ret; +} + +/** + * @brief Minimum threshold to detect a peak. Default is 10h.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of ths_min in reg + * CONFIG_PEDO_THS_MIN + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_pedo_threshold_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3tr_c_config_pedo_ths_min_t config_pedo_ths_min; + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_A); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CONFIG_PEDO_THS_MIN, + (uint8_t *)&config_pedo_ths_min, 1); + + if (ret == 0) + { + config_pedo_ths_min.ths_min = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CONFIG_PEDO_THS_MIN, + (uint8_t *)&config_pedo_ths_min, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + } + + return ret; +} + +/** + * @brief Minimum threshold to detect a peak. Default is 10h.[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of ths_min in reg CONFIG_PEDO_THS_MIN + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_pedo_threshold_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3tr_c_config_pedo_ths_min_t config_pedo_ths_min; + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_A); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CONFIG_PEDO_THS_MIN, + (uint8_t *)&config_pedo_ths_min, 1); + + if (ret == 0) + { + *val = config_pedo_ths_min.ths_min; + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + + return ret; +} + +/** + * @brief pedo_full_scale: Pedometer data range.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of pedo_fs in + * reg CONFIG_PEDO_THS_MIN + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_pedo_full_scale_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_pedo_fs_t val) +{ + lsm6ds3tr_c_config_pedo_ths_min_t config_pedo_ths_min; + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_A); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CONFIG_PEDO_THS_MIN, + (uint8_t *)&config_pedo_ths_min, 1); + + if (ret == 0) + { + config_pedo_ths_min.pedo_fs = (uint8_t) val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CONFIG_PEDO_THS_MIN, + (uint8_t *)&config_pedo_ths_min, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + } + + return ret; +} + +/** + * @brief Pedometer data range.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of pedo_fs in + * reg CONFIG_PEDO_THS_MIN + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_pedo_full_scale_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_pedo_fs_t *val) +{ + lsm6ds3tr_c_config_pedo_ths_min_t config_pedo_ths_min; + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_A); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CONFIG_PEDO_THS_MIN, + (uint8_t *)&config_pedo_ths_min, 1); + + if (ret == 0) + { + switch (config_pedo_ths_min.pedo_fs) + { + case LSM6DS3TR_C_PEDO_AT_2g: + *val = LSM6DS3TR_C_PEDO_AT_2g; + break; + + case LSM6DS3TR_C_PEDO_AT_4g: + *val = LSM6DS3TR_C_PEDO_AT_4g; + break; + + default: + *val = LSM6DS3TR_C_PEDO_FS_ND; + break; + } + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + + return ret; +} + +/** + * @brief Pedometer debounce configuration register (r/w).[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of deb_step in reg PEDO_DEB_REG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_pedo_debounce_steps_set(stmdev_ctx_t *ctx, + uint8_t val) +{ + lsm6ds3tr_c_pedo_deb_reg_t pedo_deb_reg; + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_A); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_PEDO_DEB_REG, + (uint8_t *)&pedo_deb_reg, 1); + + if (ret == 0) + { + pedo_deb_reg.deb_step = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_PEDO_DEB_REG, + (uint8_t *)&pedo_deb_reg, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + } + + return ret; +} + +/** + * @brief Pedometer debounce configuration register (r/w).[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of deb_step in reg PEDO_DEB_REG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_pedo_debounce_steps_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3tr_c_pedo_deb_reg_t pedo_deb_reg; + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_A); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_PEDO_DEB_REG, + (uint8_t *)&pedo_deb_reg, 1); + + if (ret == 0) + { + *val = pedo_deb_reg.deb_step; + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + + return ret; +} + +/** + * @brief Debounce time. If the time between two consecutive steps is + * greater than DEB_TIME*80ms, the debouncer is reactivated. + * Default value: 01101[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of deb_time in reg PEDO_DEB_REG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_pedo_timeout_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3tr_c_pedo_deb_reg_t pedo_deb_reg; + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_A); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_PEDO_DEB_REG, + (uint8_t *)&pedo_deb_reg, 1); + + if (ret == 0) + { + pedo_deb_reg.deb_time = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_PEDO_DEB_REG, + (uint8_t *)&pedo_deb_reg, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + } + + return ret; +} + +/** + * @brief Debounce time. If the time between two consecutive steps is + * greater than DEB_TIME*80ms, the debouncer is reactivated. + * Default value: 01101[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of deb_time in reg PEDO_DEB_REG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_pedo_timeout_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3tr_c_pedo_deb_reg_t pedo_deb_reg; + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_A); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_PEDO_DEB_REG, + (uint8_t *)&pedo_deb_reg, 1); + + if (ret == 0) + { + *val = pedo_deb_reg.deb_time; + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + + return ret; +} + +/** + * @brief Time period register for step detection on delta time (r/w).[set] + * + * @param ctx Read / write interface definitions + * @param buff Buffer that contains data to write + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_pedo_steps_period_set(stmdev_ctx_t *ctx, + uint8_t *buff) +{ + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_A); + + if (ret == 0) + { + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_STEP_COUNT_DELTA, buff, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + + return ret; +} + +/** + * @brief Time period register for step detection on delta time (r/w).[get] + * + * @param ctx Read / write interface definitions + * @param buff Buffer that stores data read + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_pedo_steps_period_get(stmdev_ctx_t *ctx, + uint8_t *buff) +{ + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_A); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_STEP_COUNT_DELTA, buff, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3TR_C_significant_motion + * @brief This section groups all the functions that manage the + * significant motion detection. + * @{ + * + */ + +/** + * @brief Enable significant motion detection function.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of sign_motion_en in reg CTRL10_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_motion_sens_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3tr_c_ctrl10_c_t ctrl10_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL10_C, + (uint8_t *)&ctrl10_c, 1); + + if (ret == 0) + { + ctrl10_c.sign_motion_en = val; + + if (val != 0x00U) + { + ctrl10_c.func_en = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL10_C, + (uint8_t *)&ctrl10_c, 1); + } + } + + return ret; +} + +/** + * @brief Enable significant motion detection function.[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of sign_motion_en in reg CTRL10_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_motion_sens_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3tr_c_ctrl10_c_t ctrl10_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL10_C, + (uint8_t *)&ctrl10_c, 1); + *val = ctrl10_c.sign_motion_en; + + return ret; +} + +/** + * @brief Significant motion threshold.[set] + * + * @param ctx Read / write interface definitions + * @param buff Buffer that store significant motion threshold. + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_motion_threshold_set(stmdev_ctx_t *ctx, + uint8_t *buff) +{ + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_A); + + if (ret == 0) + { + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_SM_THS, buff, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + + return ret; +} + +/** + * @brief Significant motion threshold.[get] + * + * @param ctx Read / write interface definitions + * @param buff Buffer that store significant motion threshold. + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_motion_threshold_get(stmdev_ctx_t *ctx, + uint8_t *buff) +{ + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_A); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_SM_THS, buff, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3TR_C_tilt_detection + * @brief This section groups all the functions that manage the tilt + * event detection. + * @{ + * + */ + +/** + * @brief Enable tilt calculation.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of tilt_en in reg CTRL10_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_tilt_sens_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3tr_c_ctrl10_c_t ctrl10_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL10_C, + (uint8_t *)&ctrl10_c, 1); + + if (ret == 0) + { + ctrl10_c.tilt_en = val; + + if (val != 0x00U) + { + ctrl10_c.func_en = val; + } + + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL10_C, + (uint8_t *)&ctrl10_c, 1); + } + + return ret; +} + +/** + * @brief Enable tilt calculation.[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of tilt_en in reg CTRL10_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_tilt_sens_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3tr_c_ctrl10_c_t ctrl10_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL10_C, + (uint8_t *)&ctrl10_c, 1); + *val = ctrl10_c.tilt_en; + + return ret; +} + +/** + * @brief Enable tilt calculation.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of tilt_en in reg CTRL10_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_wrist_tilt_sens_set(stmdev_ctx_t *ctx, + uint8_t val) +{ + lsm6ds3tr_c_ctrl10_c_t ctrl10_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL10_C, + (uint8_t *)&ctrl10_c, 1); + + if (ret == 0) + { + ctrl10_c.wrist_tilt_en = val; + + if (val != 0x00U) + { + ctrl10_c.func_en = val; + } + + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL10_C, + (uint8_t *)&ctrl10_c, 1); + } + + return ret; +} + +/** + * @brief Enable tilt calculation.[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of tilt_en in reg CTRL10_C + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_wrist_tilt_sens_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3tr_c_ctrl10_c_t ctrl10_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL10_C, + (uint8_t *)&ctrl10_c, 1); + *val = ctrl10_c.wrist_tilt_en; + + return ret; +} + +/** + * @brief Absolute Wrist Tilt latency register (r/w). + * Absolute wrist tilt latency parameters. + * 1 LSB = 40 ms. Default value: 0Fh (600 ms).[set] + * + * @param ctx Read / write interface definitions + * @param buff Buffer that contains data to write + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_tilt_latency_set(stmdev_ctx_t *ctx, uint8_t *buff) +{ + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_B); + + if (ret == 0) + { + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_A_WRIST_TILT_LAT, buff, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + + return ret; +} + +/** + * @brief Absolute Wrist Tilt latency register (r/w). + * Absolute wrist tilt latency parameters. + * 1 LSB = 40 ms. Default value: 0Fh (600 ms).[get] + * + * @param ctx Read / write interface definitions + * @param buff Buffer that stores data read + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_tilt_latency_get(stmdev_ctx_t *ctx, uint8_t *buff) +{ + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_B); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_A_WRIST_TILT_LAT, buff, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + + return ret; +} + +/** + * @brief Absolute Wrist Tilt threshold register(r/w). + * Absolute wrist tilt threshold parameters. + * 1 LSB = 15.625 mg.Default value: 20h (500 mg).[set] + * + * @param ctx Read / write interface definitions + * @param buff Buffer that contains data to write + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_tilt_threshold_set(stmdev_ctx_t *ctx, + uint8_t *buff) +{ + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_B); + + if (ret == 0) + { + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_A_WRIST_TILT_THS, buff, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + + return ret; +} + +/** + * @brief Absolute Wrist Tilt threshold register(r/w). + * Absolute wrist tilt threshold parameters. + * 1 LSB = 15.625 mg.Default value: 20h (500 mg).[get] + * + * @param ctx Read / write interface definitions + * @param buff Buffer that stores data read + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_tilt_threshold_get(stmdev_ctx_t *ctx, + uint8_t *buff) +{ + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_B); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_A_WRIST_TILT_THS, buff, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + + return ret; +} + +/** + * @brief Absolute Wrist Tilt mask register (r/w).[set] + * + * @param ctx Read / write interface definitions + * @param val Registers A_WRIST_TILT_MASK + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_tilt_src_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_a_wrist_tilt_mask_t *val) +{ + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_B); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_A_WRIST_TILT_MASK, + (uint8_t *) val, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + + return ret; +} + +/** + * @brief Absolute Wrist Tilt mask register (r/w).[get] + * + * @param ctx Read / write interface definitions + * @param val Registers A_WRIST_TILT_MASK + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_tilt_src_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_a_wrist_tilt_mask_t *val) +{ + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_B); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_A_WRIST_TILT_MASK, + (uint8_t *) val, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3TR_C_ magnetometer_sensor + * @brief This section groups all the functions that manage additional + * magnetometer sensor. + * @{ + * + */ + +/** + * @brief Enable soft-iron correction algorithm for magnetometer.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of soft_en in reg CTRL9_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_mag_soft_iron_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3tr_c_ctrl9_xl_t ctrl9_xl; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL9_XL, + (uint8_t *)&ctrl9_xl, 1); + + if (ret == 0) + { + ctrl9_xl.soft_en = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL9_XL, + (uint8_t *)&ctrl9_xl, 1); + } + + return ret; +} + +/** + * @brief Enable soft-iron correction algorithm for magnetometer.[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of soft_en in reg CTRL9_XL + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_mag_soft_iron_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3tr_c_ctrl9_xl_t ctrl9_xl; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL9_XL, + (uint8_t *)&ctrl9_xl, 1); + *val = ctrl9_xl.soft_en; + + return ret; +} + +/** + * @brief Enable hard-iron correction algorithm for magnetometer.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of iron_en in reg MASTER_CONFIG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_mag_hard_iron_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3tr_c_master_config_t master_config; + lsm6ds3tr_c_ctrl10_c_t ctrl10_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + + if (ret == 0) + { + master_config.iron_en = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL10_C, + (uint8_t *)&ctrl10_c, 1); + + if (ret == 0) + { + if (val != 0x00U) + { + ctrl10_c.func_en = val; + } + + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL10_C, + (uint8_t *)&ctrl10_c, 1); + } + } + } + + return ret; +} + +/** + * @brief Enable hard-iron correction algorithm for magnetometer.[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of iron_en in reg MASTER_CONFIG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_mag_hard_iron_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3tr_c_master_config_t master_config; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + *val = master_config.iron_en; + + return ret; +} + +/** + * @brief Soft iron 3x3 matrix. Value are expressed in sign-module format. + * (Es. SVVVVVVVb where S is the sign 0/+1/- and V is the value).[set] + * + * @param ctx Read / write interface definitions + * @param buff Buffer that contains data to write + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_mag_soft_iron_mat_set(stmdev_ctx_t *ctx, + uint8_t *buff) +{ + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_A); + + if (ret == 0) + { + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_MAG_SI_XX, buff, 9); + + if (ret == 0) + { + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + + return ret; +} + +/** + * @brief Soft iron 3x3 matrix. Value are expressed in sign-module format. + * (Es. SVVVVVVVb where S is the sign 0/+1/- and V is the value).[get] + * + * @param ctx Read / write interface definitions + * @param buff Buffer that stores data read + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_mag_soft_iron_mat_get(stmdev_ctx_t *ctx, + uint8_t *buff) +{ + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_A); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_MAG_SI_XX, buff, 9); + + if (ret == 0) + { + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + + return ret; +} + +/** + * @brief Offset for hard-iron compensation register (r/w). The value is + * expressed as a 16-bit word in two’s complement.[set] + * + * @param ctx Read / write interface definitions + * @param buff Buffer that contains data to write + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_mag_offset_set(stmdev_ctx_t *ctx, int16_t *val) +{ + uint8_t buff[6]; + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_A); + + if (ret == 0) + { + buff[1] = (uint8_t)((uint16_t)val[0] / 256U); + buff[0] = (uint8_t)((uint16_t)val[0] - (buff[1] * 256U)); + buff[3] = (uint8_t)((uint16_t)val[1] / 256U); + buff[2] = (uint8_t)((uint16_t)val[1] - (buff[3] * 256U)); + buff[5] = (uint8_t)((uint16_t)val[2] / 256U); + buff[4] = (uint8_t)((uint16_t)val[2] - (buff[5] * 256U)); + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_MAG_OFFX_L, buff, 6); + + if (ret == 0) + { + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + + return ret; +} + +/** + * @brief Offset for hard-iron compensation register(r/w). + * The value is expressed as a 16-bit word in two’s complement.[get] + * + * @param ctx Read / write interface definitions + * @param buff Buffer that stores data read + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_mag_offset_get(stmdev_ctx_t *ctx, int16_t *val) +{ + uint8_t buff[6]; + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_A); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_MAG_OFFX_L, buff, 6); + + if (ret == 0) + { + 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]; + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DS3TR_C_Sensor_hub + * @brief This section groups all the functions that manage the sensor + * hub functionality. + * @{ + * + */ + +/** + * @brief Enable function.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values func_en + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_func_en_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3tr_c_ctrl10_c_t ctrl10_c; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_CTRL10_C, + (uint8_t *)&ctrl10_c, 1); + + if (ret == 0) + { + ctrl10_c.func_en = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_CTRL10_C, + (uint8_t *)&ctrl10_c, 1); + } + + return ret; +} + +/** + * @brief Sensor synchronization time frame with the step of 500 ms and + * full range of 5s. Unsigned 8-bit.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of tph in reg SENSOR_SYNC_TIME_FRAME + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_sh_sync_sens_frame_set(stmdev_ctx_t *ctx, + uint8_t val) +{ + lsm6ds3tr_c_sensor_sync_time_frame_t sensor_sync_time_frame; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_SENSOR_SYNC_TIME_FRAME, + (uint8_t *)&sensor_sync_time_frame, 1); + + if (ret == 0) + { + sensor_sync_time_frame.tph = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_SENSOR_SYNC_TIME_FRAME, + (uint8_t *)&sensor_sync_time_frame, 1); + } + + return ret; +} + +/** + * @brief Sensor synchronization time frame with the step of 500 ms and + * full range of 5s. Unsigned 8-bit.[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of tph in reg SENSOR_SYNC_TIME_FRAME + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_sh_sync_sens_frame_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3tr_c_sensor_sync_time_frame_t sensor_sync_time_frame; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_SENSOR_SYNC_TIME_FRAME, + (uint8_t *)&sensor_sync_time_frame, 1); + *val = sensor_sync_time_frame.tph; + + return ret; +} + +/** + * @brief Resolution ratio of error code for sensor synchronization.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of rr in reg SENSOR_SYNC_RES_RATIO + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_sh_sync_sens_ratio_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_rr_t val) +{ + lsm6ds3tr_c_sensor_sync_res_ratio_t sensor_sync_res_ratio; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_SENSOR_SYNC_RES_RATIO, + (uint8_t *)&sensor_sync_res_ratio, 1); + + if (ret == 0) + { + sensor_sync_res_ratio.rr = (uint8_t) val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_SENSOR_SYNC_RES_RATIO, + (uint8_t *)&sensor_sync_res_ratio, 1); + } + + return ret; +} + +/** + * @brief Resolution ratio of error code for sensor synchronization.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of rr in reg SENSOR_SYNC_RES_RATIO + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_sh_sync_sens_ratio_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_rr_t *val) +{ + lsm6ds3tr_c_sensor_sync_res_ratio_t sensor_sync_res_ratio; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_SENSOR_SYNC_RES_RATIO, + (uint8_t *)&sensor_sync_res_ratio, 1); + + switch (sensor_sync_res_ratio.rr) + { + case LSM6DS3TR_C_RES_RATIO_2_11: + *val = LSM6DS3TR_C_RES_RATIO_2_11; + break; + + case LSM6DS3TR_C_RES_RATIO_2_12: + *val = LSM6DS3TR_C_RES_RATIO_2_12; + break; + + case LSM6DS3TR_C_RES_RATIO_2_13: + *val = LSM6DS3TR_C_RES_RATIO_2_13; + break; + + case LSM6DS3TR_C_RES_RATIO_2_14: + *val = LSM6DS3TR_C_RES_RATIO_2_14; + break; + + default: + *val = LSM6DS3TR_C_RES_RATIO_ND; + break; + } + + return ret; +} + +/** + * @brief Sensor hub I2C master enable.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of master_on in reg MASTER_CONFIG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_sh_master_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6ds3tr_c_master_config_t master_config; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + + if (ret == 0) + { + master_config.master_on = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + } + + return ret; +} + +/** + * @brief Sensor hub I2C master enable.[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of master_on in reg MASTER_CONFIG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_sh_master_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6ds3tr_c_master_config_t master_config; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + *val = master_config.master_on; + + return ret; +} + +/** + * @brief I2C interface pass-through.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of pass_through_mode in reg MASTER_CONFIG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_sh_pass_through_set(stmdev_ctx_t *ctx, + uint8_t val) +{ + lsm6ds3tr_c_master_config_t master_config; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + + if (ret == 0) + { + master_config.pass_through_mode = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + } + + return ret; +} + +/** + * @brief I2C interface pass-through.[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of pass_through_mode in reg MASTER_CONFIG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_sh_pass_through_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3tr_c_master_config_t master_config; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + *val = master_config.pass_through_mode; + + return ret; +} + +/** + * @brief Master I2C pull-up enable/disable.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of pull_up_en in reg MASTER_CONFIG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_sh_pin_mode_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_pull_up_en_t val) +{ + lsm6ds3tr_c_master_config_t master_config; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + + if (ret == 0) + { + master_config.pull_up_en = (uint8_t) val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + } + + return ret; +} + +/** + * @brief Master I2C pull-up enable/disable.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of pull_up_en in reg MASTER_CONFIG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_sh_pin_mode_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_pull_up_en_t *val) +{ + lsm6ds3tr_c_master_config_t master_config; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + + switch (master_config.pull_up_en) + { + case LSM6DS3TR_C_EXT_PULL_UP: + *val = LSM6DS3TR_C_EXT_PULL_UP; + break; + + case LSM6DS3TR_C_INTERNAL_PULL_UP: + *val = LSM6DS3TR_C_INTERNAL_PULL_UP; + break; + + default: + *val = LSM6DS3TR_C_SH_PIN_MODE; + break; + } + + return ret; +} + +/** + * @brief Sensor hub trigger signal selection.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of start_config in reg MASTER_CONFIG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_sh_syncro_mode_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_start_config_t val) +{ + lsm6ds3tr_c_master_config_t master_config; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + + if (ret == 0) + { + master_config.start_config = (uint8_t)val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + } + + return ret; +} + +/** + * @brief Sensor hub trigger signal selection.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of start_config in reg MASTER_CONFIG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_sh_syncro_mode_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_start_config_t *val) +{ + lsm6ds3tr_c_master_config_t master_config; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + + switch (master_config.start_config) + { + case LSM6DS3TR_C_XL_GY_DRDY: + *val = LSM6DS3TR_C_XL_GY_DRDY; + break; + + case LSM6DS3TR_C_EXT_ON_INT2_PIN: + *val = LSM6DS3TR_C_EXT_ON_INT2_PIN; + break; + + default: + *val = LSM6DS3TR_C_SH_SYNCRO_ND; + break; + } + + return ret; +} + +/** + * @brief Manage the Master DRDY signal on INT1 pad.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of drdy_on_int1 in reg MASTER_CONFIG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_sh_drdy_on_int1_set(stmdev_ctx_t *ctx, + uint8_t val) +{ + lsm6ds3tr_c_master_config_t master_config; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + + if (ret == 0) + { + master_config.drdy_on_int1 = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + } + + return ret; +} + +/** + * @brief Manage the Master DRDY signal on INT1 pad.[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of drdy_on_int1 in reg MASTER_CONFIG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_sh_drdy_on_int1_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3tr_c_master_config_t master_config; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_MASTER_CONFIG, + (uint8_t *)&master_config, 1); + *val = master_config.drdy_on_int1; + + return ret; +} + +/** + * @brief Sensor hub output registers.[get] + * + * @param ctx Read / write interface definitions + * @param val Structure of registers from SENSORHUB1_REG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_sh_read_data_raw_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_emb_sh_read_t *val) +{ + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_SENSORHUB1_REG, + (uint8_t *) & (val->sh_byte_1), 12); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_SENSORHUB13_REG, + (uint8_t *) & (val->sh_byte_13), 6); + } + + return ret; +} + +/** + * @brief Master command code used for stamping for sensor sync.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of master_cmd_code in + * reg MASTER_CMD_CODE + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_sh_cmd_sens_sync_set(stmdev_ctx_t *ctx, + uint8_t val) +{ + lsm6ds3tr_c_master_cmd_code_t master_cmd_code; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_MASTER_CMD_CODE, + (uint8_t *)&master_cmd_code, 1); + + if (ret == 0) + { + master_cmd_code.master_cmd_code = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_MASTER_CMD_CODE, + (uint8_t *)&master_cmd_code, 1); + } + + return ret; +} + +/** + * @brief Master command code used for stamping for sensor sync.[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of master_cmd_code in + * reg MASTER_CMD_CODE + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_sh_cmd_sens_sync_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3tr_c_master_cmd_code_t master_cmd_code; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_MASTER_CMD_CODE, + (uint8_t *)&master_cmd_code, 1); + *val = master_cmd_code.master_cmd_code; + + return ret; +} + +/** + * @brief Error code used for sensor synchronization.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of error_code in + * reg SENS_SYNC_SPI_ERROR_CODE. + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_sh_spi_sync_error_set(stmdev_ctx_t *ctx, + uint8_t val) +{ + lsm6ds3tr_c_sens_sync_spi_error_code_t sens_sync_spi_error_code; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_SENS_SYNC_SPI_ERROR_CODE, + (uint8_t *)&sens_sync_spi_error_code, 1); + + if (ret == 0) + { + sens_sync_spi_error_code.error_code = val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_SENS_SYNC_SPI_ERROR_CODE, + (uint8_t *)&sens_sync_spi_error_code, 1); + } + + return ret; +} + +/** + * @brief Error code used for sensor synchronization.[get] + * + * @param ctx Read / write interface definitions + * @param val Change the values of error_code in + * reg SENS_SYNC_SPI_ERROR_CODE. + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_sh_spi_sync_error_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6ds3tr_c_sens_sync_spi_error_code_t sens_sync_spi_error_code; + int32_t ret; + + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_SENS_SYNC_SPI_ERROR_CODE, + (uint8_t *)&sens_sync_spi_error_code, 1); + *val = sens_sync_spi_error_code.error_code; + + return ret; +} + +/** + * @brief Number of external sensors to be read by the sensor hub.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of aux_sens_on in reg SLAVE0_CONFIG. + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_sh_num_of_dev_connected_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_aux_sens_on_t val) +{ + lsm6ds3tr_c_slave0_config_t slave0_config; + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_A); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_SLAVE0_CONFIG, + (uint8_t *)&slave0_config, 1); + + if (ret == 0) + { + slave0_config.aux_sens_on = (uint8_t) val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_SLAVE0_CONFIG, + (uint8_t *)&slave0_config, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + } + + return ret; +} + +/** + * @brief Number of external sensors to be read by the sensor hub.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of aux_sens_on in reg SLAVE0_CONFIG. + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_sh_num_of_dev_connected_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_aux_sens_on_t *val) +{ + lsm6ds3tr_c_slave0_config_t slave0_config; + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_A); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_SLAVE0_CONFIG, + (uint8_t *)&slave0_config, 1); + + if (ret == 0) + { + switch (slave0_config.aux_sens_on) + { + case LSM6DS3TR_C_SLV_0: + *val = LSM6DS3TR_C_SLV_0; + break; + + case LSM6DS3TR_C_SLV_0_1: + *val = LSM6DS3TR_C_SLV_0_1; + break; + + case LSM6DS3TR_C_SLV_0_1_2: + *val = LSM6DS3TR_C_SLV_0_1_2; + break; + + case LSM6DS3TR_C_SLV_0_1_2_3: + *val = LSM6DS3TR_C_SLV_0_1_2_3; + break; + + default: + *val = LSM6DS3TR_C_SLV_EN_ND; + break; + } + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + + return ret; +} + +/** + * @brief Configure slave 0 for perform a write.[set] + * + * @param ctx Read / write interface definitions + * @param val Structure that contain: + * - uint8_t slv_add; 8 bit i2c device address + * - uint8_t slv_subadd; 8 bit register device address + * - uint8_t slv_data; 8 bit data to write + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_sh_cfg_write(stmdev_ctx_t *ctx, + lsm6ds3tr_c_sh_cfg_write_t *val) +{ + lsm6ds3tr_c_slv0_add_t slv0_add; + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_A); + + if (ret == 0) + { + slv0_add.slave0_add = val->slv0_add; + slv0_add.rw_0 = 0; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_SLV0_ADD, + (uint8_t *)&slv0_add, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_SLV0_SUBADD, + &(val->slv0_subadd), 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_write_reg(ctx, + LSM6DS3TR_C_DATAWRITE_SRC_MODE_SUB_SLV0, + &(val->slv0_data), 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + } + } + + return ret; +} + +/** + * @brief Configure slave 0 for perform a read.[get] + * + * @param ctx Read / write interface definitions + * @param val Structure that contain: + * - 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 lsm6ds3tr_c_sh_slv0_cfg_read(stmdev_ctx_t *ctx, + lsm6ds3tr_c_sh_cfg_read_t *val) +{ + lsm6ds3tr_c_slave0_config_t slave0_config; + lsm6ds3tr_c_slv0_add_t slv0_add; + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_A); + + if (ret == 0) + { + slv0_add.slave0_add = val->slv_add; + slv0_add.rw_0 = 1; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_SLV0_ADD, + (uint8_t *)&slv0_add, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_SLV0_SUBADD, + &(val->slv_subadd), 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_SLAVE0_CONFIG, + (uint8_t *)&slave0_config, 1); + slave0_config.slave0_numop = val->slv_len; + + if (ret == 0) + { + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_SLAVE0_CONFIG, + (uint8_t *)&slave0_config, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + } + } + } + + return ret; +} + +/** + * @brief Configure slave 1 for perform a read.[get] + * + * @param ctx Read / write interface definitions + * @param val Structure that contain: + * - 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 lsm6ds3tr_c_sh_slv1_cfg_read(stmdev_ctx_t *ctx, + lsm6ds3tr_c_sh_cfg_read_t *val) +{ + lsm6ds3tr_c_slave1_config_t slave1_config; + lsm6ds3tr_c_slv1_add_t slv1_add; + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_A); + + if (ret == 0) + { + slv1_add.slave1_add = val->slv_add; + slv1_add.r_1 = 1; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_SLV1_ADD, + (uint8_t *)&slv1_add, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_SLV1_SUBADD, + &(val->slv_subadd), 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_SLAVE1_CONFIG, + (uint8_t *)&slave1_config, 1); + slave1_config.slave1_numop = val->slv_len; + + if (ret == 0) + { + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_SLAVE1_CONFIG, + (uint8_t *)&slave1_config, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + } + } + } + + return ret; +} + +/** + * @brief Configure slave 2 for perform a read.[get] + * + * @param ctx Read / write interface definitions + * @param val Structure that contain: + * - 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 lsm6ds3tr_c_sh_slv2_cfg_read(stmdev_ctx_t *ctx, + lsm6ds3tr_c_sh_cfg_read_t *val) +{ + lsm6ds3tr_c_slv2_add_t slv2_add; + lsm6ds3tr_c_slave2_config_t slave2_config; + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_A); + + if (ret == 0) + { + slv2_add.slave2_add = val->slv_add; + slv2_add.r_2 = 1; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_SLV2_ADD, + (uint8_t *)&slv2_add, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_SLV2_SUBADD, + &(val->slv_subadd), 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_SLAVE2_CONFIG, + (uint8_t *)&slave2_config, 1); + + if (ret == 0) + { + slave2_config.slave2_numop = val->slv_len; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_SLAVE2_CONFIG, + (uint8_t *)&slave2_config, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + } + } + } + + return ret; +} + +/** + * @brief Configure slave 3 for perform a read.[get] + * + * @param ctx Read / write interface definitions + * @param val Structure that contain: + * - 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 lsm6ds3tr_c_sh_slv3_cfg_read(stmdev_ctx_t *ctx, + lsm6ds3tr_c_sh_cfg_read_t *val) +{ + lsm6ds3tr_c_slave3_config_t slave3_config; + lsm6ds3tr_c_slv3_add_t slv3_add; + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_A); + + if (ret == 0) + { + slv3_add.slave3_add = val->slv_add; + slv3_add.r_3 = 1; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_SLV3_ADD, + (uint8_t *)&slv3_add, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_SLV3_SUBADD, + (uint8_t *) & (val->slv_subadd), 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_SLAVE3_CONFIG, + (uint8_t *)&slave3_config, 1); + + if (ret == 0) + { + slave3_config.slave3_numop = val->slv_len; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_SLAVE3_CONFIG, + (uint8_t *)&slave3_config, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + } + } + } + + return ret; +} + +/** + * @brief Decimation of read operation on Slave 0 starting from the + * sensor hub trigger.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of slave0_rate in reg SLAVE0_CONFIG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_sh_slave_0_dec_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_slave0_rate_t val) +{ + lsm6ds3tr_c_slave0_config_t slave0_config; + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_A); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_SLAVE0_CONFIG, + (uint8_t *)&slave0_config, 1); + + if (ret == 0) + { + slave0_config.slave0_rate = (uint8_t) val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_SLAVE0_CONFIG, + (uint8_t *)&slave0_config, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + } + + return ret; +} + +/** + * @brief Decimation of read operation on Slave 0 starting from the + * sensor hub trigger.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of slave0_rate in reg SLAVE0_CONFIG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_sh_slave_0_dec_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_slave0_rate_t *val) +{ + lsm6ds3tr_c_slave0_config_t slave0_config; + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_A); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_SLAVE0_CONFIG, + (uint8_t *)&slave0_config, 1); + + if (ret == 0) + { + switch (slave0_config.slave0_rate) + { + case LSM6DS3TR_C_SL0_NO_DEC: + *val = LSM6DS3TR_C_SL0_NO_DEC; + break; + + case LSM6DS3TR_C_SL0_DEC_2: + *val = LSM6DS3TR_C_SL0_DEC_2; + break; + + case LSM6DS3TR_C_SL0_DEC_4: + *val = LSM6DS3TR_C_SL0_DEC_4; + break; + + case LSM6DS3TR_C_SL0_DEC_8: + *val = LSM6DS3TR_C_SL0_DEC_8; + break; + + default: + *val = LSM6DS3TR_C_SL0_DEC_ND; + break; + } + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + + return ret; +} + +/** + * @brief Slave 0 write operation is performed only at the first sensor + * hub cycle. + * This is effective if the Aux_sens_on[1:0] field in + * SLAVE0_CONFIG(04h) is set to a value other than 00.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of write_once in reg SLAVE1_CONFIG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_sh_write_mode_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_write_once_t val) +{ + lsm6ds3tr_c_slave1_config_t slave1_config; + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_A); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_SLAVE1_CONFIG, + (uint8_t *)&slave1_config, 1); + slave1_config.write_once = (uint8_t) val; + + if (ret == 0) + { + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_SLAVE1_CONFIG, + (uint8_t *)&slave1_config, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + } + + return ret; +} + +/** + * @brief Slave 0 write operation is performed only at the first sensor + * hub cycle. + * This is effective if the Aux_sens_on[1:0] field in + * SLAVE0_CONFIG(04h) is set to a value other than 00.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of write_once in reg SLAVE1_CONFIG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_sh_write_mode_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_write_once_t *val) +{ + lsm6ds3tr_c_slave1_config_t slave1_config; + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_A); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_SLAVE1_CONFIG, + (uint8_t *)&slave1_config, 1); + + if (ret == 0) + { + switch (slave1_config.write_once) + { + case LSM6DS3TR_C_EACH_SH_CYCLE: + *val = LSM6DS3TR_C_EACH_SH_CYCLE; + break; + + case LSM6DS3TR_C_ONLY_FIRST_CYCLE: + *val = LSM6DS3TR_C_ONLY_FIRST_CYCLE; + break; + + default: + *val = LSM6DS3TR_C_SH_WR_MODE_ND; + break; + } + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + + return ret; +} + +/** + * @brief Decimation of read operation on Slave 1 starting from the + * sensor hub trigger.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of slave1_rate in reg SLAVE1_CONFIG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_sh_slave_1_dec_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_slave1_rate_t val) +{ + lsm6ds3tr_c_slave1_config_t slave1_config; + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_A); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_SLAVE1_CONFIG, + (uint8_t *)&slave1_config, 1); + + if (ret == 0) + { + slave1_config.slave1_rate = (uint8_t) val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_SLAVE1_CONFIG, + (uint8_t *)&slave1_config, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + } + + return ret; +} + +/** + * @brief Decimation of read operation on Slave 1 starting from the + * sensor hub trigger.[get] + * + * @param ctx Read / write interface definitions reg SLAVE1_CONFIG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_sh_slave_1_dec_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_slave1_rate_t *val) +{ + lsm6ds3tr_c_slave1_config_t slave1_config; + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_A); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_SLAVE1_CONFIG, + (uint8_t *)&slave1_config, 1); + + if (ret == 0) + { + switch (slave1_config.slave1_rate) + { + case LSM6DS3TR_C_SL1_NO_DEC: + *val = LSM6DS3TR_C_SL1_NO_DEC; + break; + + case LSM6DS3TR_C_SL1_DEC_2: + *val = LSM6DS3TR_C_SL1_DEC_2; + break; + + case LSM6DS3TR_C_SL1_DEC_4: + *val = LSM6DS3TR_C_SL1_DEC_4; + break; + + case LSM6DS3TR_C_SL1_DEC_8: + *val = LSM6DS3TR_C_SL1_DEC_8; + break; + + default: + *val = LSM6DS3TR_C_SL1_DEC_ND; + break; + } + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + + return ret; +} + +/** + * @brief Decimation of read operation on Slave 2 starting from the + * sensor hub trigger.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of slave2_rate in reg SLAVE2_CONFIG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_sh_slave_2_dec_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_slave2_rate_t val) +{ + lsm6ds3tr_c_slave2_config_t slave2_config; + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_A); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_SLAVE2_CONFIG, + (uint8_t *)&slave2_config, 1); + + if (ret == 0) + { + slave2_config.slave2_rate = (uint8_t) val; + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_SLAVE2_CONFIG, + (uint8_t *)&slave2_config, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + } + + return ret; +} + +/** + * @brief Decimation of read operation on Slave 2 starting from the + * sensor hub trigger.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of slave2_rate in reg SLAVE2_CONFIG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_sh_slave_2_dec_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_slave2_rate_t *val) +{ + lsm6ds3tr_c_slave2_config_t slave2_config; + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_A); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_SLAVE2_CONFIG, + (uint8_t *)&slave2_config, 1); + + if (ret == 0) + { + switch (slave2_config.slave2_rate) + { + case LSM6DS3TR_C_SL2_NO_DEC: + *val = LSM6DS3TR_C_SL2_NO_DEC; + break; + + case LSM6DS3TR_C_SL2_DEC_2: + *val = LSM6DS3TR_C_SL2_DEC_2; + break; + + case LSM6DS3TR_C_SL2_DEC_4: + *val = LSM6DS3TR_C_SL2_DEC_4; + break; + + case LSM6DS3TR_C_SL2_DEC_8: + *val = LSM6DS3TR_C_SL2_DEC_8; + break; + + default: + *val = LSM6DS3TR_C_SL2_DEC_ND; + break; + } + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + + return ret; +} + +/** + * @brief Decimation of read operation on Slave 3 starting from the + * sensor hub trigger.[set] + * + * @param ctx Read / write interface definitions + * @param val Change the values of slave3_rate in reg SLAVE3_CONFIG + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_sh_slave_3_dec_set(stmdev_ctx_t *ctx, + lsm6ds3tr_c_slave3_rate_t val) +{ + lsm6ds3tr_c_slave3_config_t slave3_config; + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_A); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_SLAVE3_CONFIG, + (uint8_t *)&slave3_config, 1); + slave3_config.slave3_rate = (uint8_t)val; + + if (ret == 0) + { + ret = lsm6ds3tr_c_write_reg(ctx, LSM6DS3TR_C_SLAVE3_CONFIG, + (uint8_t *)&slave3_config, 1); + + if (ret == 0) + { + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + } + + return ret; +} + +/** + * @brief Decimation of read operation on Slave 3 starting from the + * sensor hub trigger.[get] + * + * @param ctx Read / write interface definitions + * @param val Get the values of slave3_rate in reg SLAVE3_CONFIG. + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6ds3tr_c_sh_slave_3_dec_get(stmdev_ctx_t *ctx, + lsm6ds3tr_c_slave3_rate_t *val) +{ + lsm6ds3tr_c_slave3_config_t slave3_config; + int32_t ret; + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_BANK_A); + + if (ret == 0) + { + ret = lsm6ds3tr_c_read_reg(ctx, LSM6DS3TR_C_SLAVE3_CONFIG, + (uint8_t *)&slave3_config, 1); + + if (ret == 0) + { + switch (slave3_config.slave3_rate) + { + case LSM6DS3TR_C_SL3_NO_DEC: + *val = LSM6DS3TR_C_SL3_NO_DEC; + break; + + case LSM6DS3TR_C_SL3_DEC_2: + *val = LSM6DS3TR_C_SL3_DEC_2; + break; + + case LSM6DS3TR_C_SL3_DEC_4: + *val = LSM6DS3TR_C_SL3_DEC_4; + break; + + case LSM6DS3TR_C_SL3_DEC_8: + *val = LSM6DS3TR_C_SL3_DEC_8; + break; + + default: + *val = LSM6DS3TR_C_SL3_DEC_ND; + break; + } + + ret = lsm6ds3tr_c_mem_bank_set(ctx, LSM6DS3TR_C_USER_BANK); + } + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @} + * + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/src/reg/lsm6dso_reg.c b/src/reg/lsm6dso_reg.c new file mode 100644 index 0000000..0d1e815 --- /dev/null +++ b/src/reg/lsm6dso_reg.c @@ -0,0 +1,11713 @@ +/** + ****************************************************************************** + * @file lsm6dso_reg.c + * @author Sensors Software Solution Team + * @brief LSM6DSO 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 "lsm6dso_reg.h" +#include + +/** + * @defgroup LSM6DSO + * @brief This file provides a set of functions needed to drive the + * lsm6dso enhanced inertial module. + * @{ + * + */ + +/** + * @defgroup LSM6DSO_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 read / write interface definitions(ptr) + * @param reg register to read + * @param data pointer to buffer that store the data read(ptr) + * @param len number of consecutive register to read + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t __weak lsm6dso_read_reg(stmdev_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 read / write interface definitions(ptr) + * @param reg register to write + * @param data pointer to data to write in register reg(ptr) + * @param len number of consecutive register to write + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t __weak lsm6dso_write_reg(stmdev_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 LSM6DSOX_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 LSM6DSO_Sensitivity + * @brief These functions convert raw-data into engineering units. + * @{ + * + */ +float_t lsm6dso_from_fs2_to_mg(int16_t lsb) +{ + return ((float_t)lsb) * 0.061f; +} + +float_t lsm6dso_from_fs4_to_mg(int16_t lsb) +{ + return ((float_t)lsb) * 0.122f; +} + +float_t lsm6dso_from_fs8_to_mg(int16_t lsb) +{ + return ((float_t)lsb) * 0.244f; +} + +float_t lsm6dso_from_fs16_to_mg(int16_t lsb) +{ + return ((float_t)lsb) * 0.488f; +} + +float_t lsm6dso_from_fs125_to_mdps(int16_t lsb) +{ + return ((float_t)lsb) * 4.375f; +} + +float_t lsm6dso_from_fs500_to_mdps(int16_t lsb) +{ + return ((float_t)lsb) * 17.50f; +} + +float_t lsm6dso_from_fs250_to_mdps(int16_t lsb) +{ + return ((float_t)lsb) * 8.750f; +} + +float_t lsm6dso_from_fs1000_to_mdps(int16_t lsb) +{ + return ((float_t)lsb) * 35.0f; +} + +float_t lsm6dso_from_fs2000_to_mdps(int16_t lsb) +{ + return ((float_t)lsb) * 70.0f; +} + +float_t lsm6dso_from_lsb_to_celsius(int16_t lsb) +{ + return (((float_t)lsb / 256.0f) + 25.0f); +} + +float_t lsm6dso_from_lsb_to_nsec(int16_t lsb) +{ + return ((float_t)lsb * 25000.0f); +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DSO_Data_Generation + * @brief This section groups all the functions concerning + * data generation. + * + */ + +/** + * @brief Accelerometer full-scale selection.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of fs_xl in reg CTRL1_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_xl_full_scale_set(stmdev_ctx_t *ctx, + lsm6dso_fs_xl_t val) +{ + lsm6dso_ctrl1_xl_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL1_XL, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.fs_xl = (uint8_t) val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL1_XL, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Accelerometer full-scale selection.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of fs_xl in reg CTRL1_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_xl_full_scale_get(stmdev_ctx_t *ctx, + lsm6dso_fs_xl_t *val) +{ + lsm6dso_ctrl1_xl_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL1_XL, (uint8_t *)®, 1); + + switch (reg.fs_xl) + { + case LSM6DSO_2g: + *val = LSM6DSO_2g; + break; + + case LSM6DSO_16g: + *val = LSM6DSO_16g; + break; + + case LSM6DSO_4g: + *val = LSM6DSO_4g; + break; + + case LSM6DSO_8g: + *val = LSM6DSO_8g; + break; + + default: + *val = LSM6DSO_2g; + break; + } + + return ret; +} + +/** + * @brief Accelerometer UI data rate selection.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of odr_xl in reg CTRL1_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_xl_data_rate_set(stmdev_ctx_t *ctx, + lsm6dso_odr_xl_t val) +{ + lsm6dso_odr_xl_t odr_xl = val; + lsm6dso_emb_fsm_enable_t fsm_enable; + lsm6dso_fsm_odr_t fsm_odr; + lsm6dso_ctrl1_xl_t reg; + int32_t ret; + + /* Check the Finite State Machine data rate constraints */ + ret = lsm6dso_fsm_enable_get(ctx, &fsm_enable); + if (ret != 0) { return ret; } + + if ((fsm_enable.fsm_enable_a.fsm1_en | + fsm_enable.fsm_enable_a.fsm2_en | + fsm_enable.fsm_enable_a.fsm3_en | + fsm_enable.fsm_enable_a.fsm4_en | + fsm_enable.fsm_enable_a.fsm5_en | + fsm_enable.fsm_enable_a.fsm6_en | + fsm_enable.fsm_enable_a.fsm7_en | + fsm_enable.fsm_enable_a.fsm8_en | + fsm_enable.fsm_enable_b.fsm9_en | + fsm_enable.fsm_enable_b.fsm10_en | + fsm_enable.fsm_enable_b.fsm11_en | + fsm_enable.fsm_enable_b.fsm12_en | + fsm_enable.fsm_enable_b.fsm13_en | + fsm_enable.fsm_enable_b.fsm14_en | + fsm_enable.fsm_enable_b.fsm15_en | + fsm_enable.fsm_enable_b.fsm16_en) == PROPERTY_ENABLE) + { + ret = lsm6dso_fsm_data_rate_get(ctx, &fsm_odr); + if (ret != 0) { return ret; } + + switch (fsm_odr) + { + case LSM6DSO_ODR_FSM_12Hz5: + if (val == LSM6DSO_XL_ODR_OFF) + { + odr_xl = LSM6DSO_XL_ODR_12Hz5; + } + else + { + odr_xl = val; + } + + break; + + case LSM6DSO_ODR_FSM_26Hz: + if (val == LSM6DSO_XL_ODR_OFF) + { + odr_xl = LSM6DSO_XL_ODR_26Hz; + } + else if (val == LSM6DSO_XL_ODR_12Hz5) + { + odr_xl = LSM6DSO_XL_ODR_26Hz; + } + else + { + odr_xl = val; + } + + break; + + case LSM6DSO_ODR_FSM_52Hz: + if (val == LSM6DSO_XL_ODR_OFF) + { + odr_xl = LSM6DSO_XL_ODR_52Hz; + } + else if (val == LSM6DSO_XL_ODR_12Hz5) + { + odr_xl = LSM6DSO_XL_ODR_52Hz; + } + else if (val == LSM6DSO_XL_ODR_26Hz) + { + odr_xl = LSM6DSO_XL_ODR_52Hz; + } + else + { + odr_xl = val; + } + + break; + + case LSM6DSO_ODR_FSM_104Hz: + if (val == LSM6DSO_XL_ODR_OFF) + { + odr_xl = LSM6DSO_XL_ODR_104Hz; + } + else if (val == LSM6DSO_XL_ODR_12Hz5) + { + odr_xl = LSM6DSO_XL_ODR_104Hz; + } + else if (val == LSM6DSO_XL_ODR_26Hz) + { + odr_xl = LSM6DSO_XL_ODR_104Hz; + } + else if (val == LSM6DSO_XL_ODR_52Hz) + { + odr_xl = LSM6DSO_XL_ODR_104Hz; + } + else + { + odr_xl = val; + } + + break; + + default: + odr_xl = val; + break; + } + } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL1_XL, (uint8_t *)®, 1); + reg.odr_xl = (uint8_t) odr_xl; + ret += lsm6dso_write_reg(ctx, LSM6DSO_CTRL1_XL, (uint8_t *)®, 1); + + return ret; +} + +/** + * @brief Accelerometer UI data rate selection.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of odr_xl in reg CTRL1_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_xl_data_rate_get(stmdev_ctx_t *ctx, + lsm6dso_odr_xl_t *val) +{ + lsm6dso_ctrl1_xl_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL1_XL, (uint8_t *)®, 1); + + switch (reg.odr_xl) + { + case LSM6DSO_XL_ODR_OFF: + *val = LSM6DSO_XL_ODR_OFF; + break; + + case LSM6DSO_XL_ODR_12Hz5: + *val = LSM6DSO_XL_ODR_12Hz5; + break; + + case LSM6DSO_XL_ODR_26Hz: + *val = LSM6DSO_XL_ODR_26Hz; + break; + + case LSM6DSO_XL_ODR_52Hz: + *val = LSM6DSO_XL_ODR_52Hz; + break; + + case LSM6DSO_XL_ODR_104Hz: + *val = LSM6DSO_XL_ODR_104Hz; + break; + + case LSM6DSO_XL_ODR_208Hz: + *val = LSM6DSO_XL_ODR_208Hz; + break; + + case LSM6DSO_XL_ODR_417Hz: + *val = LSM6DSO_XL_ODR_417Hz; + break; + + case LSM6DSO_XL_ODR_833Hz: + *val = LSM6DSO_XL_ODR_833Hz; + break; + + case LSM6DSO_XL_ODR_1667Hz: + *val = LSM6DSO_XL_ODR_1667Hz; + break; + + case LSM6DSO_XL_ODR_3333Hz: + *val = LSM6DSO_XL_ODR_3333Hz; + break; + + case LSM6DSO_XL_ODR_6667Hz: + *val = LSM6DSO_XL_ODR_6667Hz; + break; + + case LSM6DSO_XL_ODR_1Hz6: + *val = LSM6DSO_XL_ODR_1Hz6; + break; + + default: + *val = LSM6DSO_XL_ODR_OFF; + break; + } + + return ret; +} + +/** + * @brief Gyroscope UI chain full-scale selection.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of fs_g in reg CTRL2_G + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_gy_full_scale_set(stmdev_ctx_t *ctx, + lsm6dso_fs_g_t val) +{ + lsm6dso_ctrl2_g_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL2_G, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.fs_g = (uint8_t) val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL2_G, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Gyroscope UI chain full-scale selection.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of fs_g in reg CTRL2_G + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_gy_full_scale_get(stmdev_ctx_t *ctx, + lsm6dso_fs_g_t *val) +{ + lsm6dso_ctrl2_g_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL2_G, (uint8_t *)®, 1); + + switch (reg.fs_g) + { + case LSM6DSO_250dps: + *val = LSM6DSO_250dps; + break; + + case LSM6DSO_125dps: + *val = LSM6DSO_125dps; + break; + + case LSM6DSO_500dps: + *val = LSM6DSO_500dps; + break; + + case LSM6DSO_1000dps: + *val = LSM6DSO_1000dps; + break; + + case LSM6DSO_2000dps: + *val = LSM6DSO_2000dps; + break; + + default: + *val = LSM6DSO_250dps; + break; + } + + return ret; +} + +/** + * @brief Gyroscope UI data rate selection.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of odr_g in reg CTRL2_G + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_gy_data_rate_set(stmdev_ctx_t *ctx, + lsm6dso_odr_g_t val) +{ + lsm6dso_odr_g_t odr_gy = val; + lsm6dso_emb_fsm_enable_t fsm_enable; + lsm6dso_fsm_odr_t fsm_odr; + lsm6dso_ctrl2_g_t reg; + int32_t ret; + + /* Check the Finite State Machine data rate constraints */ + ret = lsm6dso_fsm_enable_get(ctx, &fsm_enable); + if (ret != 0) { return ret; } + + if ((fsm_enable.fsm_enable_a.fsm1_en | + fsm_enable.fsm_enable_a.fsm2_en | + fsm_enable.fsm_enable_a.fsm3_en | + fsm_enable.fsm_enable_a.fsm4_en | + fsm_enable.fsm_enable_a.fsm5_en | + fsm_enable.fsm_enable_a.fsm6_en | + fsm_enable.fsm_enable_a.fsm7_en | + fsm_enable.fsm_enable_a.fsm8_en | + fsm_enable.fsm_enable_b.fsm9_en | + fsm_enable.fsm_enable_b.fsm10_en | + fsm_enable.fsm_enable_b.fsm11_en | + fsm_enable.fsm_enable_b.fsm12_en | + fsm_enable.fsm_enable_b.fsm13_en | + fsm_enable.fsm_enable_b.fsm14_en | + fsm_enable.fsm_enable_b.fsm15_en | + fsm_enable.fsm_enable_b.fsm16_en) == PROPERTY_ENABLE) + { + ret = lsm6dso_fsm_data_rate_get(ctx, &fsm_odr); + if (ret != 0) { return ret; } + + switch (fsm_odr) + { + case LSM6DSO_ODR_FSM_12Hz5: + if (val == LSM6DSO_GY_ODR_OFF) + { + odr_gy = LSM6DSO_GY_ODR_12Hz5; + } + else + { + odr_gy = val; + } + + break; + + case LSM6DSO_ODR_FSM_26Hz: + if (val == LSM6DSO_GY_ODR_OFF) + { + odr_gy = LSM6DSO_GY_ODR_26Hz; + } + else if (val == LSM6DSO_GY_ODR_12Hz5) + { + odr_gy = LSM6DSO_GY_ODR_26Hz; + } + else + { + odr_gy = val; + } + + break; + + case LSM6DSO_ODR_FSM_52Hz: + if (val == LSM6DSO_GY_ODR_OFF) + { + odr_gy = LSM6DSO_GY_ODR_52Hz; + } + else if (val == LSM6DSO_GY_ODR_12Hz5) + { + odr_gy = LSM6DSO_GY_ODR_52Hz; + } + else if (val == LSM6DSO_GY_ODR_26Hz) + { + odr_gy = LSM6DSO_GY_ODR_52Hz; + } + else + { + odr_gy = val; + } + + break; + + case LSM6DSO_ODR_FSM_104Hz: + if (val == LSM6DSO_GY_ODR_OFF) + { + odr_gy = LSM6DSO_GY_ODR_104Hz; + } + else if (val == LSM6DSO_GY_ODR_12Hz5) + { + odr_gy = LSM6DSO_GY_ODR_104Hz; + } + else if (val == LSM6DSO_GY_ODR_26Hz) + { + odr_gy = LSM6DSO_GY_ODR_104Hz; + } + else if (val == LSM6DSO_GY_ODR_52Hz) + { + odr_gy = LSM6DSO_GY_ODR_104Hz; + } + else + { + odr_gy = val; + } + + break; + + default: + odr_gy = val; + break; + } + } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL2_G, (uint8_t *)®, 1); + reg.odr_g = (uint8_t) odr_gy; + ret += lsm6dso_write_reg(ctx, LSM6DSO_CTRL2_G, (uint8_t *)®, 1); + + return ret; +} + +/** + * @brief Gyroscope UI data rate selection.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of odr_g in reg CTRL2_G + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_gy_data_rate_get(stmdev_ctx_t *ctx, + lsm6dso_odr_g_t *val) +{ + lsm6dso_ctrl2_g_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL2_G, (uint8_t *)®, 1); + + switch (reg.odr_g) + { + case LSM6DSO_GY_ODR_OFF: + *val = LSM6DSO_GY_ODR_OFF; + break; + + case LSM6DSO_GY_ODR_12Hz5: + *val = LSM6DSO_GY_ODR_12Hz5; + break; + + case LSM6DSO_GY_ODR_26Hz: + *val = LSM6DSO_GY_ODR_26Hz; + break; + + case LSM6DSO_GY_ODR_52Hz: + *val = LSM6DSO_GY_ODR_52Hz; + break; + + case LSM6DSO_GY_ODR_104Hz: + *val = LSM6DSO_GY_ODR_104Hz; + break; + + case LSM6DSO_GY_ODR_208Hz: + *val = LSM6DSO_GY_ODR_208Hz; + break; + + case LSM6DSO_GY_ODR_417Hz: + *val = LSM6DSO_GY_ODR_417Hz; + break; + + case LSM6DSO_GY_ODR_833Hz: + *val = LSM6DSO_GY_ODR_833Hz; + break; + + case LSM6DSO_GY_ODR_1667Hz: + *val = LSM6DSO_GY_ODR_1667Hz; + break; + + case LSM6DSO_GY_ODR_3333Hz: + *val = LSM6DSO_GY_ODR_3333Hz; + break; + + case LSM6DSO_GY_ODR_6667Hz: + *val = LSM6DSO_GY_ODR_6667Hz; + break; + + default: + *val = LSM6DSO_GY_ODR_OFF; + break; + } + + return ret; +} + +/** + * @brief Block data update.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of bdu in reg CTRL3_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_block_data_update_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dso_ctrl3_c_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL3_C, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.bdu = val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL3_C, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Block data update.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of bdu in reg CTRL3_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_block_data_update_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_ctrl3_c_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL3_C, (uint8_t *)®, 1); + *val = reg.bdu; + + return ret; +} + +/** + * @brief Weight of XL user offset bits of registers X_OFS_USR (73h), + * Y_OFS_USR (74h), Z_OFS_USR (75h).[set] + * + * @param ctx read / write interface definitions + * @param val change the values of usr_off_w in reg CTRL6_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_xl_offset_weight_set(stmdev_ctx_t *ctx, + lsm6dso_usr_off_w_t val) +{ + lsm6dso_ctrl6_c_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL6_C, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.usr_off_w = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL6_C, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Weight of XL user offset bits of registers X_OFS_USR (73h), + * Y_OFS_USR (74h), Z_OFS_USR (75h).[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of usr_off_w in reg CTRL6_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_xl_offset_weight_get(stmdev_ctx_t *ctx, + lsm6dso_usr_off_w_t *val) +{ + lsm6dso_ctrl6_c_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL6_C, (uint8_t *)®, 1); + + switch (reg.usr_off_w) + { + case LSM6DSO_LSb_1mg: + *val = LSM6DSO_LSb_1mg; + break; + + case LSM6DSO_LSb_16mg: + *val = LSM6DSO_LSb_16mg; + break; + + default: + *val = LSM6DSO_LSb_1mg; + break; + } + + return ret; +} + +/** + * @brief Accelerometer power mode.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of xl_hm_mode in + * reg CTRL6_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_xl_power_mode_set(stmdev_ctx_t *ctx, + lsm6dso_xl_hm_mode_t val) +{ + lsm6dso_ctrl5_c_t ctrl5_c; + lsm6dso_ctrl6_c_t ctrl6_c; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL5_C, (uint8_t *) &ctrl5_c, 1); + if (ret != 0) { return ret; } + + ctrl5_c.xl_ulp_en = ((uint8_t)val & 0x02U) >> 1; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL5_C, (uint8_t *) &ctrl5_c, 1); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL6_C, (uint8_t *) &ctrl6_c, 1); + if (ret != 0) { return ret; } + + ctrl6_c.xl_hm_mode = (uint8_t)val & 0x01U; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL6_C, (uint8_t *) &ctrl6_c, 1); + + return ret; +} + +/** + * @brief Accelerometer power mode.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of xl_hm_mode in reg CTRL6_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_xl_power_mode_get(stmdev_ctx_t *ctx, + lsm6dso_xl_hm_mode_t *val) +{ + lsm6dso_ctrl5_c_t ctrl5_c; + lsm6dso_ctrl6_c_t ctrl6_c; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL5_C, (uint8_t *) &ctrl5_c, 1); + + if (ret == 0) + { + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL6_C, (uint8_t *) &ctrl6_c, 1); + + switch ((ctrl5_c.xl_ulp_en << 1) | ctrl6_c.xl_hm_mode) + { + case LSM6DSO_HIGH_PERFORMANCE_MD: + *val = LSM6DSO_HIGH_PERFORMANCE_MD; + break; + + case LSM6DSO_LOW_NORMAL_POWER_MD: + *val = LSM6DSO_LOW_NORMAL_POWER_MD; + break; + + case LSM6DSO_ULTRA_LOW_POWER_MD: + *val = LSM6DSO_ULTRA_LOW_POWER_MD; + break; + + default: + *val = LSM6DSO_HIGH_PERFORMANCE_MD; + break; + } + } + + return ret; +} + +/** + * @brief Operating mode for gyroscope.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of g_hm_mode in reg CTRL7_G + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_gy_power_mode_set(stmdev_ctx_t *ctx, + lsm6dso_g_hm_mode_t val) +{ + lsm6dso_ctrl7_g_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL7_G, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.g_hm_mode = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL7_G, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Operating mode for gyroscope.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of g_hm_mode in reg CTRL7_G + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_gy_power_mode_get(stmdev_ctx_t *ctx, + lsm6dso_g_hm_mode_t *val) +{ + lsm6dso_ctrl7_g_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL7_G, (uint8_t *)®, 1); + + switch (reg.g_hm_mode) + { + case LSM6DSO_GY_HIGH_PERFORMANCE: + *val = LSM6DSO_GY_HIGH_PERFORMANCE; + break; + + case LSM6DSO_GY_NORMAL: + *val = LSM6DSO_GY_NORMAL; + break; + + default: + *val = LSM6DSO_GY_HIGH_PERFORMANCE; + break; + } + + return ret; +} + +/** + * @brief The STATUS_REG register is read by the primary interface.[get] + * + * @param ctx read / write interface definitions + * @param val register STATUS_REG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_status_reg_get(stmdev_ctx_t *ctx, + lsm6dso_status_reg_t *val) +{ + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_STATUS_REG, (uint8_t *) val, 1); + + return ret; +} + +/** + * @brief Accelerometer new data available.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of xlda in reg STATUS_REG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_xl_flag_data_ready_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6dso_status_reg_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_STATUS_REG, (uint8_t *)®, 1); + *val = reg.xlda; + + return ret; +} + +/** + * @brief Gyroscope new data available.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of gda in reg STATUS_REG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_gy_flag_data_ready_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6dso_status_reg_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_STATUS_REG, (uint8_t *)®, 1); + *val = reg.gda; + + return ret; +} + +/** + * @brief Temperature new data available.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of tda in reg STATUS_REG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_temp_flag_data_ready_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6dso_status_reg_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_STATUS_REG, (uint8_t *)®, 1); + *val = reg.tda; + + return ret; +} + +/** + * @brief Accelerometer X-axis user offset correction expressed in + * two's complement, weight depends on USR_OFF_W in CTRL6_C (15h). + * The value must be in the range [-127 127].[set] + * + * @param ctx read / write interface definitions + * @param buff buffer that contains data to write + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_xl_usr_offset_x_set(stmdev_ctx_t *ctx, uint8_t *buff) +{ + int32_t ret; + + ret = lsm6dso_write_reg(ctx, LSM6DSO_X_OFS_USR, buff, 1); + + return ret; +} + +/** + * @brief Accelerometer X-axis user offset correction expressed in two's + * complement, weight depends on USR_OFF_W in CTRL6_C (15h). + * The value must be in the range [-127 127].[get] + * + * @param ctx read / write interface definitions + * @param buff buffer that stores data read + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_xl_usr_offset_x_get(stmdev_ctx_t *ctx, uint8_t *buff) +{ + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_X_OFS_USR, buff, 1); + + return ret; +} + +/** + * @brief Accelerometer Y-axis user offset correction expressed in two's + * complement, weight depends on USR_OFF_W in CTRL6_C (15h). + * The value must be in the range [-127 127].[set] + * + * @param ctx read / write interface definitions + * @param buff buffer that contains data to write + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_xl_usr_offset_y_set(stmdev_ctx_t *ctx, uint8_t *buff) +{ + int32_t ret; + + ret = lsm6dso_write_reg(ctx, LSM6DSO_Y_OFS_USR, buff, 1); + + return ret; +} + +/** + * @brief Accelerometer Y-axis user offset correction expressed in two's + * complement, weight depends on USR_OFF_W in CTRL6_C (15h). + * The value must be in the range [-127 127].[get] + * + * @param ctx read / write interface definitions + * @param buff buffer that stores data read + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_xl_usr_offset_y_get(stmdev_ctx_t *ctx, uint8_t *buff) +{ + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_Y_OFS_USR, buff, 1); + + return ret; +} + +/** + * @brief Accelerometer Z-axis user offset correction expressed in two's + * complement, weight depends on USR_OFF_W in CTRL6_C (15h). + * The value must be in the range [-127 127].[set] + * + * @param ctx read / write interface definitions + * @param buff buffer that contains data to write + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_xl_usr_offset_z_set(stmdev_ctx_t *ctx, uint8_t *buff) +{ + int32_t ret; + + ret = lsm6dso_write_reg(ctx, LSM6DSO_Z_OFS_USR, buff, 1); + + return ret; +} + +/** + * @brief Accelerometer Z-axis user offset correction expressed in two's + * complement, weight depends on USR_OFF_W in CTRL6_C (15h). + * The value must be in the range [-127 127].[get] + * + * @param ctx read / write interface definitions + * @param buff buffer that stores data read + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_xl_usr_offset_z_get(stmdev_ctx_t *ctx, uint8_t *buff) +{ + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_Z_OFS_USR, buff, 1); + + return ret; +} + +/** + * @brief Enables user offset on out.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of usr_off_on_out in reg CTRL7_G + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_xl_usr_offset_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dso_ctrl7_g_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL7_G, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.usr_off_on_out = val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL7_G, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief User offset on out flag.[get] + * + * @param ctx read / write interface definitions + * @param val values of usr_off_on_out in reg CTRL7_G + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_xl_usr_offset_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_ctrl7_g_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL7_G, (uint8_t *)®, 1); + *val = reg.usr_off_on_out; + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DSO_Timestamp + * @brief This section groups all the functions that manage the + * timestamp generation. + * @{ + * + */ + +/** + * @brief Reset timestamp counter.[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @retval Interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_timestamp_rst(stmdev_ctx_t *ctx) +{ + uint8_t rst_val = 0xAA; + return lsm6dso_write_reg(ctx, LSM6DSO_TIMESTAMP2, &rst_val, 1); +} + +/** + * @brief Enables timestamp counter.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of timestamp_en in reg CTRL10_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_timestamp_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dso_ctrl10_c_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL10_C, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.timestamp_en = val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL10_C, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Enables timestamp counter.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of timestamp_en in reg CTRL10_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_timestamp_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_ctrl10_c_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL10_C, (uint8_t *)®, 1); + *val = reg.timestamp_en; + + return ret; +} + +/** + * @brief Timestamp first data output register (r). + * The value is expressed as a 32-bit word and the bit + * resolution is 25 us.[get] + * + * @param ctx read / write interface definitions + * @param buff buffer that stores data read + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_timestamp_raw_get(stmdev_ctx_t *ctx, uint32_t *val) +{ + uint8_t buff[4]; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_TIMESTAMP0, buff, 4); + *val = buff[3]; + *val = (*val * 256U) + buff[2]; + *val = (*val * 256U) + buff[1]; + *val = (*val * 256U) + buff[0]; + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DSO_Data output + * @brief This section groups all the data output functions. + * @{ + * + */ + +/** + * @brief Circular burst-mode (rounding) read of the output + * registers.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of rounding in reg CTRL5_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_rounding_mode_set(stmdev_ctx_t *ctx, + lsm6dso_rounding_t val) +{ + lsm6dso_ctrl5_c_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL5_C, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.rounding = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL5_C, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Gyroscope UI chain full-scale selection.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of rounding in reg CTRL5_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_rounding_mode_get(stmdev_ctx_t *ctx, + lsm6dso_rounding_t *val) +{ + lsm6dso_ctrl5_c_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL5_C, (uint8_t *)®, 1); + + switch (reg.rounding) + { + case LSM6DSO_NO_ROUND: + *val = LSM6DSO_NO_ROUND; + break; + + case LSM6DSO_ROUND_XL: + *val = LSM6DSO_ROUND_XL; + break; + + case LSM6DSO_ROUND_GY: + *val = LSM6DSO_ROUND_GY; + break; + + case LSM6DSO_ROUND_GY_XL: + *val = LSM6DSO_ROUND_GY_XL; + break; + + default: + *val = LSM6DSO_NO_ROUND; + break; + } + + return ret; +} + +/** + * @brief Temperature data output register (r). + * L and H registers together express a 16-bit word in two's + * complement.[get] + * + * @param ctx read / write interface definitions + * @param buff buffer that stores data read + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_temperature_raw_get(stmdev_ctx_t *ctx, int16_t *val) +{ + uint8_t buff[2]; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_OUT_TEMP_L, buff, 2); + val[0] = (int16_t)buff[1]; + val[0] = (val[0] * 256) + (int16_t)buff[0]; + + return ret; +} + +/** + * @brief Angular rate sensor. The value is expressed as a 16-bit + * word in two's complement.[get] + * + * @param ctx read / write interface definitions + * @param buff buffer that stores data read + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_angular_rate_raw_get(stmdev_ctx_t *ctx, int16_t *val) +{ + uint8_t buff[6]; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_OUTX_L_G, buff, 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 output register. + * The value is expressed as a 16-bit word in two's complement.[get] + * + * @param ctx read / write interface definitions + * @param buff buffer that stores data read + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_acceleration_raw_get(stmdev_ctx_t *ctx, int16_t *val) +{ + uint8_t buff[6]; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_OUTX_L_A, buff, 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 FIFO data output [get] + * + * @param ctx read / write interface definitions + * @param buff buffer that stores data read + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_fifo_out_raw_get(stmdev_ctx_t *ctx, uint8_t *buff) +{ + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_FIFO_DATA_OUT_X_L, buff, 6); + + return ret; +} + +/** + * @brief Step counter output register.[get] + * + * @param ctx read / write interface definitions + * @param buff buffer that stores data read + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_number_of_steps_get(stmdev_ctx_t *ctx, uint16_t *val) +{ + uint8_t buff[2]; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_EMBEDDED_FUNC_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_STEP_COUNTER_L, buff, 2); + if (ret != 0) { goto exit; } + + *val = buff[1]; + *val = (*val * 256U) + buff[0]; + +exit: + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @brief Reset step counter register.[get] + * + * @param ctx read / write interface definitions + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_steps_reset(stmdev_ctx_t *ctx) +{ + lsm6dso_emb_func_src_t reg; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_EMBEDDED_FUNC_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_EMB_FUNC_SRC, (uint8_t *)®, 1); + if (ret != 0) { goto exit; } + + reg.pedo_rst_step = PROPERTY_ENABLE; + ret = lsm6dso_write_reg(ctx, LSM6DSO_EMB_FUNC_SRC, (uint8_t *)®, 1); + +exit: + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DSO_common + * @brief This section groups common useful functions. + * @{ + * + */ + +/** + * @brief Difference in percentage of the effective ODR(and timestamp rate) + * with respect to the typical. + * Step: 0.15%. 8-bit format, 2's complement.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of freq_fine in reg + * INTERNAL_FREQ_FINE + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_odr_cal_reg_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dso_internal_freq_fine_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_INTERNAL_FREQ_FINE, + (uint8_t *)®, 1); + + if (ret == 0) + { + reg.freq_fine = val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_INTERNAL_FREQ_FINE, + (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Difference in percentage of the effective ODR(and timestamp rate) + * with respect to the typical. + * Step: 0.15%. 8-bit format, 2's complement.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of freq_fine in reg INTERNAL_FREQ_FINE + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_odr_cal_reg_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_internal_freq_fine_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_INTERNAL_FREQ_FINE, + (uint8_t *)®, 1); + *val = reg.freq_fine; + + return ret; +} + + +/** + * @brief Enable access to the embedded functions/sensor + * hub configuration registers.[set] + * + * @param ctx read / write interface definitions + * @param val change register page (reg_access in FUNC_CFG_ACCESS) + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_mem_bank_set(stmdev_ctx_t *ctx, + lsm6dso_reg_access_t val) +{ + lsm6dso_func_cfg_access_t reg = {0}; + int32_t ret; + + /* no need to read it first as the pther bits are reserved and must be zero */ + reg.reg_access = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_FUNC_CFG_ACCESS, (uint8_t *)®, 1); + + return ret; +} + +/** + * @brief Enable access to the embedded functions/sensor + * hub configuration registers.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of reg_access in reg FUNC_CFG_ACCESS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_mem_bank_get(stmdev_ctx_t *ctx, + lsm6dso_reg_access_t *val) +{ + lsm6dso_func_cfg_access_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_FUNC_CFG_ACCESS, (uint8_t *)®, 1); + + switch (reg.reg_access) + { + case LSM6DSO_USER_BANK: + *val = LSM6DSO_USER_BANK; + break; + + case LSM6DSO_SENSOR_HUB_BANK: + *val = LSM6DSO_SENSOR_HUB_BANK; + break; + + case LSM6DSO_EMBEDDED_FUNC_BANK: + *val = LSM6DSO_EMBEDDED_FUNC_BANK; + break; + + default: + *val = LSM6DSO_USER_BANK; + break; + } + + return ret; +} + +/** + * @brief Write a line(byte) in a page.[set] + * + * @param ctx read / write interface definitions + * @param uint8_t address: page line address + * @param val value to write + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_ln_pg_write_byte(stmdev_ctx_t *ctx, uint16_t address, uint8_t *val) +{ + return lsm6dso_ln_pg_write(ctx, address, val, 1); +} + +/** + * @brief Write buffer in a page.[set] + * + * @param ctx read / write interface definitions + * @param uint8_t address: page line address + * @param uint8_t *buf: buffer to write + * @param uint8_t len: buffer len + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_ln_pg_write(stmdev_ctx_t *ctx, uint16_t address, + uint8_t *buf, uint8_t len) +{ + lsm6dso_page_rw_t page_rw; + lsm6dso_page_sel_t page_sel; + lsm6dso_page_address_t page_address; + uint8_t msb; + uint8_t lsb; + int32_t ret; + uint8_t i ; + + msb = ((uint8_t)(address >> 8) & 0x0FU); + lsb = (uint8_t)address & 0xFFU; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_EMBEDDED_FUNC_BANK); + if (ret != 0) { return ret; } + + /* set page write */ + ret = lsm6dso_read_reg(ctx, LSM6DSO_PAGE_RW, (uint8_t *) &page_rw, 1); + page_rw.page_rw = 0x02; /* page_write enable*/ + ret += lsm6dso_write_reg(ctx, LSM6DSO_PAGE_RW, (uint8_t *) &page_rw, 1); + if (ret != 0) { goto exit; } + + /* select page */ + ret = lsm6dso_read_reg(ctx, LSM6DSO_PAGE_SEL, (uint8_t *) &page_sel, 1); + page_sel.page_sel = msb; + page_sel.not_used_01 = 1; + ret += lsm6dso_write_reg(ctx, LSM6DSO_PAGE_SEL, (uint8_t *) &page_sel, 1); + if (ret != 0) { goto exit; } + + /* set page addr */ + page_address.page_addr = lsb; + ret += lsm6dso_write_reg(ctx, LSM6DSO_PAGE_ADDRESS, + (uint8_t *)&page_address, 1); + if (ret != 0) { goto exit; } + + for (i = 0; ((i < len) && (ret == 0)); i++) + { + ret = lsm6dso_write_reg(ctx, LSM6DSO_PAGE_VALUE, &buf[i], 1); + if (ret != 0) { goto exit; } + + lsb++; + + /* Check if page wrap */ + if (((lsb & 0xFFU) == 0x00U) && (ret == 0)) + { + msb++; + ret += lsm6dso_read_reg(ctx, LSM6DSO_PAGE_SEL, (uint8_t *)&page_sel, 1); + if (ret != 0) { goto exit; } + + page_sel.page_sel = msb; + page_sel.not_used_01 = 1; + ret = lsm6dso_write_reg(ctx, LSM6DSO_PAGE_SEL, (uint8_t *)&page_sel, 1); + if (ret != 0) { goto exit; } + } + } + + page_sel.page_sel = 0; + page_sel.not_used_01 = 1; + ret = lsm6dso_write_reg(ctx, LSM6DSO_PAGE_SEL, (uint8_t *) &page_sel, 1); + if (ret != 0) { goto exit; } + + /* unset page write */ + ret = lsm6dso_read_reg(ctx, LSM6DSO_PAGE_RW, (uint8_t *) &page_rw, 1); + page_rw.page_rw = 0x00; /* page_write disable */ + ret += lsm6dso_write_reg(ctx, LSM6DSO_PAGE_RW, (uint8_t *) &page_rw, 1); + +exit: + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @brief Read a line(byte) in a page.[get] + * + * @param ctx read / write interface definitions + * @param uint8_t address: page line address + * @param val read value + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_ln_pg_read_byte(stmdev_ctx_t *ctx, uint16_t address, uint8_t *val) +{ + return lsm6dso_ln_pg_read(ctx, address, val, 1); +} + +int32_t lsm6dso_ln_pg_read(stmdev_ctx_t *ctx, uint16_t address, uint8_t *buf, + uint8_t len) +{ + lsm6dso_page_rw_t page_rw; + lsm6dso_page_sel_t page_sel; + lsm6dso_page_address_t page_address; + uint8_t msb; + uint8_t lsb; + int32_t ret; + uint8_t i ; + + msb = ((uint8_t)(address >> 8) & 0x0FU); + lsb = (uint8_t)address & 0xFFU; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_EMBEDDED_FUNC_BANK); + if (ret != 0) { return ret; } + + /* set page write */ + ret = lsm6dso_read_reg(ctx, LSM6DSO_PAGE_RW, (uint8_t *) &page_rw, 1); + page_rw.page_rw = 0x01; /* page_read enable*/ + ret += lsm6dso_write_reg(ctx, LSM6DSO_PAGE_RW, (uint8_t *) &page_rw, 1); + if (ret != 0) { goto exit; } + + /* select page */ + ret = lsm6dso_read_reg(ctx, LSM6DSO_PAGE_SEL, (uint8_t *) &page_sel, 1); + page_sel.page_sel = msb; + page_sel.not_used_01 = 1; + ret += lsm6dso_write_reg(ctx, LSM6DSO_PAGE_SEL, (uint8_t *) &page_sel, 1); + if (ret != 0) { goto exit; } + + for (i = 0; ((i < len) && (ret == 0)); i++) + { + /* set page addr */ + page_address.page_addr = lsb; + ret = lsm6dso_write_reg(ctx, LSM6DSO_PAGE_ADDRESS, + (uint8_t *)&page_address, 1); + if (ret != 0) { goto exit; } + + ret += lsm6dso_read_reg(ctx, LSM6DSO_PAGE_VALUE, &buf[i], 1); + if (ret != 0) { goto exit; } + + lsb++; + + /* Check if page wrap */ + if (((lsb & 0xFFU) == 0x00U) && (ret == 0)) + { + msb++; + ret += lsm6dso_read_reg(ctx, LSM6DSO_PAGE_SEL, (uint8_t *)&page_sel, 1); + if (ret != 0) { goto exit; } + + page_sel.page_sel = msb; + page_sel.not_used_01 = 1; + ret += lsm6dso_write_reg(ctx, LSM6DSO_PAGE_SEL, (uint8_t *)&page_sel, 1); + if (ret != 0) { goto exit; } + } + } + + page_sel.page_sel = 0; + page_sel.not_used_01 = 1; + ret = lsm6dso_write_reg(ctx, LSM6DSO_PAGE_SEL, (uint8_t *) &page_sel, 1); + if (ret != 0) { goto exit; } + + /* unset page write */ + ret = lsm6dso_read_reg(ctx, LSM6DSO_PAGE_RW, (uint8_t *) &page_rw, 1); + page_rw.page_rw = 0x00; /* page_write disable */ + ret += lsm6dso_write_reg(ctx, LSM6DSO_PAGE_RW, (uint8_t *) &page_rw, 1); + +exit: + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @brief Data-ready pulsed / letched mode.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of + * dataready_pulsed in + * reg COUNTER_BDR_REG1 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_data_ready_mode_set(stmdev_ctx_t *ctx, + lsm6dso_dataready_pulsed_t val) +{ + lsm6dso_counter_bdr_reg1_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_COUNTER_BDR_REG1, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.dataready_pulsed = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_COUNTER_BDR_REG1, + (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Data-ready pulsed / letched mode.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of + * dataready_pulsed in + * reg COUNTER_BDR_REG1 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_data_ready_mode_get(stmdev_ctx_t *ctx, + lsm6dso_dataready_pulsed_t *val) +{ + lsm6dso_counter_bdr_reg1_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_COUNTER_BDR_REG1, (uint8_t *)®, 1); + + switch (reg.dataready_pulsed) + { + case LSM6DSO_DRDY_LATCHED: + *val = LSM6DSO_DRDY_LATCHED; + break; + + case LSM6DSO_DRDY_PULSED: + *val = LSM6DSO_DRDY_PULSED; + break; + + default: + *val = LSM6DSO_DRDY_LATCHED; + break; + } + + return ret; +} + +/** + * @brief Device "Who am I".[get] + * + * @param ctx read / write interface definitions + * @param buff buffer that stores data read + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_device_id_get(stmdev_ctx_t *ctx, uint8_t *buff) +{ + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_WHO_AM_I, buff, 1); + + return ret; +} + +/** + * @brief Software reset. Restore the default values + * in user registers[set] + * + * @param ctx read / write interface definitions + * @param val change the values of sw_reset in reg CTRL3_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_reset_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dso_ctrl3_c_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL3_C, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.sw_reset = val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL3_C, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Software reset. Restore the default values in user registers.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of sw_reset in reg CTRL3_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_reset_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_ctrl3_c_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL3_C, (uint8_t *)®, 1); + *val = reg.sw_reset; + + return ret; +} + +/** + * @brief Register address automatically incremented during a multiple byte + * access with a serial interface.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of if_inc in reg CTRL3_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_auto_increment_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dso_ctrl3_c_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL3_C, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.if_inc = val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL3_C, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Register address automatically incremented during a multiple byte + * access with a serial interface.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of if_inc in reg CTRL3_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_auto_increment_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_ctrl3_c_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL3_C, (uint8_t *)®, 1); + *val = reg.if_inc; + + return ret; +} + +/** + * @brief Reboot memory content. Reload the calibration parameters.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of boot in reg CTRL3_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_boot_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dso_ctrl3_c_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL3_C, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.boot = val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL3_C, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Reboot memory content. Reload the calibration parameters.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of boot in reg CTRL3_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_boot_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_ctrl3_c_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL3_C, (uint8_t *)®, 1); + *val = reg.boot; + + return ret; +} + +/** + * @brief Linear acceleration sensor self-test enable.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of st_xl in reg CTRL5_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_xl_self_test_set(stmdev_ctx_t *ctx, + lsm6dso_st_xl_t val) +{ + lsm6dso_ctrl5_c_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL5_C, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.st_xl = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL5_C, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Linear acceleration sensor self-test enable.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of st_xl in reg CTRL5_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_xl_self_test_get(stmdev_ctx_t *ctx, + lsm6dso_st_xl_t *val) +{ + lsm6dso_ctrl5_c_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL5_C, (uint8_t *)®, 1); + + switch (reg.st_xl) + { + case LSM6DSO_XL_ST_DISABLE: + *val = LSM6DSO_XL_ST_DISABLE; + break; + + case LSM6DSO_XL_ST_POSITIVE: + *val = LSM6DSO_XL_ST_POSITIVE; + break; + + case LSM6DSO_XL_ST_NEGATIVE: + *val = LSM6DSO_XL_ST_NEGATIVE; + break; + + default: + *val = LSM6DSO_XL_ST_DISABLE; + break; + } + + return ret; +} + +/** + * @brief Angular rate sensor self-test enable.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of st_g in reg CTRL5_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_gy_self_test_set(stmdev_ctx_t *ctx, + lsm6dso_st_g_t val) +{ + lsm6dso_ctrl5_c_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL5_C, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.st_g = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL5_C, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Angular rate sensor self-test enable.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of st_g in reg CTRL5_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_gy_self_test_get(stmdev_ctx_t *ctx, + lsm6dso_st_g_t *val) +{ + lsm6dso_ctrl5_c_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL5_C, (uint8_t *)®, 1); + + switch (reg.st_g) + { + case LSM6DSO_GY_ST_DISABLE: + *val = LSM6DSO_GY_ST_DISABLE; + break; + + case LSM6DSO_GY_ST_POSITIVE: + *val = LSM6DSO_GY_ST_POSITIVE; + break; + + case LSM6DSO_GY_ST_NEGATIVE: + *val = LSM6DSO_GY_ST_NEGATIVE; + break; + + default: + *val = LSM6DSO_GY_ST_DISABLE; + break; + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DSO_filters + * @brief This section group all the functions concerning the + * filters configuration + * @{ + * + */ + +/** + * @brief Accelerometer output from LPF2 filtering stage selection.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of lpf2_xl_en in reg CTRL1_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_xl_filter_lp2_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dso_ctrl1_xl_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL1_XL, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.lpf2_xl_en = val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL1_XL, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Accelerometer output from LPF2 filtering stage selection.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of lpf2_xl_en in reg CTRL1_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_xl_filter_lp2_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_ctrl1_xl_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL1_XL, (uint8_t *)®, 1); + *val = reg.lpf2_xl_en; + + return ret; +} + +/** + * @brief Enables gyroscope digital LPF1 if auxiliary SPI is disabled; + * the bandwidth can be selected through FTYPE [2:0] + * in CTRL6_C (15h).[set] + * + * @param ctx read / write interface definitions + * @param val change the values of lpf1_sel_g in reg CTRL4_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_gy_filter_lp1_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dso_ctrl4_c_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL4_C, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.lpf1_sel_g = val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL4_C, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Enables gyroscope digital LPF1 if auxiliary SPI is disabled; + * the bandwidth can be selected through FTYPE [2:0] + * in CTRL6_C (15h).[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of lpf1_sel_g in reg CTRL4_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_gy_filter_lp1_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_ctrl4_c_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL4_C, (uint8_t *)®, 1); + *val = reg.lpf1_sel_g; + + return ret; +} + +/** + * @brief Mask DRDY on pin (both XL & Gyro) until filter settling ends + * (XL and Gyro independently masked).[set] + * + * @param ctx read / write interface definitions + * @param val change the values of drdy_mask in reg CTRL4_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_filter_settling_mask_set(stmdev_ctx_t *ctx, + uint8_t val) +{ + lsm6dso_ctrl4_c_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL4_C, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.drdy_mask = val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL4_C, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Mask DRDY on pin (both XL & Gyro) until filter settling ends + * (XL and Gyro independently masked).[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of drdy_mask in reg CTRL4_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_filter_settling_mask_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6dso_ctrl4_c_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL4_C, (uint8_t *)®, 1); + *val = reg.drdy_mask; + + return ret; +} + +/** + * @brief Gyroscope lp1 bandwidth.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of ftype in reg CTRL6_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_gy_lp1_bandwidth_set(stmdev_ctx_t *ctx, + lsm6dso_ftype_t val) +{ + lsm6dso_ctrl6_c_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL6_C, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.ftype = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL6_C, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Gyroscope lp1 bandwidth.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of ftype in reg CTRL6_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_gy_lp1_bandwidth_get(stmdev_ctx_t *ctx, + lsm6dso_ftype_t *val) +{ + lsm6dso_ctrl6_c_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL6_C, (uint8_t *)®, 1); + + switch (reg.ftype) + { + case LSM6DSO_ULTRA_LIGHT: + *val = LSM6DSO_ULTRA_LIGHT; + break; + + case LSM6DSO_VERY_LIGHT: + *val = LSM6DSO_VERY_LIGHT; + break; + + case LSM6DSO_LIGHT: + *val = LSM6DSO_LIGHT; + break; + + case LSM6DSO_MEDIUM: + *val = LSM6DSO_MEDIUM; + break; + + case LSM6DSO_STRONG: + *val = LSM6DSO_STRONG; + break; + + case LSM6DSO_VERY_STRONG: + *val = LSM6DSO_VERY_STRONG; + break; + + case LSM6DSO_AGGRESSIVE: + *val = LSM6DSO_AGGRESSIVE; + break; + + case LSM6DSO_XTREME: + *val = LSM6DSO_XTREME; + break; + + default: + *val = LSM6DSO_ULTRA_LIGHT; + break; + } + + return ret; +} + +/** + * @brief Low pass filter 2 on 6D function selection.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of low_pass_on_6d in reg CTRL8_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_xl_lp2_on_6d_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dso_ctrl8_xl_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL8_XL, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.low_pass_on_6d = val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL8_XL, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Low pass filter 2 on 6D function selection.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of low_pass_on_6d in reg CTRL8_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_xl_lp2_on_6d_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_ctrl8_xl_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL8_XL, (uint8_t *)®, 1); + *val = reg.low_pass_on_6d; + + return ret; +} + +/** + * @brief Accelerometer slope filter / high-pass filter selection + * on output.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of hp_slope_xl_en + * in reg CTRL8_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_xl_hp_path_on_out_set(stmdev_ctx_t *ctx, + lsm6dso_hp_slope_xl_en_t val) +{ + lsm6dso_ctrl8_xl_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL8_XL, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.hp_slope_xl_en = ((uint8_t)val & 0x10U) >> 4; + reg.hp_ref_mode_xl = ((uint8_t)val & 0x20U) >> 5; + reg.hpcf_xl = (uint8_t)val & 0x07U; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL8_XL, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Accelerometer slope filter / high-pass filter selection + * on output.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of hp_slope_xl_en + * in reg CTRL8_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_xl_hp_path_on_out_get(stmdev_ctx_t *ctx, + lsm6dso_hp_slope_xl_en_t *val) +{ + lsm6dso_ctrl8_xl_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL8_XL, (uint8_t *)®, 1); + + switch ((reg.hp_ref_mode_xl << 5) | (reg.hp_slope_xl_en << 4) | + reg.hpcf_xl) + { + case LSM6DSO_HP_PATH_DISABLE_ON_OUT: + *val = LSM6DSO_HP_PATH_DISABLE_ON_OUT; + break; + + case LSM6DSO_SLOPE_ODR_DIV_4: + *val = LSM6DSO_SLOPE_ODR_DIV_4; + break; + + case LSM6DSO_HP_ODR_DIV_10: + *val = LSM6DSO_HP_ODR_DIV_10; + break; + + case LSM6DSO_HP_ODR_DIV_20: + *val = LSM6DSO_HP_ODR_DIV_20; + break; + + case LSM6DSO_HP_ODR_DIV_45: + *val = LSM6DSO_HP_ODR_DIV_45; + break; + + case LSM6DSO_HP_ODR_DIV_100: + *val = LSM6DSO_HP_ODR_DIV_100; + break; + + case LSM6DSO_HP_ODR_DIV_200: + *val = LSM6DSO_HP_ODR_DIV_200; + break; + + case LSM6DSO_HP_ODR_DIV_400: + *val = LSM6DSO_HP_ODR_DIV_400; + break; + + case LSM6DSO_HP_ODR_DIV_800: + *val = LSM6DSO_HP_ODR_DIV_800; + break; + + case LSM6DSO_HP_REF_MD_ODR_DIV_10: + *val = LSM6DSO_HP_REF_MD_ODR_DIV_10; + break; + + case LSM6DSO_HP_REF_MD_ODR_DIV_20: + *val = LSM6DSO_HP_REF_MD_ODR_DIV_20; + break; + + case LSM6DSO_HP_REF_MD_ODR_DIV_45: + *val = LSM6DSO_HP_REF_MD_ODR_DIV_45; + break; + + case LSM6DSO_HP_REF_MD_ODR_DIV_100: + *val = LSM6DSO_HP_REF_MD_ODR_DIV_100; + break; + + case LSM6DSO_HP_REF_MD_ODR_DIV_200: + *val = LSM6DSO_HP_REF_MD_ODR_DIV_200; + break; + + case LSM6DSO_HP_REF_MD_ODR_DIV_400: + *val = LSM6DSO_HP_REF_MD_ODR_DIV_400; + break; + + case LSM6DSO_HP_REF_MD_ODR_DIV_800: + *val = LSM6DSO_HP_REF_MD_ODR_DIV_800; + break; + + case LSM6DSO_LP_ODR_DIV_10: + *val = LSM6DSO_LP_ODR_DIV_10; + break; + + case LSM6DSO_LP_ODR_DIV_20: + *val = LSM6DSO_LP_ODR_DIV_20; + break; + + case LSM6DSO_LP_ODR_DIV_45: + *val = LSM6DSO_LP_ODR_DIV_45; + break; + + case LSM6DSO_LP_ODR_DIV_100: + *val = LSM6DSO_LP_ODR_DIV_100; + break; + + case LSM6DSO_LP_ODR_DIV_200: + *val = LSM6DSO_LP_ODR_DIV_200; + break; + + case LSM6DSO_LP_ODR_DIV_400: + *val = LSM6DSO_LP_ODR_DIV_400; + break; + + case LSM6DSO_LP_ODR_DIV_800: + *val = LSM6DSO_LP_ODR_DIV_800; + break; + + default: + *val = LSM6DSO_HP_PATH_DISABLE_ON_OUT; + break; + } + + return ret; +} + +/** + * @brief Enables accelerometer LPF2 and HPF fast-settling mode. + * The filter sets the second samples after writing this bit. + * Active only during device exit from power-down mode.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of fastsettl_mode_xl in + * reg CTRL8_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_xl_fast_settling_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dso_ctrl8_xl_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL8_XL, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.fastsettl_mode_xl = val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL8_XL, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Enables accelerometer LPF2 and HPF fast-settling mode. + * The filter sets the second samples after writing this bit. + * Active only during device exit from power-down mode.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of fastsettl_mode_xl in reg CTRL8_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_xl_fast_settling_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_ctrl8_xl_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL8_XL, (uint8_t *)®, 1); + *val = reg.fastsettl_mode_xl; + + return ret; +} + +/** + * @brief HPF or SLOPE filter selection on wake-up and Activity/Inactivity + * functions.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of slope_fds in reg TAP_CFG0 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_xl_hp_path_internal_set(stmdev_ctx_t *ctx, + lsm6dso_slope_fds_t val) +{ + lsm6dso_tap_cfg0_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_TAP_CFG0, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.slope_fds = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_TAP_CFG0, (uint8_t *)®, 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 Get the values of slope_fds in reg TAP_CFG0 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_xl_hp_path_internal_get(stmdev_ctx_t *ctx, + lsm6dso_slope_fds_t *val) +{ + lsm6dso_tap_cfg0_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_TAP_CFG0, (uint8_t *)®, 1); + + switch (reg.slope_fds) + { + case LSM6DSO_USE_SLOPE: + *val = LSM6DSO_USE_SLOPE; + break; + + case LSM6DSO_USE_HPF: + *val = LSM6DSO_USE_HPF; + break; + + default: + *val = LSM6DSO_USE_SLOPE; + break; + } + + return ret; +} + +/** + * @brief Enables gyroscope digital high-pass filter. The filter is + * enabled only if the gyro is in HP mode.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of hp_en_g and hp_en_g + * in reg CTRL7_G + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_gy_hp_path_internal_set(stmdev_ctx_t *ctx, + lsm6dso_hpm_g_t val) +{ + lsm6dso_ctrl7_g_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL7_G, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.hp_en_g = ((uint8_t)val & 0x80U) >> 7; + reg.hpm_g = (uint8_t)val & 0x03U; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL7_G, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Enables gyroscope digital high-pass filter. The filter is + * enabled only if the gyro is in HP mode.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of hp_en_g and hp_en_g + * in reg CTRL7_G + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_gy_hp_path_internal_get(stmdev_ctx_t *ctx, + lsm6dso_hpm_g_t *val) +{ + lsm6dso_ctrl7_g_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL7_G, (uint8_t *)®, 1); + + switch ((reg.hp_en_g << 7) + reg.hpm_g) + { + case LSM6DSO_HP_FILTER_NONE: + *val = LSM6DSO_HP_FILTER_NONE; + break; + + case LSM6DSO_HP_FILTER_16mHz: + *val = LSM6DSO_HP_FILTER_16mHz; + break; + + case LSM6DSO_HP_FILTER_65mHz: + *val = LSM6DSO_HP_FILTER_65mHz; + break; + + case LSM6DSO_HP_FILTER_260mHz: + *val = LSM6DSO_HP_FILTER_260mHz; + break; + + case LSM6DSO_HP_FILTER_1Hz04: + *val = LSM6DSO_HP_FILTER_1Hz04; + break; + + default: + *val = LSM6DSO_HP_FILTER_NONE; + break; + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DSO_ Auxiliary_interface + * @brief This section groups all the functions concerning + * auxiliary interface. + * @{ + * + */ + +/** + * @brief aOn auxiliary interface connect/disconnect SDO and OCS + * internal pull-up.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of ois_pu_dis in + * reg PIN_CTRL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_aux_sdo_ocs_mode_set(stmdev_ctx_t *ctx, + lsm6dso_ois_pu_dis_t val) +{ + lsm6dso_pin_ctrl_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_PIN_CTRL, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.ois_pu_dis = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_PIN_CTRL, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief On auxiliary interface connect/disconnect SDO and OCS + * internal pull-up.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of ois_pu_dis in reg PIN_CTRL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_aux_sdo_ocs_mode_get(stmdev_ctx_t *ctx, + lsm6dso_ois_pu_dis_t *val) +{ + lsm6dso_pin_ctrl_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_PIN_CTRL, (uint8_t *)®, 1); + + switch (reg.ois_pu_dis) + { + case LSM6DSO_AUX_PULL_UP_DISC: + *val = LSM6DSO_AUX_PULL_UP_DISC; + break; + + case LSM6DSO_AUX_PULL_UP_CONNECT: + *val = LSM6DSO_AUX_PULL_UP_CONNECT; + break; + + default: + *val = LSM6DSO_AUX_PULL_UP_DISC; + break; + } + + return ret; +} + +/** + * @brief OIS chain on aux interface power on mode.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of ois_on in reg CTRL7_G + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_aux_pw_on_ctrl_set(stmdev_ctx_t *ctx, + lsm6dso_ois_on_t val) +{ + lsm6dso_ctrl7_g_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL7_G, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.ois_on_en = (uint8_t)val & 0x01U; + reg.ois_on = (uint8_t)val & 0x01U; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL7_G, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief aux_pw_on_ctrl: [get] OIS chain on aux interface power on mode + * + * @param ctx read / write interface definitions + * @param val Get the values of ois_on in reg CTRL7_G + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_aux_pw_on_ctrl_get(stmdev_ctx_t *ctx, + lsm6dso_ois_on_t *val) +{ + lsm6dso_ctrl7_g_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL7_G, (uint8_t *)®, 1); + + switch (reg.ois_on) + { + case LSM6DSO_AUX_ON: + *val = LSM6DSO_AUX_ON; + break; + + case LSM6DSO_AUX_ON_BY_AUX_INTERFACE: + *val = LSM6DSO_AUX_ON_BY_AUX_INTERFACE; + break; + + default: + *val = LSM6DSO_AUX_ON; + break; + } + + return ret; +} + +/** + * @brief Accelerometer full-scale management between UI chain and + * OIS chain. When XL UI is on, the full scale is the same + * between UI/OIS and is chosen by the UI CTRL registers; + * when XL UI is in PD, the OIS can choose the FS. + * Full scales are independent between the UI/OIS chain + * but both bound to 8 g.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of xl_fs_mode in + * reg CTRL8_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_aux_xl_fs_mode_set(stmdev_ctx_t *ctx, + lsm6dso_xl_fs_mode_t val) +{ + lsm6dso_ctrl8_xl_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL8_XL, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.xl_fs_mode = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL8_XL, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Accelerometer full-scale management between UI chain and + * OIS chain. When XL UI is on, the full scale is the same + * between UI/OIS and is chosen by the UI CTRL registers; + * when XL UI is in PD, the OIS can choose the FS. + * Full scales are independent between the UI/OIS chain + * but both bound to 8 g.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of xl_fs_mode in reg CTRL8_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_aux_xl_fs_mode_get(stmdev_ctx_t *ctx, + lsm6dso_xl_fs_mode_t *val) +{ + lsm6dso_ctrl8_xl_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL8_XL, (uint8_t *)®, 1); + + switch (reg.xl_fs_mode) + { + case LSM6DSO_USE_SAME_XL_FS: + *val = LSM6DSO_USE_SAME_XL_FS; + break; + + case LSM6DSO_USE_DIFFERENT_XL_FS: + *val = LSM6DSO_USE_DIFFERENT_XL_FS; + break; + + default: + *val = LSM6DSO_USE_SAME_XL_FS; + break; + } + + return ret; +} + +/** + * @brief The STATUS_SPIAux register is read by the auxiliary SPI.[get] + * + * @param ctx read / write interface definitions + * @param lsm6dso_status_spiaux_t: registers STATUS_SPIAUX + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_aux_status_reg_get(stmdev_ctx_t *ctx, + lsm6dso_status_spiaux_t *val) +{ + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_STATUS_SPIAUX, (uint8_t *) val, 1); + + return ret; +} + +/** + * @brief aux_xl_flag_data_ready: [get] AUX accelerometer data available + * + * @param ctx read / write interface definitions + * @param val Get the values of xlda in reg STATUS_SPIAUX + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_aux_xl_flag_data_ready_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6dso_status_spiaux_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_STATUS_SPIAUX, (uint8_t *)®, 1); + *val = reg.xlda; + + return ret; +} + +/** + * @brief aux_gy_flag_data_ready: [get] AUX gyroscope data available. + * + * @param ctx read / write interface definitions + * @param val Get the values of gda in reg STATUS_SPIAUX + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_aux_gy_flag_data_ready_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6dso_status_spiaux_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_STATUS_SPIAUX, (uint8_t *)®, 1); + *val = reg.gda; + + return ret; +} + +/** + * @brief High when the gyroscope output is in the settling phase.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of gyro_settling in reg STATUS_SPIAUX + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_aux_gy_flag_settling_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6dso_status_spiaux_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_STATUS_SPIAUX, (uint8_t *)®, 1); + *val = reg.gyro_settling; + + return ret; +} + +/** + * @brief Selects accelerometer self-test. Effective only if XL OIS + * chain is enabled.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of st_xl_ois in reg INT_OIS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_aux_xl_self_test_set(stmdev_ctx_t *ctx, + lsm6dso_st_xl_ois_t val) +{ + lsm6dso_int_ois_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_INT_OIS, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.st_xl_ois = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_INT_OIS, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Selects accelerometer self-test. Effective only if XL OIS + * chain is enabled.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of st_xl_ois in reg INT_OIS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_aux_xl_self_test_get(stmdev_ctx_t *ctx, + lsm6dso_st_xl_ois_t *val) +{ + lsm6dso_int_ois_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_INT_OIS, (uint8_t *)®, 1); + + switch (reg.st_xl_ois) + { + case LSM6DSO_AUX_XL_DISABLE: + *val = LSM6DSO_AUX_XL_DISABLE; + break; + + case LSM6DSO_AUX_XL_POS: + *val = LSM6DSO_AUX_XL_POS; + break; + + case LSM6DSO_AUX_XL_NEG: + *val = LSM6DSO_AUX_XL_NEG; + break; + + default: + *val = LSM6DSO_AUX_XL_DISABLE; + break; + } + + return ret; +} + +/** + * @brief Indicates polarity of DEN signal on OIS chain.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of den_lh_ois in + * reg INT_OIS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_aux_den_polarity_set(stmdev_ctx_t *ctx, + lsm6dso_den_lh_ois_t val) +{ + lsm6dso_int_ois_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_INT_OIS, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.den_lh_ois = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_INT_OIS, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Indicates polarity of DEN signal on OIS chain.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of den_lh_ois in reg INT_OIS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_aux_den_polarity_get(stmdev_ctx_t *ctx, + lsm6dso_den_lh_ois_t *val) +{ + lsm6dso_int_ois_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_INT_OIS, (uint8_t *)®, 1); + + switch (reg.den_lh_ois) + { + case LSM6DSO_AUX_DEN_ACTIVE_LOW: + *val = LSM6DSO_AUX_DEN_ACTIVE_LOW; + break; + + case LSM6DSO_AUX_DEN_ACTIVE_HIGH: + *val = LSM6DSO_AUX_DEN_ACTIVE_HIGH; + break; + + default: + *val = LSM6DSO_AUX_DEN_ACTIVE_LOW; + break; + } + + return ret; +} + +/** + * @brief Configure DEN mode on the OIS chain.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of lvl2_ois in reg INT_OIS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_aux_den_mode_set(stmdev_ctx_t *ctx, + lsm6dso_lvl2_ois_t val) +{ + lsm6dso_ctrl1_ois_t ctrl1_ois; + lsm6dso_int_ois_t int_ois; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_INT_OIS, (uint8_t *) &int_ois, 1); + int_ois.lvl2_ois = (uint8_t)val & 0x01U; + ret += lsm6dso_write_reg(ctx, LSM6DSO_INT_OIS, (uint8_t *) &int_ois, 1); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL1_OIS, (uint8_t *) &ctrl1_ois, 1); + ctrl1_ois.lvl1_ois = ((uint8_t)val & 0x02U) >> 1; + ret += lsm6dso_write_reg(ctx, LSM6DSO_CTRL1_OIS, + (uint8_t *) &ctrl1_ois, 1); + + return ret; +} + +/** + * @brief Configure DEN mode on the OIS chain.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of lvl2_ois in reg INT_OIS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_aux_den_mode_get(stmdev_ctx_t *ctx, + lsm6dso_lvl2_ois_t *val) +{ + lsm6dso_ctrl1_ois_t ctrl1_ois; + lsm6dso_int_ois_t int_ois; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_INT_OIS, (uint8_t *) &int_ois, 1); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL1_OIS, (uint8_t *) &ctrl1_ois, 1); + if (ret != 0) { return ret; } + + switch ((ctrl1_ois.lvl1_ois << 1) + int_ois.lvl2_ois) + { + case LSM6DSO_AUX_DEN_DISABLE: + *val = LSM6DSO_AUX_DEN_DISABLE; + break; + + case LSM6DSO_AUX_DEN_LEVEL_LATCH: + *val = LSM6DSO_AUX_DEN_LEVEL_LATCH; + break; + + case LSM6DSO_AUX_DEN_LEVEL_TRIG: + *val = LSM6DSO_AUX_DEN_LEVEL_TRIG; + break; + + default: + *val = LSM6DSO_AUX_DEN_DISABLE; + break; + } + + return ret; +} + +/** + * @brief Enables/Disable OIS chain DRDY on INT2 pin. + * This setting has priority over all other INT2 settings.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of int2_drdy_ois in reg INT_OIS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_aux_drdy_on_int2_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dso_int_ois_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_INT_OIS, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.int2_drdy_ois = val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_INT_OIS, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Enables/Disable OIS chain DRDY on INT2 pin. + * This setting has priority over all other INT2 settings.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of int2_drdy_ois in reg INT_OIS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_aux_drdy_on_int2_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_int_ois_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_INT_OIS, (uint8_t *)®, 1); + *val = reg.int2_drdy_ois; + + return ret; +} + +/** + * @brief Enables OIS chain data processing for gyro in Mode 3 and Mode 4 + * (mode4_en = 1) and accelerometer data in and Mode 4 (mode4_en = 1). + * When the OIS chain is enabled, the OIS outputs are available + * through the SPI2 in registers OUTX_L_G (22h) through + * OUTZ_H_G (27h) and STATUS_REG (1Eh) / STATUS_SPIAux, and + * LPF1 is dedicated to this chain.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of ois_en_spi2 in + * reg CTRL1_OIS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_aux_mode_set(stmdev_ctx_t *ctx, + lsm6dso_ois_en_spi2_t val) +{ + lsm6dso_ctrl1_ois_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL1_OIS, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.ois_en_spi2 = (uint8_t)val & 0x01U; + reg.mode4_en = ((uint8_t)val & 0x02U) >> 1; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL1_OIS, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Enables OIS chain data processing for gyro in Mode 3 and Mode 4 + * (mode4_en = 1) and accelerometer data in and Mode 4 (mode4_en = 1). + * When the OIS chain is enabled, the OIS outputs are available + * through the SPI2 in registers OUTX_L_G (22h) through + * OUTZ_H_G (27h) and STATUS_REG (1Eh) / STATUS_SPIAux, and + * LPF1 is dedicated to this chain.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of ois_en_spi2 in + * reg CTRL1_OIS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_aux_mode_get(stmdev_ctx_t *ctx, + lsm6dso_ois_en_spi2_t *val) +{ + lsm6dso_ctrl1_ois_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL1_OIS, (uint8_t *)®, 1); + + switch ((reg.mode4_en << 1) | reg.ois_en_spi2) + { + case LSM6DSO_AUX_DISABLE: + *val = LSM6DSO_AUX_DISABLE; + break; + + case LSM6DSO_MODE_3_GY: + *val = LSM6DSO_MODE_3_GY; + break; + + case LSM6DSO_MODE_4_GY_XL: + *val = LSM6DSO_MODE_4_GY_XL; + break; + + default: + *val = LSM6DSO_AUX_DISABLE; + break; + } + + return ret; +} + +/** + * @brief Selects gyroscope OIS chain full-scale.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of fs_g_ois in reg CTRL1_OIS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_aux_gy_full_scale_set(stmdev_ctx_t *ctx, + lsm6dso_fs_g_ois_t val) +{ + lsm6dso_ctrl1_ois_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL1_OIS, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.fs_g_ois = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL1_OIS, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Selects gyroscope OIS chain full-scale.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of fs_g_ois in reg CTRL1_OIS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_aux_gy_full_scale_get(stmdev_ctx_t *ctx, + lsm6dso_fs_g_ois_t *val) +{ + lsm6dso_ctrl1_ois_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL1_OIS, (uint8_t *)®, 1); + + switch (reg.fs_g_ois) + { + case LSM6DSO_250dps_AUX: + *val = LSM6DSO_250dps_AUX; + break; + + case LSM6DSO_125dps_AUX: + *val = LSM6DSO_125dps_AUX; + break; + + case LSM6DSO_500dps_AUX: + *val = LSM6DSO_500dps_AUX; + break; + + case LSM6DSO_1000dps_AUX: + *val = LSM6DSO_1000dps_AUX; + break; + + case LSM6DSO_2000dps_AUX: + *val = LSM6DSO_2000dps_AUX; + break; + + default: + *val = LSM6DSO_250dps_AUX; + break; + } + + return ret; +} + +/** + * @brief SPI2 3- or 4-wire interface.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of sim_ois in reg CTRL1_OIS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_aux_spi_mode_set(stmdev_ctx_t *ctx, + lsm6dso_sim_ois_t val) +{ + lsm6dso_ctrl1_ois_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL1_OIS, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.sim_ois = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL1_OIS, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief SPI2 3- or 4-wire interface.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of sim_ois in reg CTRL1_OIS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_aux_spi_mode_get(stmdev_ctx_t *ctx, + lsm6dso_sim_ois_t *val) +{ + lsm6dso_ctrl1_ois_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL1_OIS, (uint8_t *)®, 1); + + switch (reg.sim_ois) + { + case LSM6DSO_AUX_SPI_4_WIRE: + *val = LSM6DSO_AUX_SPI_4_WIRE; + break; + + case LSM6DSO_AUX_SPI_3_WIRE: + *val = LSM6DSO_AUX_SPI_3_WIRE; + break; + + default: + *val = LSM6DSO_AUX_SPI_4_WIRE; + break; + } + + return ret; +} + +/** + * @brief Selects gyroscope digital LPF1 filter bandwidth.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of ftype_ois in + * reg CTRL2_OIS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_aux_gy_lp1_bandwidth_set(stmdev_ctx_t *ctx, + lsm6dso_ftype_ois_t val) +{ + lsm6dso_ctrl2_ois_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL2_OIS, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.ftype_ois = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL2_OIS, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Selects gyroscope digital LPF1 filter bandwidth.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of ftype_ois in reg CTRL2_OIS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_aux_gy_lp1_bandwidth_get(stmdev_ctx_t *ctx, + lsm6dso_ftype_ois_t *val) +{ + lsm6dso_ctrl2_ois_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL2_OIS, (uint8_t *)®, 1); + + switch (reg.ftype_ois) + { + case LSM6DSO_351Hz39: + *val = LSM6DSO_351Hz39; + break; + + case LSM6DSO_236Hz63: + *val = LSM6DSO_236Hz63; + break; + + case LSM6DSO_172Hz70: + *val = LSM6DSO_172Hz70; + break; + + case LSM6DSO_937Hz91: + *val = LSM6DSO_937Hz91; + break; + + default: + *val = LSM6DSO_351Hz39; + break; + } + + return ret; +} + +/** + * @brief Selects gyroscope OIS chain digital high-pass filter cutoff.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of hpm_ois in reg CTRL2_OIS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_aux_gy_hp_bandwidth_set(stmdev_ctx_t *ctx, + lsm6dso_hpm_ois_t val) +{ + lsm6dso_ctrl2_ois_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL2_OIS, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.hpm_ois = (uint8_t)val & 0x03U; + reg.hp_en_ois = ((uint8_t)val & 0x10U) >> 4; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL2_OIS, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Selects gyroscope OIS chain digital high-pass filter cutoff.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of hpm_ois in reg CTRL2_OIS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_aux_gy_hp_bandwidth_get(stmdev_ctx_t *ctx, + lsm6dso_hpm_ois_t *val) +{ + lsm6dso_ctrl2_ois_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL2_OIS, (uint8_t *)®, 1); + + switch ((reg.hp_en_ois << 4) | reg.hpm_ois) + { + case LSM6DSO_AUX_HP_DISABLE: + *val = LSM6DSO_AUX_HP_DISABLE; + break; + + case LSM6DSO_AUX_HP_Hz016: + *val = LSM6DSO_AUX_HP_Hz016; + break; + + case LSM6DSO_AUX_HP_Hz065: + *val = LSM6DSO_AUX_HP_Hz065; + break; + + case LSM6DSO_AUX_HP_Hz260: + *val = LSM6DSO_AUX_HP_Hz260; + break; + + case LSM6DSO_AUX_HP_1Hz040: + *val = LSM6DSO_AUX_HP_1Hz040; + break; + + default: + *val = LSM6DSO_AUX_HP_DISABLE; + break; + } + + return ret; +} + +/** + * @brief Enable / Disables OIS chain clamp. + * Enable: All OIS chain outputs = 8000h + * during self-test; Disable: OIS chain self-test + * outputs dependent from the aux gyro full + * scale selected.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of st_ois_clampdis in + * reg CTRL3_OIS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_aux_gy_clamp_set(stmdev_ctx_t *ctx, + lsm6dso_st_ois_clampdis_t val) +{ + lsm6dso_ctrl3_ois_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL3_OIS, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.st_ois_clampdis = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL3_OIS, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Enable / Disables OIS chain clamp. + * Enable: All OIS chain outputs = 8000h + * during self-test; Disable: OIS chain self-test + * outputs dependent from the aux gyro full + * scale selected.[set] + * + * @param ctx read / write interface definitions + * @param val Get the values of st_ois_clampdis in + * reg CTRL3_OIS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_aux_gy_clamp_get(stmdev_ctx_t *ctx, + lsm6dso_st_ois_clampdis_t *val) +{ + lsm6dso_ctrl3_ois_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL3_OIS, (uint8_t *)®, 1); + + switch (reg.st_ois_clampdis) + { + case LSM6DSO_ENABLE_CLAMP: + *val = LSM6DSO_ENABLE_CLAMP; + break; + + case LSM6DSO_DISABLE_CLAMP: + *val = LSM6DSO_DISABLE_CLAMP; + break; + + default: + *val = LSM6DSO_ENABLE_CLAMP; + break; + } + + return ret; +} + +/** + * @brief Selects gyroscope OIS chain self-test.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of st_ois in reg CTRL3_OIS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_aux_gy_self_test_set(stmdev_ctx_t *ctx, + lsm6dso_st_ois_t val) +{ + lsm6dso_ctrl3_ois_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL3_OIS, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.st_ois = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL3_OIS, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Selects gyroscope OIS chain self-test.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of st_ois in reg CTRL3_OIS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_aux_gy_self_test_get(stmdev_ctx_t *ctx, + lsm6dso_st_ois_t *val) +{ + lsm6dso_ctrl3_ois_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL3_OIS, (uint8_t *)®, 1); + + switch (reg.st_ois) + { + case LSM6DSO_AUX_GY_DISABLE: + *val = LSM6DSO_AUX_GY_DISABLE; + break; + + case LSM6DSO_AUX_GY_POS: + *val = LSM6DSO_AUX_GY_POS; + break; + + case LSM6DSO_AUX_GY_NEG: + *val = LSM6DSO_AUX_GY_NEG; + break; + + default: + *val = LSM6DSO_AUX_GY_DISABLE; + break; + } + + return ret; +} + +/** + * @brief Selects accelerometer OIS channel bandwidth.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of + * filter_xl_conf_ois in reg CTRL3_OIS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_aux_xl_bandwidth_set(stmdev_ctx_t *ctx, + lsm6dso_filter_xl_conf_ois_t val) +{ + lsm6dso_ctrl3_ois_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL3_OIS, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.filter_xl_conf_ois = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL3_OIS, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Selects accelerometer OIS channel bandwidth.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of + * filter_xl_conf_ois in reg CTRL3_OIS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_aux_xl_bandwidth_get(stmdev_ctx_t *ctx, + lsm6dso_filter_xl_conf_ois_t *val) +{ + lsm6dso_ctrl3_ois_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL3_OIS, (uint8_t *)®, 1); + + switch (reg.filter_xl_conf_ois) + { + case LSM6DSO_289Hz: + *val = LSM6DSO_289Hz; + break; + + case LSM6DSO_258Hz: + *val = LSM6DSO_258Hz; + break; + + case LSM6DSO_120Hz: + *val = LSM6DSO_120Hz; + break; + + case LSM6DSO_65Hz2: + *val = LSM6DSO_65Hz2; + break; + + case LSM6DSO_33Hz2: + *val = LSM6DSO_33Hz2; + break; + + case LSM6DSO_16Hz6: + *val = LSM6DSO_16Hz6; + break; + + case LSM6DSO_8Hz30: + *val = LSM6DSO_8Hz30; + break; + + case LSM6DSO_4Hz15: + *val = LSM6DSO_4Hz15; + break; + + default: + *val = LSM6DSO_289Hz; + break; + } + + return ret; +} + +/** + * @brief Selects accelerometer OIS channel full-scale.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of fs_xl_ois in + * reg CTRL3_OIS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_aux_xl_full_scale_set(stmdev_ctx_t *ctx, + lsm6dso_fs_xl_ois_t val) +{ + lsm6dso_ctrl3_ois_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL3_OIS, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.fs_xl_ois = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL3_OIS, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Selects accelerometer OIS channel full-scale.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of fs_xl_ois in reg CTRL3_OIS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_aux_xl_full_scale_get(stmdev_ctx_t *ctx, + lsm6dso_fs_xl_ois_t *val) +{ + lsm6dso_ctrl3_ois_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL3_OIS, (uint8_t *)®, 1); + + switch (reg.fs_xl_ois) + { + case LSM6DSO_AUX_2g: + *val = LSM6DSO_AUX_2g; + break; + + case LSM6DSO_AUX_16g: + *val = LSM6DSO_AUX_16g; + break; + + case LSM6DSO_AUX_4g: + *val = LSM6DSO_AUX_4g; + break; + + case LSM6DSO_AUX_8g: + *val = LSM6DSO_AUX_8g; + break; + + default: + *val = LSM6DSO_AUX_2g; + break; + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DSO_ main_serial_interface + * @brief This section groups all the functions concerning main + * serial interface management (not auxiliary) + * @{ + * + */ + +/** + * @brief Connect/Disconnect SDO/SA0 internal pull-up.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of sdo_pu_en in + * reg PIN_CTRL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_sdo_sa0_mode_set(stmdev_ctx_t *ctx, + lsm6dso_sdo_pu_en_t val) +{ + lsm6dso_pin_ctrl_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_PIN_CTRL, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.sdo_pu_en = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_PIN_CTRL, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Connect/Disconnect SDO/SA0 internal pull-up.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of sdo_pu_en in reg PIN_CTRL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_sdo_sa0_mode_get(stmdev_ctx_t *ctx, + lsm6dso_sdo_pu_en_t *val) +{ + lsm6dso_pin_ctrl_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_PIN_CTRL, (uint8_t *)®, 1); + + switch (reg.sdo_pu_en) + { + case LSM6DSO_PULL_UP_DISC: + *val = LSM6DSO_PULL_UP_DISC; + break; + + case LSM6DSO_PULL_UP_CONNECT: + *val = LSM6DSO_PULL_UP_CONNECT; + break; + + default: + *val = LSM6DSO_PULL_UP_DISC; + break; + } + + return ret; +} + +/** + * @brief SPI Serial Interface Mode selection.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of sim in reg CTRL3_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_spi_mode_set(stmdev_ctx_t *ctx, lsm6dso_sim_t val) +{ + lsm6dso_ctrl3_c_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL3_C, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.sim = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL3_C, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief SPI Serial Interface Mode selection.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of sim in reg CTRL3_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_spi_mode_get(stmdev_ctx_t *ctx, lsm6dso_sim_t *val) +{ + lsm6dso_ctrl3_c_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL3_C, (uint8_t *)®, 1); + + switch (reg.sim) + { + case LSM6DSO_SPI_4_WIRE: + *val = LSM6DSO_SPI_4_WIRE; + break; + + case LSM6DSO_SPI_3_WIRE: + *val = LSM6DSO_SPI_3_WIRE; + break; + + default: + *val = LSM6DSO_SPI_4_WIRE; + break; + } + + return ret; +} + +/** + * @brief Disable / Enable I2C interface.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of i2c_disable in + * reg CTRL4_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_i2c_interface_set(stmdev_ctx_t *ctx, + lsm6dso_i2c_disable_t val) +{ + lsm6dso_ctrl4_c_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL4_C, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.i2c_disable = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL4_C, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Disable / Enable I2C interface.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of i2c_disable in + * reg CTRL4_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_i2c_interface_get(stmdev_ctx_t *ctx, + lsm6dso_i2c_disable_t *val) +{ + lsm6dso_ctrl4_c_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL4_C, (uint8_t *)®, 1); + + switch (reg.i2c_disable) + { + case LSM6DSO_I2C_ENABLE: + *val = LSM6DSO_I2C_ENABLE; + break; + + case LSM6DSO_I2C_DISABLE: + *val = LSM6DSO_I2C_DISABLE; + break; + + default: + *val = LSM6DSO_I2C_ENABLE; + break; + } + + return ret; +} + +/** + * @brief I3C Enable/Disable communication protocol[.set] + * + * @param ctx read / write interface definitions + * @param val change the values of i3c_disable + * in reg CTRL9_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_i3c_disable_set(stmdev_ctx_t *ctx, + lsm6dso_i3c_disable_t val) +{ + lsm6dso_i3c_bus_avb_t i3c_bus_avb; + lsm6dso_ctrl9_xl_t ctrl9_xl; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL9_XL, (uint8_t *)&ctrl9_xl, 1); + ctrl9_xl.i3c_disable = ((uint8_t)val & 0x80U) >> 7; + ret += lsm6dso_write_reg(ctx, LSM6DSO_CTRL9_XL, (uint8_t *)&ctrl9_xl, 1); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_I3C_BUS_AVB, (uint8_t *)&i3c_bus_avb, 1); + i3c_bus_avb.i3c_bus_avb_sel = (uint8_t)val & 0x03U; + ret += lsm6dso_write_reg(ctx, LSM6DSO_I3C_BUS_AVB, (uint8_t *)&i3c_bus_avb, 1); + + return ret; +} + +/** + * @brief I3C Enable/Disable communication protocol.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of i3c_disable in + * reg CTRL9_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_i3c_disable_get(stmdev_ctx_t *ctx, + lsm6dso_i3c_disable_t *val) +{ + lsm6dso_ctrl9_xl_t ctrl9_xl; + lsm6dso_i3c_bus_avb_t i3c_bus_avb; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL9_XL, (uint8_t *)&ctrl9_xl, 1); + ret += lsm6dso_read_reg(ctx, LSM6DSO_I3C_BUS_AVB, (uint8_t *)&i3c_bus_avb, 1); + if (ret != 0) { return ret; } + + switch ((ctrl9_xl.i3c_disable << 7) | i3c_bus_avb.i3c_bus_avb_sel) + { + case LSM6DSO_I3C_DISABLE: + *val = LSM6DSO_I3C_DISABLE; + break; + + case LSM6DSO_I3C_ENABLE_T_50us: + *val = LSM6DSO_I3C_ENABLE_T_50us; + break; + + case LSM6DSO_I3C_ENABLE_T_2us: + *val = LSM6DSO_I3C_ENABLE_T_2us; + break; + + case LSM6DSO_I3C_ENABLE_T_1ms: + *val = LSM6DSO_I3C_ENABLE_T_1ms; + break; + + case LSM6DSO_I3C_ENABLE_T_25ms: + *val = LSM6DSO_I3C_ENABLE_T_25ms; + break; + + default: + *val = LSM6DSO_I3C_DISABLE; + break; + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DSO_interrupt_pins + * @brief This section groups all the functions that manage interrupt pins + * @{ + * + */ + +/** + * @brief Connect/Disconnect INT1 internal pull-down.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of pd_dis_int1 in reg I3C_BUS_AVB + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_int1_mode_set(stmdev_ctx_t *ctx, + lsm6dso_int1_pd_en_t val) +{ + lsm6dso_i3c_bus_avb_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_I3C_BUS_AVB, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.pd_dis_int1 = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_I3C_BUS_AVB, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Connect/Disconnect INT1 internal pull-down.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of pd_dis_int1 in reg I3C_BUS_AVB + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_int1_mode_get(stmdev_ctx_t *ctx, + lsm6dso_int1_pd_en_t *val) +{ + lsm6dso_i3c_bus_avb_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_I3C_BUS_AVB, (uint8_t *)®, 1); + + switch (reg.pd_dis_int1) + { + case LSM6DSO_PULL_DOWN_DISC: + *val = LSM6DSO_PULL_DOWN_DISC; + break; + + case LSM6DSO_PULL_DOWN_CONNECT: + *val = LSM6DSO_PULL_DOWN_CONNECT; + break; + + default: + *val = LSM6DSO_PULL_DOWN_DISC; + break; + } + + return ret; +} + +/** + * @brief Push-pull/open drain selection on interrupt pads.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of pp_od in reg CTRL3_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_pin_mode_set(stmdev_ctx_t *ctx, lsm6dso_pp_od_t val) +{ + lsm6dso_ctrl3_c_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL3_C, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.pp_od = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL3_C, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Push-pull/open drain selection on interrupt pads.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of pp_od in reg CTRL3_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_pin_mode_get(stmdev_ctx_t *ctx, lsm6dso_pp_od_t *val) +{ + lsm6dso_ctrl3_c_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL3_C, (uint8_t *)®, 1); + + switch (reg.pp_od) + { + case LSM6DSO_PUSH_PULL: + *val = LSM6DSO_PUSH_PULL; + break; + + case LSM6DSO_OPEN_DRAIN: + *val = LSM6DSO_OPEN_DRAIN; + break; + + default: + *val = LSM6DSO_PUSH_PULL; + break; + } + + return ret; +} + +/** + * @brief Interrupt active-high/low.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of h_lactive in reg CTRL3_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_pin_polarity_set(stmdev_ctx_t *ctx, + lsm6dso_h_lactive_t val) +{ + lsm6dso_ctrl3_c_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL3_C, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.h_lactive = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL3_C, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Interrupt active-high/low.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of h_lactive in reg CTRL3_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_pin_polarity_get(stmdev_ctx_t *ctx, + lsm6dso_h_lactive_t *val) +{ + lsm6dso_ctrl3_c_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL3_C, (uint8_t *)®, 1); + + switch (reg.h_lactive) + { + case LSM6DSO_ACTIVE_HIGH: + *val = LSM6DSO_ACTIVE_HIGH; + break; + + case LSM6DSO_ACTIVE_LOW: + *val = LSM6DSO_ACTIVE_LOW; + break; + + default: + *val = LSM6DSO_ACTIVE_HIGH; + break; + } + + return ret; +} + +/** + * @brief All interrupt signals become available on INT1 pin.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of int2_on_int1 in reg CTRL4_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_all_on_int1_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dso_ctrl4_c_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL4_C, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.int2_on_int1 = val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL4_C, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief All interrupt signals become available on INT1 pin.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of int2_on_int1 in reg CTRL4_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_all_on_int1_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_ctrl4_c_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL4_C, (uint8_t *)®, 1); + *val = reg.int2_on_int1; + + return ret; +} + +/** + * @brief Interrupt notification mode.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of lir in reg TAP_CFG0 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_int_notification_set(stmdev_ctx_t *ctx, + lsm6dso_lir_t val) +{ + lsm6dso_tap_cfg0_t tap_cfg0; + lsm6dso_page_rw_t page_rw; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_TAP_CFG0, (uint8_t *) &tap_cfg0, 1); + tap_cfg0.lir = (uint8_t)val & 0x01U; + tap_cfg0.int_clr_on_read = (uint8_t)val & 0x01U; + ret += lsm6dso_write_reg(ctx, LSM6DSO_TAP_CFG0, (uint8_t *) &tap_cfg0, 1); + if (ret != 0) { return ret; } + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_EMBEDDED_FUNC_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_PAGE_RW, (uint8_t *) &page_rw, 1); + page_rw.emb_func_lir = ((uint8_t)val & 0x02U) >> 1; + ret += lsm6dso_write_reg(ctx, LSM6DSO_PAGE_RW, (uint8_t *) &page_rw, 1); + + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @brief Interrupt notification mode.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of lir in reg TAP_CFG0 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_int_notification_get(stmdev_ctx_t *ctx, + lsm6dso_lir_t *val) +{ + lsm6dso_tap_cfg0_t tap_cfg0; + lsm6dso_page_rw_t page_rw; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_TAP_CFG0, (uint8_t *) &tap_cfg0, 1); + if (ret != 0) { return ret; } + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_EMBEDDED_FUNC_BANK); + ret += lsm6dso_read_reg(ctx, LSM6DSO_PAGE_RW, (uint8_t *) &page_rw, 1); + if (ret != 0) { goto exit; } + + switch ((page_rw.emb_func_lir << 1) | tap_cfg0.lir) + { + case LSM6DSO_ALL_INT_PULSED: + *val = LSM6DSO_ALL_INT_PULSED; + break; + + case LSM6DSO_BASE_LATCHED_EMB_PULSED: + *val = LSM6DSO_BASE_LATCHED_EMB_PULSED; + break; + + case LSM6DSO_BASE_PULSED_EMB_LATCHED: + *val = LSM6DSO_BASE_PULSED_EMB_LATCHED; + break; + + case LSM6DSO_ALL_INT_LATCHED: + *val = LSM6DSO_ALL_INT_LATCHED; + break; + + default: + *val = LSM6DSO_ALL_INT_PULSED; + break; + } + + ret += lsm6dso_read_reg(ctx, LSM6DSO_PAGE_RW, (uint8_t *) &page_rw, 1); + +exit: + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DSO_Wake_Up_event + * @brief This section groups all the functions that manage the Wake Up + * event generation. + * @{ + * + */ + +/** + * @brief Weight of 1 LSB of wakeup threshold.[set] + * 0: 1 LSB =FS_XL / 64 + * 1: 1 LSB = FS_XL / 256 + * + * @param ctx read / write interface definitions + * @param val change the values of wake_ths_w in + * reg WAKE_UP_DUR + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_wkup_ths_weight_set(stmdev_ctx_t *ctx, + lsm6dso_wake_ths_w_t val) +{ + lsm6dso_wake_up_dur_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_WAKE_UP_DUR, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.wake_ths_w = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_WAKE_UP_DUR, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Weight of 1 LSB of wakeup threshold.[get] + * 0: 1 LSB =FS_XL / 64 + * 1: 1 LSB = FS_XL / 256 + * + * @param ctx read / write interface definitions + * @param val Get the values of wake_ths_w in + * reg WAKE_UP_DUR + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_wkup_ths_weight_get(stmdev_ctx_t *ctx, + lsm6dso_wake_ths_w_t *val) +{ + lsm6dso_wake_up_dur_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_WAKE_UP_DUR, (uint8_t *)®, 1); + + switch (reg.wake_ths_w) + { + case LSM6DSO_LSb_FS_DIV_64: + *val = LSM6DSO_LSb_FS_DIV_64; + break; + + case LSM6DSO_LSb_FS_DIV_256: + *val = LSM6DSO_LSb_FS_DIV_256; + break; + + default: + *val = LSM6DSO_LSb_FS_DIV_64; + break; + } + + return ret; +} + +/** + * @brief Threshold for wakeup: 1 LSB weight depends on WAKE_THS_W in + * WAKE_UP_DUR.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of wk_ths in reg WAKE_UP_THS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_wkup_threshold_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dso_wake_up_ths_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_WAKE_UP_THS, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.wk_ths = val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_WAKE_UP_THS, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Threshold for wakeup: 1 LSB weight depends on WAKE_THS_W in + * WAKE_UP_DUR.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of wk_ths in reg WAKE_UP_THS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_wkup_threshold_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_wake_up_ths_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_WAKE_UP_THS, (uint8_t *)®, 1); + *val = reg.wk_ths; + + return ret; +} + +/** + * @brief Wake up duration event.[set] + * 1LSb = 1 / ODR + * + * @param ctx read / write interface definitions + * @param val change the values of usr_off_on_wu in reg WAKE_UP_THS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_xl_usr_offset_on_wkup_set(stmdev_ctx_t *ctx, + uint8_t val) +{ + lsm6dso_wake_up_ths_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_WAKE_UP_THS, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.usr_off_on_wu = val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_WAKE_UP_THS, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Wake up duration event.[get] + * 1LSb = 1 / ODR + * + * @param ctx read / write interface definitions + * @param val Get the values of usr_off_on_wu in reg WAKE_UP_THS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_xl_usr_offset_on_wkup_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6dso_wake_up_ths_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_WAKE_UP_THS, (uint8_t *)®, 1); + *val = reg.usr_off_on_wu; + + return ret; +} + +/** + * @brief Wake up duration event.[set] + * 1LSb = 1 / ODR + * + * @param ctx read / write interface definitions + * @param val change the values of wake_dur in reg WAKE_UP_DUR + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_wkup_dur_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dso_wake_up_dur_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_WAKE_UP_DUR, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.wake_dur = val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_WAKE_UP_DUR, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Wake up duration event.[get] + * 1LSb = 1 / ODR + * + * @param ctx read / write interface definitions + * @param val Get the values of wake_dur in reg WAKE_UP_DUR + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_wkup_dur_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_wake_up_dur_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_WAKE_UP_DUR, (uint8_t *)®, 1); + *val = reg.wake_dur; + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DSO_ Activity/Inactivity_detection + * @brief This section groups all the functions concerning + * activity/inactivity detection. + * @{ + * + */ + +/** + * @brief Enables gyroscope Sleep mode.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of sleep_g in reg CTRL4_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_gy_sleep_mode_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dso_ctrl4_c_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL4_C, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.sleep_g = val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL4_C, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Enables gyroscope Sleep mode.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of sleep_g in reg CTRL4_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_gy_sleep_mode_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_ctrl4_c_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL4_C, (uint8_t *)®, 1); + *val = reg.sleep_g; + + return ret; +} + +/** + * @brief Drives the sleep status instead of + * sleep change on INT pins + * (only if INT1_SLEEP_CHANGE or + * INT2_SLEEP_CHANGE bits are enabled).[set] + * + * @param ctx read / write interface definitions + * @param val change the values of sleep_status_on_int in reg TAP_CFG0 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_act_pin_notification_set(stmdev_ctx_t *ctx, + lsm6dso_sleep_status_on_int_t val) +{ + lsm6dso_tap_cfg0_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_TAP_CFG0, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.sleep_status_on_int = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_TAP_CFG0, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Drives the sleep status instead of + * sleep change on INT pins (only if + * INT1_SLEEP_CHANGE or + * INT2_SLEEP_CHANGE bits are enabled).[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of sleep_status_on_int in reg TAP_CFG0 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_act_pin_notification_get(stmdev_ctx_t *ctx, + lsm6dso_sleep_status_on_int_t *val) +{ + lsm6dso_tap_cfg0_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_TAP_CFG0, (uint8_t *)®, 1); + + switch (reg.sleep_status_on_int) + { + case LSM6DSO_DRIVE_SLEEP_CHG_EVENT: + *val = LSM6DSO_DRIVE_SLEEP_CHG_EVENT; + break; + + case LSM6DSO_DRIVE_SLEEP_STATUS: + *val = LSM6DSO_DRIVE_SLEEP_STATUS; + break; + + default: + *val = LSM6DSO_DRIVE_SLEEP_CHG_EVENT; + break; + } + + return ret; +} + +/** + * @brief Enable inactivity function.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of inact_en in reg TAP_CFG2 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_act_mode_set(stmdev_ctx_t *ctx, + lsm6dso_inact_en_t val) +{ + lsm6dso_tap_cfg2_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_TAP_CFG2, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.inact_en = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_TAP_CFG2, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Enable inactivity function.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of inact_en in reg TAP_CFG2 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_act_mode_get(stmdev_ctx_t *ctx, + lsm6dso_inact_en_t *val) +{ + lsm6dso_tap_cfg2_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_TAP_CFG2, (uint8_t *)®, 1); + + switch (reg.inact_en) + { + case LSM6DSO_XL_AND_GY_NOT_AFFECTED: + *val = LSM6DSO_XL_AND_GY_NOT_AFFECTED; + break; + + case LSM6DSO_XL_12Hz5_GY_NOT_AFFECTED: + *val = LSM6DSO_XL_12Hz5_GY_NOT_AFFECTED; + break; + + case LSM6DSO_XL_12Hz5_GY_SLEEP: + *val = LSM6DSO_XL_12Hz5_GY_SLEEP; + break; + + case LSM6DSO_XL_12Hz5_GY_PD: + *val = LSM6DSO_XL_12Hz5_GY_PD; + break; + + default: + *val = LSM6DSO_XL_AND_GY_NOT_AFFECTED; + break; + } + + return ret; +} + +/** + * @brief Duration to go in sleep mode.[set] + * 1 LSb = 512 / ODR + * + * @param ctx read / write interface definitions + * @param val change the values of sleep_dur in reg WAKE_UP_DUR + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_act_sleep_dur_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dso_wake_up_dur_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_WAKE_UP_DUR, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.sleep_dur = val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_WAKE_UP_DUR, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Duration to go in sleep mode.[get] + * 1 LSb = 512 / ODR + * + * @param ctx read / write interface definitions + * @param val Get the values of sleep_dur in reg WAKE_UP_DUR + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_act_sleep_dur_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_wake_up_dur_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_WAKE_UP_DUR, (uint8_t *)®, 1); + *val = reg.sleep_dur; + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DSO_tap_generator + * @brief This section groups all the functions that manage the + * tap and double tap event generation. + * @{ + * + */ + +/** + * @brief Enable Z direction in tap recognition.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of tap_z_en in reg TAP_CFG0 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_tap_detection_on_z_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dso_tap_cfg0_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_TAP_CFG0, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.tap_z_en = val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_TAP_CFG0, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Enable Z direction in tap recognition.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of tap_z_en in reg TAP_CFG0 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_tap_detection_on_z_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6dso_tap_cfg0_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_TAP_CFG0, (uint8_t *)®, 1); + *val = reg.tap_z_en; + + return ret; +} + +/** + * @brief Enable Y direction in tap recognition.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of tap_y_en in reg TAP_CFG0 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_tap_detection_on_y_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dso_tap_cfg0_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_TAP_CFG0, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.tap_y_en = val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_TAP_CFG0, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Enable Y direction in tap recognition.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of tap_y_en in reg TAP_CFG0 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_tap_detection_on_y_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6dso_tap_cfg0_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_TAP_CFG0, (uint8_t *)®, 1); + *val = reg.tap_y_en; + + return ret; +} + +/** + * @brief Enable X direction in tap recognition.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of tap_x_en in reg TAP_CFG0 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_tap_detection_on_x_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dso_tap_cfg0_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_TAP_CFG0, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.tap_x_en = val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_TAP_CFG0, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Enable X direction in tap recognition.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of tap_x_en in reg TAP_CFG0 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_tap_detection_on_x_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6dso_tap_cfg0_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_TAP_CFG0, (uint8_t *)®, 1); + *val = reg.tap_x_en; + + return ret; +} + +/** + * @brief X-axis tap recognition threshold.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of tap_ths_x in reg TAP_CFG1 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_tap_threshold_x_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dso_tap_cfg1_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_TAP_CFG1, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.tap_ths_x = val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_TAP_CFG1, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief X-axis tap recognition threshold.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of tap_ths_x in reg TAP_CFG1 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_tap_threshold_x_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_tap_cfg1_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_TAP_CFG1, (uint8_t *)®, 1); + *val = reg.tap_ths_x; + + return ret; +} + +/** + * @brief Selection of axis priority for TAP detection.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of tap_priority in + * reg TAP_CFG1 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_tap_axis_priority_set(stmdev_ctx_t *ctx, + lsm6dso_tap_priority_t val) +{ + lsm6dso_tap_cfg1_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_TAP_CFG1, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.tap_priority = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_TAP_CFG1, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Selection of axis priority for TAP detection.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of tap_priority in + * reg TAP_CFG1 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_tap_axis_priority_get(stmdev_ctx_t *ctx, + lsm6dso_tap_priority_t *val) +{ + lsm6dso_tap_cfg1_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_TAP_CFG1, (uint8_t *)®, 1); + + switch (reg.tap_priority) + { + case LSM6DSO_XYZ: + *val = LSM6DSO_XYZ; + break; + + case LSM6DSO_YXZ: + *val = LSM6DSO_YXZ; + break; + + case LSM6DSO_XZY: + *val = LSM6DSO_XZY; + break; + + case LSM6DSO_ZYX: + *val = LSM6DSO_ZYX; + break; + + case LSM6DSO_YZX: + *val = LSM6DSO_YZX; + break; + + case LSM6DSO_ZXY: + *val = LSM6DSO_ZXY; + break; + + default: + *val = LSM6DSO_XYZ; + break; + } + + return ret; +} + +/** + * @brief Y-axis tap recognition threshold.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of tap_ths_y in reg TAP_CFG2 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_tap_threshold_y_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dso_tap_cfg2_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_TAP_CFG2, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.tap_ths_y = val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_TAP_CFG2, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Y-axis tap recognition threshold.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of tap_ths_y in reg TAP_CFG2 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_tap_threshold_y_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_tap_cfg2_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_TAP_CFG2, (uint8_t *)®, 1); + *val = reg.tap_ths_y; + + return ret; +} + +/** + * @brief Z-axis recognition threshold.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of tap_ths_z in reg TAP_THS_6D + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_tap_threshold_z_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dso_tap_ths_6d_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_TAP_THS_6D, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.tap_ths_z = val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_TAP_THS_6D, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Z-axis recognition threshold.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of tap_ths_z in reg TAP_THS_6D + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_tap_threshold_z_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_tap_ths_6d_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_TAP_THS_6D, (uint8_t *)®, 1); + *val = reg.tap_ths_z; + + return ret; +} + +/** + * @brief Maximum duration is the maximum time of an + * over threshold 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[1:0] bits are set to a different + * value, 1LSB corresponds to 8*ODR_XL time.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of shock in reg INT_DUR2 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_tap_shock_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dso_int_dur2_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_INT_DUR2, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.shock = val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_INT_DUR2, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Maximum duration is the maximum time of an + * over threshold 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[1:0] bits are set to a different + * value, 1LSB corresponds to 8*ODR_XL time.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of shock in reg INT_DUR2 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_tap_shock_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_int_dur2_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_INT_DUR2, (uint8_t *)®, 1); + *val = reg.shock; + + return ret; +} + +/** + * @brief Quiet time is the time after the first detected + * tap in which there must not be any over threshold + * event. + * The default value of these bits is 00b which + * corresponds to 2*ODR_XL time. If the QUIET[1:0] + * bits are set to a different value, + * 1LSB corresponds to 4*ODR_XL time.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of quiet in reg INT_DUR2 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_tap_quiet_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dso_int_dur2_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_INT_DUR2, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.quiet = val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_INT_DUR2, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Quiet time is the time after the first detected + * tap in which there must not be any over threshold + * event. + * The default value of these bits is 00b which + * corresponds to 2*ODR_XL time. + * If the QUIET[1:0] bits are set to a different + * value, 1LSB corresponds to 4*ODR_XL time.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of quiet in reg INT_DUR2 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_tap_quiet_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_int_dur2_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_INT_DUR2, (uint8_t *)®, 1); + *val = reg.quiet; + + return ret; +} + +/** + * @brief 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 change the values of dur in reg INT_DUR2 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_tap_dur_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dso_int_dur2_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_INT_DUR2, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.dur = val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_INT_DUR2, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief 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 Get the values of dur in reg INT_DUR2 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_tap_dur_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_int_dur2_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_INT_DUR2, (uint8_t *)®, 1); + *val = reg.dur; + + return ret; +} + +/** + * @brief Single/double-tap event enable.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of single_double_tap in reg WAKE_UP_THS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_tap_mode_set(stmdev_ctx_t *ctx, + lsm6dso_single_double_tap_t val) +{ + lsm6dso_wake_up_ths_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_WAKE_UP_THS, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.single_double_tap = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_WAKE_UP_THS, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Single/double-tap event enable.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of single_double_tap in reg WAKE_UP_THS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_tap_mode_get(stmdev_ctx_t *ctx, + lsm6dso_single_double_tap_t *val) +{ + lsm6dso_wake_up_ths_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_WAKE_UP_THS, (uint8_t *)®, 1); + + switch (reg.single_double_tap) + { + case LSM6DSO_ONLY_SINGLE: + *val = LSM6DSO_ONLY_SINGLE; + break; + + case LSM6DSO_BOTH_SINGLE_DOUBLE: + *val = LSM6DSO_BOTH_SINGLE_DOUBLE; + break; + + default: + *val = LSM6DSO_ONLY_SINGLE; + break; + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DSO_ Six_position_detection(6D/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 change the values of sixd_ths in reg TAP_THS_6D + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_6d_threshold_set(stmdev_ctx_t *ctx, + lsm6dso_sixd_ths_t val) +{ + lsm6dso_tap_ths_6d_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_TAP_THS_6D, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.sixd_ths = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_TAP_THS_6D, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Threshold for 4D/6D function.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of sixd_ths in reg TAP_THS_6D + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_6d_threshold_get(stmdev_ctx_t *ctx, + lsm6dso_sixd_ths_t *val) +{ + lsm6dso_tap_ths_6d_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_TAP_THS_6D, (uint8_t *)®, 1); + + switch (reg.sixd_ths) + { + case LSM6DSO_DEG_80: + *val = LSM6DSO_DEG_80; + break; + + case LSM6DSO_DEG_70: + *val = LSM6DSO_DEG_70; + break; + + case LSM6DSO_DEG_60: + *val = LSM6DSO_DEG_60; + break; + + case LSM6DSO_DEG_50: + *val = LSM6DSO_DEG_50; + break; + + default: + *val = LSM6DSO_DEG_80; + break; + } + + return ret; +} + +/** + * @brief 4D orientation detection enable.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of d4d_en in reg TAP_THS_6D + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_4d_mode_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dso_tap_ths_6d_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_TAP_THS_6D, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.d4d_en = val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_TAP_THS_6D, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief 4D orientation detection enable.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of d4d_en in reg TAP_THS_6D + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_4d_mode_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_tap_ths_6d_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_TAP_THS_6D, (uint8_t *)®, 1); + *val = reg.d4d_en; + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DSO_free_fall + * @brief This section group all the functions concerning the free + * fall detection. + * @{ + * + */ +/** + * @brief Free fall threshold setting.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of ff_ths in reg FREE_FALL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_ff_threshold_set(stmdev_ctx_t *ctx, + lsm6dso_ff_ths_t val) +{ + lsm6dso_free_fall_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_FREE_FALL, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.ff_ths = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_FREE_FALL, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Free fall threshold setting.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of ff_ths in reg FREE_FALL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_ff_threshold_get(stmdev_ctx_t *ctx, + lsm6dso_ff_ths_t *val) +{ + lsm6dso_free_fall_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_FREE_FALL, (uint8_t *)®, 1); + + switch (reg.ff_ths) + { + case LSM6DSO_FF_TSH_156mg: + *val = LSM6DSO_FF_TSH_156mg; + break; + + case LSM6DSO_FF_TSH_219mg: + *val = LSM6DSO_FF_TSH_219mg; + break; + + case LSM6DSO_FF_TSH_250mg: + *val = LSM6DSO_FF_TSH_250mg; + break; + + case LSM6DSO_FF_TSH_312mg: + *val = LSM6DSO_FF_TSH_312mg; + break; + + case LSM6DSO_FF_TSH_344mg: + *val = LSM6DSO_FF_TSH_344mg; + break; + + case LSM6DSO_FF_TSH_406mg: + *val = LSM6DSO_FF_TSH_406mg; + break; + + case LSM6DSO_FF_TSH_469mg: + *val = LSM6DSO_FF_TSH_469mg; + break; + + case LSM6DSO_FF_TSH_500mg: + *val = LSM6DSO_FF_TSH_500mg; + break; + + default: + *val = LSM6DSO_FF_TSH_156mg; + break; + } + + return ret; +} + +/** + * @brief Free-fall duration event.[set] + * 1LSb = 1 / ODR + * + * @param ctx read / write interface definitions + * @param val change the values of ff_dur in reg FREE_FALL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_ff_dur_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dso_wake_up_dur_t wake_up_dur; + lsm6dso_free_fall_t free_fall; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_WAKE_UP_DUR, (uint8_t *)&wake_up_dur, 1); + ret += lsm6dso_read_reg(ctx, LSM6DSO_FREE_FALL, (uint8_t *)&free_fall, 1); + if (ret != 0) { return ret; } + + wake_up_dur.ff_dur = ((uint8_t)val & 0x20U) >> 5; + free_fall.ff_dur = (uint8_t)val & 0x1FU; + + ret = lsm6dso_write_reg(ctx, LSM6DSO_WAKE_UP_DUR, (uint8_t *)&wake_up_dur, 1); + ret += lsm6dso_write_reg(ctx, LSM6DSO_FREE_FALL, (uint8_t *)&free_fall, 1); + + return ret; +} + +/** + * @brief Free-fall duration event.[get] + * 1LSb = 1 / ODR + * + * @param ctx read / write interface definitions + * @param val Get the values of ff_dur in reg FREE_FALL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_ff_dur_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_wake_up_dur_t wake_up_dur; + lsm6dso_free_fall_t free_fall; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_WAKE_UP_DUR, + (uint8_t *)&wake_up_dur, 1); + + if (ret == 0) + { + ret = lsm6dso_read_reg(ctx, LSM6DSO_FREE_FALL, (uint8_t *)&free_fall, 1); + *val = (wake_up_dur.ff_dur << 5) + free_fall.ff_dur; + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DSO_fifo + * @brief This section group all the functions concerning the fifo usage + * @{ + * + */ + +/** + * @brief FIFO watermark level selection.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of wtm in reg FIFO_CTRL1 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_fifo_watermark_set(stmdev_ctx_t *ctx, uint16_t val) +{ + lsm6dso_fifo_ctrl1_t fifo_ctrl1; + lsm6dso_fifo_ctrl2_t fifo_ctrl2; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); + if (ret != 0) { return ret; } + + fifo_ctrl1.wtm = 0x00FFU & (uint8_t)val; + fifo_ctrl2.wtm = (uint8_t)((0x0100U & val) >> 8); + + ret = lsm6dso_write_reg(ctx, LSM6DSO_FIFO_CTRL1, (uint8_t *)&fifo_ctrl1, 1); + ret += lsm6dso_write_reg(ctx, LSM6DSO_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); + + return ret; +} + +/** + * @brief FIFO watermark level selection.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of wtm in reg FIFO_CTRL1 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_fifo_watermark_get(stmdev_ctx_t *ctx, uint16_t *val) +{ + lsm6dso_fifo_ctrl1_t fifo_ctrl1; + lsm6dso_fifo_ctrl2_t fifo_ctrl2; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_FIFO_CTRL1, (uint8_t *)&fifo_ctrl1, 1); + + if (ret == 0) + { + ret = lsm6dso_read_reg(ctx, LSM6DSO_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); + *val = ((uint16_t)fifo_ctrl2.wtm << 8) + (uint16_t)fifo_ctrl1.wtm; + } + + return ret; +} + +/** + * @brief FIFO compression feature initialization request [set]. + * + * @param ctx read / write interface definitions + * @param val change the values of FIFO_COMPR_INIT in + * reg EMB_FUNC_INIT_B + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_compression_algo_init_set(stmdev_ctx_t *ctx, + uint8_t val) +{ + lsm6dso_emb_func_init_b_t reg; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_EMBEDDED_FUNC_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_EMB_FUNC_INIT_B, (uint8_t *)®, 1); + reg.fifo_compr_init = val; + ret += lsm6dso_write_reg(ctx, LSM6DSO_EMB_FUNC_INIT_B, (uint8_t *)®, 1); + + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @brief FIFO compression feature initialization request [get]. + * + * @param ctx read / write interface definitions + * @param val Get the values of FIFO_COMPR_INIT in + * reg EMB_FUNC_INIT_B + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_compression_algo_init_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6dso_emb_func_init_b_t reg; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_EMBEDDED_FUNC_BANK); + ret += lsm6dso_read_reg(ctx, LSM6DSO_EMB_FUNC_INIT_B, (uint8_t *)®, 1); + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + *val = reg.fifo_compr_init; + + return ret; +} + +/** + * @brief Enable and configure compression algo.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of uncoptr_rate in + * reg FIFO_CTRL2 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_compression_algo_set(stmdev_ctx_t *ctx, + lsm6dso_uncoptr_rate_t val) +{ + lsm6dso_fifo_ctrl2_t fifo_ctrl2; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_FIFO_CTRL2, + (uint8_t *)&fifo_ctrl2, 1); + + if (ret == 0) + { + fifo_ctrl2.fifo_compr_rt_en = ((uint8_t)val & 0x04U) >> 2; + fifo_ctrl2.uncoptr_rate = (uint8_t)val & 0x03U; + ret = lsm6dso_write_reg(ctx, LSM6DSO_FIFO_CTRL2, + (uint8_t *)&fifo_ctrl2, 1); + } + + return ret; +} + +/** + * @brief Enable and configure compression algo.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of uncoptr_rate in + * reg FIFO_CTRL2 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_compression_algo_get(stmdev_ctx_t *ctx, + lsm6dso_uncoptr_rate_t *val) +{ + lsm6dso_fifo_ctrl2_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_FIFO_CTRL2, (uint8_t *)®, 1); + + switch ((reg.fifo_compr_rt_en << 2) | reg.uncoptr_rate) + { + case LSM6DSO_CMP_DISABLE: + *val = LSM6DSO_CMP_DISABLE; + break; + + case LSM6DSO_CMP_ALWAYS: + *val = LSM6DSO_CMP_ALWAYS; + break; + + case LSM6DSO_CMP_8_TO_1: + *val = LSM6DSO_CMP_8_TO_1; + break; + + case LSM6DSO_CMP_16_TO_1: + *val = LSM6DSO_CMP_16_TO_1; + break; + + case LSM6DSO_CMP_32_TO_1: + *val = LSM6DSO_CMP_32_TO_1; + break; + + default: + *val = LSM6DSO_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 change the values of odrchg_en in reg FIFO_CTRL2 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_fifo_virtual_sens_odr_chg_set(stmdev_ctx_t *ctx, + uint8_t val) +{ + lsm6dso_fifo_ctrl2_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_FIFO_CTRL2, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.odrchg_en = val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_FIFO_CTRL2, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Enables ODR CHANGE virtual sensor to be batched in FIFO.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of odrchg_en in reg FIFO_CTRL2 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_fifo_virtual_sens_odr_chg_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6dso_fifo_ctrl2_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_FIFO_CTRL2, (uint8_t *)®, 1); + *val = reg.odrchg_en; + + return ret; +} + +/** + * @brief Enables/Disables compression algorithm runtime.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of fifo_compr_rt_en in + * reg FIFO_CTRL2 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_compression_algo_real_time_set(stmdev_ctx_t *ctx, + uint8_t val) +{ + lsm6dso_fifo_ctrl2_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_FIFO_CTRL2, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.fifo_compr_rt_en = val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_FIFO_CTRL2, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Enables/Disables compression algorithm runtime. [get] + * + * @param ctx read / write interface definitions + * @param val Get the values of fifo_compr_rt_en in reg FIFO_CTRL2 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_compression_algo_real_time_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6dso_fifo_ctrl2_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_FIFO_CTRL2, (uint8_t *)®, 1); + *val = reg.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 change the values of stop_on_wtm in reg FIFO_CTRL2 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_fifo_stop_on_wtm_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dso_fifo_ctrl2_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_FIFO_CTRL2, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.stop_on_wtm = val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_FIFO_CTRL2, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Sensing chain FIFO stop values memorization at + * threshold level.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of stop_on_wtm in reg FIFO_CTRL2 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_fifo_stop_on_wtm_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_fifo_ctrl2_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_FIFO_CTRL2, (uint8_t *)®, 1); + *val = reg.stop_on_wtm; + + return ret; +} + +/** + * @brief Selects Batching Data Rate (writing frequency in FIFO) + * for accelerometer data.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of bdr_xl in reg FIFO_CTRL3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_fifo_xl_batch_set(stmdev_ctx_t *ctx, + lsm6dso_bdr_xl_t val) +{ + lsm6dso_fifo_ctrl3_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_FIFO_CTRL3, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.bdr_xl = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_FIFO_CTRL3, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Selects Batching Data Rate (writing frequency in FIFO) + * for accelerometer data.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of bdr_xl in reg FIFO_CTRL3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_fifo_xl_batch_get(stmdev_ctx_t *ctx, + lsm6dso_bdr_xl_t *val) +{ + lsm6dso_fifo_ctrl3_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_FIFO_CTRL3, (uint8_t *)®, 1); + + switch (reg.bdr_xl) + { + case LSM6DSO_XL_NOT_BATCHED: + *val = LSM6DSO_XL_NOT_BATCHED; + break; + + case LSM6DSO_XL_BATCHED_AT_12Hz5: + *val = LSM6DSO_XL_BATCHED_AT_12Hz5; + break; + + case LSM6DSO_XL_BATCHED_AT_26Hz: + *val = LSM6DSO_XL_BATCHED_AT_26Hz; + break; + + case LSM6DSO_XL_BATCHED_AT_52Hz: + *val = LSM6DSO_XL_BATCHED_AT_52Hz; + break; + + case LSM6DSO_XL_BATCHED_AT_104Hz: + *val = LSM6DSO_XL_BATCHED_AT_104Hz; + break; + + case LSM6DSO_XL_BATCHED_AT_208Hz: + *val = LSM6DSO_XL_BATCHED_AT_208Hz; + break; + + case LSM6DSO_XL_BATCHED_AT_417Hz: + *val = LSM6DSO_XL_BATCHED_AT_417Hz; + break; + + case LSM6DSO_XL_BATCHED_AT_833Hz: + *val = LSM6DSO_XL_BATCHED_AT_833Hz; + break; + + case LSM6DSO_XL_BATCHED_AT_1667Hz: + *val = LSM6DSO_XL_BATCHED_AT_1667Hz; + break; + + case LSM6DSO_XL_BATCHED_AT_3333Hz: + *val = LSM6DSO_XL_BATCHED_AT_3333Hz; + break; + + case LSM6DSO_XL_BATCHED_AT_6667Hz: + *val = LSM6DSO_XL_BATCHED_AT_6667Hz; + break; + + case LSM6DSO_XL_BATCHED_AT_6Hz5: + *val = LSM6DSO_XL_BATCHED_AT_6Hz5; + break; + + default: + *val = LSM6DSO_XL_NOT_BATCHED; + break; + } + + return ret; +} + +/** + * @brief Selects Batching Data Rate (writing frequency in FIFO) + * for gyroscope data.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of bdr_gy in reg FIFO_CTRL3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_fifo_gy_batch_set(stmdev_ctx_t *ctx, + lsm6dso_bdr_gy_t val) +{ + lsm6dso_fifo_ctrl3_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_FIFO_CTRL3, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.bdr_gy = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_FIFO_CTRL3, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Selects Batching Data Rate (writing frequency in FIFO) + * for gyroscope data.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of bdr_gy in reg FIFO_CTRL3 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_fifo_gy_batch_get(stmdev_ctx_t *ctx, + lsm6dso_bdr_gy_t *val) +{ + lsm6dso_fifo_ctrl3_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_FIFO_CTRL3, (uint8_t *)®, 1); + + switch (reg.bdr_gy) + { + case LSM6DSO_GY_NOT_BATCHED: + *val = LSM6DSO_GY_NOT_BATCHED; + break; + + case LSM6DSO_GY_BATCHED_AT_12Hz5: + *val = LSM6DSO_GY_BATCHED_AT_12Hz5; + break; + + case LSM6DSO_GY_BATCHED_AT_26Hz: + *val = LSM6DSO_GY_BATCHED_AT_26Hz; + break; + + case LSM6DSO_GY_BATCHED_AT_52Hz: + *val = LSM6DSO_GY_BATCHED_AT_52Hz; + break; + + case LSM6DSO_GY_BATCHED_AT_104Hz: + *val = LSM6DSO_GY_BATCHED_AT_104Hz; + break; + + case LSM6DSO_GY_BATCHED_AT_208Hz: + *val = LSM6DSO_GY_BATCHED_AT_208Hz; + break; + + case LSM6DSO_GY_BATCHED_AT_417Hz: + *val = LSM6DSO_GY_BATCHED_AT_417Hz; + break; + + case LSM6DSO_GY_BATCHED_AT_833Hz: + *val = LSM6DSO_GY_BATCHED_AT_833Hz; + break; + + case LSM6DSO_GY_BATCHED_AT_1667Hz: + *val = LSM6DSO_GY_BATCHED_AT_1667Hz; + break; + + case LSM6DSO_GY_BATCHED_AT_3333Hz: + *val = LSM6DSO_GY_BATCHED_AT_3333Hz; + break; + + case LSM6DSO_GY_BATCHED_AT_6667Hz: + *val = LSM6DSO_GY_BATCHED_AT_6667Hz; + break; + + case LSM6DSO_GY_BATCHED_AT_6Hz5: + *val = LSM6DSO_GY_BATCHED_AT_6Hz5; + break; + + default: + *val = LSM6DSO_GY_NOT_BATCHED; + break; + } + + return ret; +} + +/** + * @brief FIFO mode selection.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of fifo_mode in reg FIFO_CTRL4 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_fifo_mode_set(stmdev_ctx_t *ctx, + lsm6dso_fifo_mode_t val) +{ + lsm6dso_fifo_ctrl4_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_FIFO_CTRL4, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.fifo_mode = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_FIFO_CTRL4, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief FIFO mode selection.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of fifo_mode in reg FIFO_CTRL4 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_fifo_mode_get(stmdev_ctx_t *ctx, + lsm6dso_fifo_mode_t *val) +{ + lsm6dso_fifo_ctrl4_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_FIFO_CTRL4, (uint8_t *)®, 1); + + switch (reg.fifo_mode) + { + case LSM6DSO_BYPASS_MODE: + *val = LSM6DSO_BYPASS_MODE; + break; + + case LSM6DSO_FIFO_MODE: + *val = LSM6DSO_FIFO_MODE; + break; + + case LSM6DSO_STREAM_TO_FIFO_MODE: + *val = LSM6DSO_STREAM_TO_FIFO_MODE; + break; + + case LSM6DSO_BYPASS_TO_STREAM_MODE: + *val = LSM6DSO_BYPASS_TO_STREAM_MODE; + break; + + case LSM6DSO_STREAM_MODE: + *val = LSM6DSO_STREAM_MODE; + break; + + case LSM6DSO_BYPASS_TO_FIFO_MODE: + *val = LSM6DSO_BYPASS_TO_FIFO_MODE; + break; + + default: + *val = LSM6DSO_BYPASS_MODE; + break; + } + + return ret; +} + +/** + * @brief Selects Batching Data Rate (writing frequency in FIFO) + * for temperature data.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of odr_t_batch in reg FIFO_CTRL4 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_fifo_temp_batch_set(stmdev_ctx_t *ctx, + lsm6dso_odr_t_batch_t val) +{ + lsm6dso_fifo_ctrl4_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_FIFO_CTRL4, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.odr_t_batch = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_FIFO_CTRL4, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Selects Batching Data Rate (writing frequency in FIFO) + * for temperature data.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of odr_t_batch in reg FIFO_CTRL4 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_fifo_temp_batch_get(stmdev_ctx_t *ctx, + lsm6dso_odr_t_batch_t *val) +{ + lsm6dso_fifo_ctrl4_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_FIFO_CTRL4, (uint8_t *)®, 1); + + switch (reg.odr_t_batch) + { + case LSM6DSO_TEMP_NOT_BATCHED: + *val = LSM6DSO_TEMP_NOT_BATCHED; + break; + + case LSM6DSO_TEMP_BATCHED_AT_1Hz6: + *val = LSM6DSO_TEMP_BATCHED_AT_1Hz6; + break; + + case LSM6DSO_TEMP_BATCHED_AT_12Hz5: + *val = LSM6DSO_TEMP_BATCHED_AT_12Hz5; + break; + + case LSM6DSO_TEMP_BATCHED_AT_52Hz: + *val = LSM6DSO_TEMP_BATCHED_AT_52Hz; + break; + + default: + *val = LSM6DSO_TEMP_NOT_BATCHED; + break; + } + + return ret; +} + +/** + * @brief Selects decimation for timestamp batching in FIFO. + * Writing rate will be the maximum rate between XL and + * GYRO BDR divided by decimation decoder.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of odr_ts_batch in reg FIFO_CTRL4 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_fifo_timestamp_decimation_set(stmdev_ctx_t *ctx, + lsm6dso_odr_ts_batch_t val) +{ + lsm6dso_fifo_ctrl4_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_FIFO_CTRL4, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.odr_ts_batch = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_FIFO_CTRL4, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Selects decimation for timestamp batching in FIFO. + * Writing rate will be the maximum rate between XL and + * GYRO BDR divided by decimation decoder.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of odr_ts_batch in reg FIFO_CTRL4 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_fifo_timestamp_decimation_get(stmdev_ctx_t *ctx, + lsm6dso_odr_ts_batch_t *val) +{ + lsm6dso_fifo_ctrl4_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_FIFO_CTRL4, (uint8_t *)®, 1); + + switch (reg.odr_ts_batch) + { + case LSM6DSO_NO_DECIMATION: + *val = LSM6DSO_NO_DECIMATION; + break; + + case LSM6DSO_DEC_1: + *val = LSM6DSO_DEC_1; + break; + + case LSM6DSO_DEC_8: + *val = LSM6DSO_DEC_8; + break; + + case LSM6DSO_DEC_32: + *val = LSM6DSO_DEC_32; + break; + + default: + *val = LSM6DSO_NO_DECIMATION; + break; + } + + return ret; +} + +/** + * @brief Selects the trigger for the internal counter of batching events + * between XL and gyro.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of trig_counter_bdr + * in reg COUNTER_BDR_REG1 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_fifo_cnt_event_batch_set(stmdev_ctx_t *ctx, + lsm6dso_trig_counter_bdr_t val) +{ + lsm6dso_counter_bdr_reg1_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_COUNTER_BDR_REG1, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.trig_counter_bdr = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_COUNTER_BDR_REG1, + (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Selects the trigger for the internal counter of batching events + * between XL and gyro.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of trig_counter_bdr + * in reg COUNTER_BDR_REG1 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_fifo_cnt_event_batch_get(stmdev_ctx_t *ctx, + lsm6dso_trig_counter_bdr_t *val) +{ + lsm6dso_counter_bdr_reg1_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_COUNTER_BDR_REG1, (uint8_t *)®, 1); + + switch (reg.trig_counter_bdr) + { + case LSM6DSO_XL_BATCH_EVENT: + *val = LSM6DSO_XL_BATCH_EVENT; + break; + + case LSM6DSO_GYRO_BATCH_EVENT: + *val = LSM6DSO_GYRO_BATCH_EVENT; + break; + + default: + *val = LSM6DSO_XL_BATCH_EVENT; + break; + } + + return ret; +} + +/** + * @brief Resets the internal counter of batching vents for a single sensor. + * This bit is automatically reset to zero if it was set to '1'.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of rst_counter_bdr in + * reg COUNTER_BDR_REG1 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_rst_batch_counter_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dso_counter_bdr_reg1_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_COUNTER_BDR_REG1, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.rst_counter_bdr = val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_COUNTER_BDR_REG1, + (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief Resets the internal counter of batching events for a single sensor. + * This bit is automatically reset to zero if it was set to '1'.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of rst_counter_bdr in + * reg COUNTER_BDR_REG1 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_rst_batch_counter_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_counter_bdr_reg1_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_COUNTER_BDR_REG1, (uint8_t *)®, 1); + *val = reg.rst_counter_bdr; + + return ret; +} + +/** + * @brief Batch data rate counter.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of cnt_bdr_th in + * reg COUNTER_BDR_REG2 and COUNTER_BDR_REG1. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_batch_counter_threshold_set(stmdev_ctx_t *ctx, + uint16_t val) +{ + lsm6dso_counter_bdr_reg1_t counter_bdr_reg1; + lsm6dso_counter_bdr_reg2_t counter_bdr_reg2; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_COUNTER_BDR_REG1, (uint8_t *)&counter_bdr_reg1, 1); + if (ret != 0) { return ret; } + + counter_bdr_reg2.cnt_bdr_th = 0x00FFU & (uint8_t)val; + counter_bdr_reg1.cnt_bdr_th = (uint8_t)(0x0700U & val) >> 8; + + ret += lsm6dso_write_reg(ctx, LSM6DSO_COUNTER_BDR_REG1, (uint8_t *)&counter_bdr_reg1, 1); + ret += lsm6dso_write_reg(ctx, LSM6DSO_COUNTER_BDR_REG2, (uint8_t *)&counter_bdr_reg2, 1); + + return ret; +} + +/** + * @brief Batch data rate counter.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of cnt_bdr_th in + * reg COUNTER_BDR_REG2 and COUNTER_BDR_REG1. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_batch_counter_threshold_get(stmdev_ctx_t *ctx, + uint16_t *val) +{ + lsm6dso_counter_bdr_reg1_t counter_bdr_reg1; + lsm6dso_counter_bdr_reg2_t counter_bdr_reg2; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_COUNTER_BDR_REG1, + (uint8_t *)&counter_bdr_reg1, 1); + + if (ret == 0) + { + ret = lsm6dso_read_reg(ctx, LSM6DSO_COUNTER_BDR_REG2, + (uint8_t *)&counter_bdr_reg2, 1); + *val = ((uint16_t)counter_bdr_reg1.cnt_bdr_th << 8) + + (uint16_t)counter_bdr_reg2.cnt_bdr_th; + } + + return ret; +} + +/** + * @brief Number of unread sensor data(TAG + 6 bytes) stored in FIFO.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of diff_fifo in reg FIFO_STATUS1 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_fifo_data_level_get(stmdev_ctx_t *ctx, uint16_t *val) +{ + lsm6dso_fifo_status1_t fifo_status1; + lsm6dso_fifo_status2_t fifo_status2; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_FIFO_STATUS1, + (uint8_t *)&fifo_status1, 1); + + if (ret == 0) + { + ret = lsm6dso_read_reg(ctx, LSM6DSO_FIFO_STATUS2, + (uint8_t *)&fifo_status2, 1); + *val = ((uint16_t)fifo_status2.diff_fifo << 8) + + (uint16_t)fifo_status1.diff_fifo; + } + + return ret; +} + +/** + * @brief FIFO status.[get] + * + * @param ctx read / write interface definitions + * @param val registers FIFO_STATUS2 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_fifo_status_get(stmdev_ctx_t *ctx, + lsm6dso_fifo_status2_t *val) +{ + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_FIFO_STATUS2, (uint8_t *) val, 1); + + return ret; +} + +/** + * @brief Smart FIFO full status.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of fifo_full_ia in reg FIFO_STATUS2 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_fifo_full_flag_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_fifo_status2_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_FIFO_STATUS2, (uint8_t *)®, 1); + *val = reg.fifo_full_ia; + + return ret; +} + +/** + * @brief FIFO overrun status.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of fifo_over_run_latched in + * reg FIFO_STATUS2 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_fifo_ovr_flag_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_fifo_status2_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_FIFO_STATUS2, (uint8_t *)®, 1); + *val = reg.fifo_ovr_ia; + + return ret; +} + +/** + * @brief FIFO watermark status.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of fifo_wtm_ia in reg FIFO_STATUS2 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_fifo_wtm_flag_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_fifo_status2_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_FIFO_STATUS2, (uint8_t *)®, 1); + *val = reg.fifo_wtm_ia; + + return ret; +} + +/** + * @brief Identifies the sensor in FIFO_DATA_OUT.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of tag_sensor in reg FIFO_DATA_OUT_TAG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_fifo_sensor_tag_get(stmdev_ctx_t *ctx, + lsm6dso_fifo_tag_t *val) +{ + lsm6dso_fifo_data_out_tag_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_FIFO_DATA_OUT_TAG, + (uint8_t *)®, 1); + + switch (reg.tag_sensor) + { + case LSM6DSO_GYRO_NC_TAG: + *val = LSM6DSO_GYRO_NC_TAG; + break; + + case LSM6DSO_XL_NC_TAG: + *val = LSM6DSO_XL_NC_TAG; + break; + + case LSM6DSO_TEMPERATURE_TAG: + *val = LSM6DSO_TEMPERATURE_TAG; + break; + + case LSM6DSO_TIMESTAMP_TAG: + *val = LSM6DSO_TIMESTAMP_TAG; + break; + + case LSM6DSO_CFG_CHANGE_TAG: + *val = LSM6DSO_CFG_CHANGE_TAG; + break; + + case LSM6DSO_XL_NC_T_2_TAG: + *val = LSM6DSO_XL_NC_T_2_TAG; + break; + + case LSM6DSO_XL_NC_T_1_TAG: + *val = LSM6DSO_XL_NC_T_1_TAG; + break; + + case LSM6DSO_XL_2XC_TAG: + *val = LSM6DSO_XL_2XC_TAG; + break; + + case LSM6DSO_XL_3XC_TAG: + *val = LSM6DSO_XL_3XC_TAG; + break; + + case LSM6DSO_GYRO_NC_T_2_TAG: + *val = LSM6DSO_GYRO_NC_T_2_TAG; + break; + + case LSM6DSO_GYRO_NC_T_1_TAG: + *val = LSM6DSO_GYRO_NC_T_1_TAG; + break; + + case LSM6DSO_GYRO_2XC_TAG: + *val = LSM6DSO_GYRO_2XC_TAG; + break; + + case LSM6DSO_GYRO_3XC_TAG: + *val = LSM6DSO_GYRO_3XC_TAG; + break; + + case LSM6DSO_SENSORHUB_SLAVE0_TAG: + *val = LSM6DSO_SENSORHUB_SLAVE0_TAG; + break; + + case LSM6DSO_SENSORHUB_SLAVE1_TAG: + *val = LSM6DSO_SENSORHUB_SLAVE1_TAG; + break; + + case LSM6DSO_SENSORHUB_SLAVE2_TAG: + *val = LSM6DSO_SENSORHUB_SLAVE2_TAG; + break; + + case LSM6DSO_SENSORHUB_SLAVE3_TAG: + *val = LSM6DSO_SENSORHUB_SLAVE3_TAG; + break; + + case LSM6DSO_STEP_COUNTER_TAG: + *val = LSM6DSO_STEP_COUNTER_TAG; + break; + + case LSM6DSO_GAME_ROTATION_TAG: + *val = LSM6DSO_GAME_ROTATION_TAG; + break; + + case LSM6DSO_GEOMAG_ROTATION_TAG: + *val = LSM6DSO_GEOMAG_ROTATION_TAG; + break; + + case LSM6DSO_ROTATION_TAG: + *val = LSM6DSO_ROTATION_TAG; + break; + + case LSM6DSO_SENSORHUB_NACK_TAG: + *val = LSM6DSO_SENSORHUB_NACK_TAG; + break; + + default: + *val = LSM6DSO_GYRO_NC_TAG; + break; + } + + return ret; +} + +/** + * @brief : Enable FIFO batching of pedometer embedded + * function values.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of gbias_fifo_en in + * reg LSM6DSO_EMB_FUNC_FIFO_CFG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_fifo_pedo_batch_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dso_emb_func_fifo_cfg_t reg; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_EMBEDDED_FUNC_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_EMB_FUNC_FIFO_CFG, (uint8_t *)®, 1); + reg.pedo_fifo_en = val; + ret += lsm6dso_write_reg(ctx, LSM6DSO_EMB_FUNC_FIFO_CFG, (uint8_t *)®, 1); + + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @brief Enable FIFO batching of pedometer embedded function values.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of pedo_fifo_en in + * reg LSM6DSO_EMB_FUNC_FIFO_CFG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_fifo_pedo_batch_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_emb_func_fifo_cfg_t reg; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_EMBEDDED_FUNC_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_EMB_FUNC_FIFO_CFG, (uint8_t *)®, 1); + *val = reg.pedo_fifo_en; + + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @brief Enable FIFO batching data of slave idx.[set] + * + * @param ctx read / write interface definitions + * @param val Enable FIFO data batching of slave idx + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_sh_batch_slave_set(stmdev_ctx_t *ctx, uint8_t idx, uint8_t val) +{ + lsm6dso_slv0_config_t reg; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_SENSOR_HUB_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_SLV0_CONFIG + 3U*idx, (uint8_t *)®, 1); + reg.batch_ext_sens_0_en = val; + ret += lsm6dso_write_reg(ctx, LSM6DSO_SLV0_CONFIG + 3U*idx, (uint8_t *)®, 1); + + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @brief Enable FIFO batching data of slave idx.[get] + * + * @param ctx read / write interface definitions + * @param val Enable FIFO data batching of slave idx + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_sh_batch_slave_get(stmdev_ctx_t *ctx, uint8_t idx, uint8_t *val) +{ + lsm6dso_slv0_config_t reg; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_SENSOR_HUB_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_SLV0_CONFIG + 3U*idx, (uint8_t *)®, 1); + *val = reg.batch_ext_sens_0_en; + + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DSO_DEN_functionality + * @brief This section groups all the functions concerning + * DEN functionality. + * @{ + * + */ + +/** + * @brief DEN functionality marking mode.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of den_mode in reg CTRL6_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_den_mode_set(stmdev_ctx_t *ctx, + lsm6dso_den_mode_t val) +{ + lsm6dso_ctrl6_c_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL6_C, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.den_mode = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL6_C, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief DEN functionality marking mode.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of den_mode in reg CTRL6_C + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_den_mode_get(stmdev_ctx_t *ctx, + lsm6dso_den_mode_t *val) +{ + lsm6dso_ctrl6_c_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL6_C, (uint8_t *)®, 1); + + switch (reg.den_mode) + { + case LSM6DSO_DEN_DISABLE: + *val = LSM6DSO_DEN_DISABLE; + break; + + case LSM6DSO_LEVEL_FIFO: + *val = LSM6DSO_LEVEL_FIFO; + break; + + case LSM6DSO_LEVEL_LETCHED: + *val = LSM6DSO_LEVEL_LETCHED; + break; + + case LSM6DSO_LEVEL_TRIGGER: + *val = LSM6DSO_LEVEL_TRIGGER; + break; + + case LSM6DSO_EDGE_TRIGGER: + *val = LSM6DSO_EDGE_TRIGGER; + break; + + default: + *val = LSM6DSO_DEN_DISABLE; + break; + } + + return ret; +} + +/** + * @brief DEN active level configuration.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of den_lh in reg CTRL9_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_den_polarity_set(stmdev_ctx_t *ctx, + lsm6dso_den_lh_t val) +{ + lsm6dso_ctrl9_xl_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL9_XL, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.den_lh = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL9_XL, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief DEN active level configuration.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of den_lh in reg CTRL9_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_den_polarity_get(stmdev_ctx_t *ctx, + lsm6dso_den_lh_t *val) +{ + lsm6dso_ctrl9_xl_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL9_XL, (uint8_t *)®, 1); + + switch (reg.den_lh) + { + case LSM6DSO_DEN_ACT_LOW: + *val = LSM6DSO_DEN_ACT_LOW; + break; + + case LSM6DSO_DEN_ACT_HIGH: + *val = LSM6DSO_DEN_ACT_HIGH; + break; + + default: + *val = LSM6DSO_DEN_ACT_LOW; + break; + } + + return ret; +} + +/** + * @brief DEN enable.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of den_xl_g in reg CTRL9_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_den_enable_set(stmdev_ctx_t *ctx, + lsm6dso_den_xl_g_t val) +{ + lsm6dso_ctrl9_xl_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL9_XL, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.den_xl_g = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL9_XL, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief DEN enable.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of den_xl_g in reg CTRL9_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_den_enable_get(stmdev_ctx_t *ctx, + lsm6dso_den_xl_g_t *val) +{ + lsm6dso_ctrl9_xl_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL9_XL, (uint8_t *)®, 1); + + switch (reg.den_xl_g) + { + case LSM6DSO_STAMP_IN_GY_DATA: + *val = LSM6DSO_STAMP_IN_GY_DATA; + break; + + case LSM6DSO_STAMP_IN_XL_DATA: + *val = LSM6DSO_STAMP_IN_XL_DATA; + break; + + case LSM6DSO_STAMP_IN_GY_XL_DATA: + *val = LSM6DSO_STAMP_IN_GY_XL_DATA; + break; + + default: + *val = LSM6DSO_STAMP_IN_GY_DATA; + break; + } + + return ret; +} + +/** + * @brief DEN value stored in LSB of X-axis.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of den_z in reg CTRL9_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_den_mark_axis_x_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dso_ctrl9_xl_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL9_XL, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.den_z = val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL9_XL, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief DEN value stored in LSB of X-axis.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of den_z in reg CTRL9_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_den_mark_axis_x_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_ctrl9_xl_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL9_XL, (uint8_t *)®, 1); + *val = reg.den_z; + + return ret; +} + +/** + * @brief DEN value stored in LSB of Y-axis.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of den_y in reg CTRL9_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_den_mark_axis_y_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dso_ctrl9_xl_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL9_XL, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.den_y = val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL9_XL, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief DEN value stored in LSB of Y-axis.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of den_y in reg CTRL9_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_den_mark_axis_y_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_ctrl9_xl_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL9_XL, (uint8_t *)®, 1); + *val = reg.den_y; + + return ret; +} + +/** + * @brief DEN value stored in LSB of Z-axis.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of den_x in reg CTRL9_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_den_mark_axis_z_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dso_ctrl9_xl_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL9_XL, (uint8_t *)®, 1); + + if (ret == 0) + { + reg.den_x = val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL9_XL, (uint8_t *)®, 1); + } + + return ret; +} + +/** + * @brief DEN value stored in LSB of Z-axis.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of den_x in reg CTRL9_XL + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_den_mark_axis_z_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_ctrl9_xl_t reg; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL9_XL, (uint8_t *)®, 1); + *val = reg.den_x; + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DSO_Pedometer + * @brief This section groups all the functions that manage pedometer. + * @{ + * + */ + +/** + * @brief Enable pedometer algorithm.[set] + * + * @param ctx read / write interface definitions + * @param val turn on and configure pedometer + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_pedo_sens_set(stmdev_ctx_t *ctx, + lsm6dso_pedo_md_t val) +{ + lsm6dso_pedo_cmd_reg_t pedo_cmd_reg; + int32_t ret; + + ret = lsm6dso_ln_pg_read_byte(ctx, LSM6DSO_PEDO_CMD_REG, + (uint8_t *)&pedo_cmd_reg); + + if (ret == 0) + { + pedo_cmd_reg.fp_rejection_en = ((uint8_t)val & 0x10U) >> 4; + pedo_cmd_reg.ad_det_en = ((uint8_t)val & 0x20U) >> 5; + ret = lsm6dso_ln_pg_write_byte(ctx, LSM6DSO_PEDO_CMD_REG, + (uint8_t *)&pedo_cmd_reg); + } + + return ret; +} + +/** + * @brief Enable pedometer algorithm.[get] + * + * @param ctx read / write interface definitions + * @param val turn on and configure pedometer + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_pedo_sens_get(stmdev_ctx_t *ctx, + lsm6dso_pedo_md_t *val) +{ + lsm6dso_pedo_cmd_reg_t pedo_cmd_reg; + int32_t ret; + + ret = lsm6dso_ln_pg_read_byte(ctx, LSM6DSO_PEDO_CMD_REG, + (uint8_t *)&pedo_cmd_reg); + + switch ((pedo_cmd_reg.ad_det_en << 5) | (pedo_cmd_reg.fp_rejection_en + << 4)) + { + case LSM6DSO_PEDO_BASE_MODE: + *val = LSM6DSO_PEDO_BASE_MODE; + break; + + case LSM6DSO_FALSE_STEP_REJ: + *val = LSM6DSO_FALSE_STEP_REJ; + break; + + case LSM6DSO_FALSE_STEP_REJ_ADV_MODE: + *val = LSM6DSO_FALSE_STEP_REJ_ADV_MODE; + break; + + default: + *val = LSM6DSO_PEDO_BASE_MODE; + break; + } + + return ret; +} + +/** + * @brief Interrupt status bit for step detection.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of is_step_det in reg EMB_FUNC_STATUS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_pedo_step_detect_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_emb_func_status_t reg; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_EMBEDDED_FUNC_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_EMB_FUNC_STATUS, (uint8_t *)®, 1); + *val = reg.is_step_det; + + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @brief Pedometer debounce configuration register (r/w).[set] + * + * @param ctx read / write interface definitions + * @param buff buffer that contains data to write + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_pedo_debounce_steps_set(stmdev_ctx_t *ctx, + uint8_t *buff) +{ + int32_t ret; + + ret = lsm6dso_ln_pg_write_byte(ctx, LSM6DSO_PEDO_DEB_STEPS_CONF, + buff); + + return ret; +} + +/** + * @brief Pedometer debounce configuration register (r/w).[get] + * + * @param ctx read / write interface definitions + * @param buff buffer that stores data read + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_pedo_debounce_steps_get(stmdev_ctx_t *ctx, + uint8_t *buff) +{ + int32_t ret; + + ret = lsm6dso_ln_pg_read_byte(ctx, LSM6DSO_PEDO_DEB_STEPS_CONF, buff); + + return ret; +} + +/** + * @brief Time period register for step detection on delta time (r/w).[set] + * + * @param ctx read / write interface definitions + * @param buff buffer that contains data to write + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_pedo_steps_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 = lsm6dso_ln_pg_write_byte(ctx, LSM6DSO_PEDO_SC_DELTAT_L, &buff[0]); + ret += lsm6dso_ln_pg_write_byte(ctx, LSM6DSO_PEDO_SC_DELTAT_H, &buff[1]); + + return ret; +} + +/** + * @brief Time period register for step detection on delta time (r/w).[get] + * + * @param ctx read / write interface definitions + * @param buff buffer that stores data read + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_pedo_steps_period_get(stmdev_ctx_t *ctx, + uint16_t *val) +{ + uint8_t buff[2]; + int32_t ret; + + ret = lsm6dso_ln_pg_read_byte(ctx, LSM6DSO_PEDO_SC_DELTAT_L, &buff[0]); + ret += lsm6dso_ln_pg_read_byte(ctx, LSM6DSO_PEDO_SC_DELTAT_H, &buff[1]); + + *val = buff[1]; + *val = (*val * 256U) + buff[0]; + + return ret; +} + +/** + * @brief Set when user wants to generate interrupt on count overflow + * event/every step.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of carry_count_en in reg PEDO_CMD_REG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_pedo_int_mode_set(stmdev_ctx_t *ctx, + lsm6dso_carry_count_en_t val) +{ + lsm6dso_pedo_cmd_reg_t reg; + int32_t ret; + + ret = lsm6dso_ln_pg_read_byte(ctx, LSM6DSO_PEDO_CMD_REG, + (uint8_t *)®); + + if (ret == 0) + { + reg.carry_count_en = (uint8_t)val; + ret = lsm6dso_ln_pg_write_byte(ctx, LSM6DSO_PEDO_CMD_REG, + (uint8_t *)®); + } + + return ret; +} + +/** + * @brief Set when user wants to generate interrupt on count overflow + * event/every step.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of carry_count_en in reg PEDO_CMD_REG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_pedo_int_mode_get(stmdev_ctx_t *ctx, + lsm6dso_carry_count_en_t *val) +{ + lsm6dso_pedo_cmd_reg_t reg; + int32_t ret; + + ret = lsm6dso_ln_pg_read_byte(ctx, LSM6DSO_PEDO_CMD_REG, + (uint8_t *)®); + + switch (reg.carry_count_en) + { + case LSM6DSO_EVERY_STEP: + *val = LSM6DSO_EVERY_STEP; + break; + + case LSM6DSO_COUNT_OVERFLOW: + *val = LSM6DSO_COUNT_OVERFLOW; + break; + + default: + *val = LSM6DSO_EVERY_STEP; + break; + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DSO_significant_motion + * @brief This section groups all the functions that manage the + * significant motion detection. + * @{ + * + */ + +/** + * @brief Interrupt status bit for significant motion detection.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of is_sigmot in reg EMB_FUNC_STATUS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_motion_flag_data_ready_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6dso_emb_func_status_t reg; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_EMBEDDED_FUNC_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_EMB_FUNC_STATUS, (uint8_t *)®, 1); + *val = reg.is_sigmot; + + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DSO_tilt_detection + * @brief This section groups all the functions that manage the tilt + * event detection. + * @{ + * + */ + +/** + * @brief Interrupt status bit for tilt detection.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of is_tilt in reg EMB_FUNC_STATUS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_tilt_flag_data_ready_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6dso_emb_func_status_t reg; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_EMBEDDED_FUNC_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_EMB_FUNC_STATUS, (uint8_t *)®, 1); + *val = reg.is_tilt; + + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DSO_ magnetometer_sensor + * @brief This section groups all the functions that manage additional + * magnetometer sensor. + * @{ + * + */ + +/** + * @brief External magnetometer sensitivity value register.[set] + * + * @param ctx read / write interface definitions + * @param buff buffer that contains data to write + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_mag_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 = lsm6dso_ln_pg_write_byte(ctx, LSM6DSO_MAG_SENSITIVITY_L, &buff[0]); + ret += lsm6dso_ln_pg_write_byte(ctx, LSM6DSO_MAG_SENSITIVITY_H, &buff[1]); + + return ret; +} + +/** + * @brief External magnetometer sensitivity value register.[get] + * + * @param ctx read / write interface definitions + * @param buff buffer that stores data read + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_mag_sensitivity_get(stmdev_ctx_t *ctx, uint16_t *val) +{ + uint8_t buff[2]; + int32_t ret; + + ret = lsm6dso_ln_pg_read_byte(ctx, LSM6DSO_MAG_SENSITIVITY_L, &buff[0]); + ret += lsm6dso_ln_pg_read_byte(ctx, LSM6DSO_MAG_SENSITIVITY_H, &buff[1]); + + *val = buff[1]; + *val = (*val * 256U) + buff[0]; + + return ret; +} + +/** + * @brief Offset for hard-iron compensation register (r/w).[set] + * + * @param ctx read / write interface definitions + * @param buff buffer that contains data to write + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_mag_offset_set(stmdev_ctx_t *ctx, int16_t *val) +{ + uint8_t buff[6]; + + buff[1] = (uint8_t)((uint16_t)val[0] / 256U); + buff[0] = (uint8_t)((uint16_t)val[0] - (buff[1] * 256U)); + buff[3] = (uint8_t)((uint16_t)val[1] / 256U); + buff[2] = (uint8_t)((uint16_t)val[1] - (buff[3] * 256U)); + buff[5] = (uint8_t)((uint16_t)val[2] / 256U); + buff[4] = (uint8_t)((uint16_t)val[2] - (buff[5] * 256U)); + + return lsm6dso_ln_pg_write(ctx, LSM6DSO_MAG_OFFX_L, &buff[0], 6); +} + +/** + * @brief Offset for hard-iron compensation register (r/w).[get] + * + * @param ctx read / write interface definitions + * @param buff buffer that stores data read + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_mag_offset_get(stmdev_ctx_t *ctx, int16_t *val) +{ + uint8_t buff[6]; + int32_t ret; + + ret = lsm6dso_ln_pg_read(ctx, LSM6DSO_MAG_OFFX_L, &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 Soft-iron (3x3 symmetric) matrix correction + * register (r/w). 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 buff buffer that contains data to write + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_mag_soft_iron_set(stmdev_ctx_t *ctx, int16_t *val) +{ + uint8_t buff[12]; + + buff[1] = (uint8_t)((uint16_t)val[0] / 256U); + buff[0] = (uint8_t)((uint16_t)val[0] - (buff[1] * 256U)); + buff[3] = (uint8_t)((uint16_t)val[1] / 256U); + buff[2] = (uint8_t)((uint16_t)val[1] - (buff[3] * 256U)); + buff[5] = (uint8_t)((uint16_t)val[2] / 256U); + buff[4] = (uint8_t)((uint16_t)val[2] - (buff[5] * 256U)); + buff[7] = (uint8_t)((uint16_t)val[3] / 256U); + buff[6] = (uint8_t)((uint16_t)val[3] - (buff[7] * 256U)); + buff[9] = (uint8_t)((uint16_t)val[4] / 256U); + buff[8] = (uint8_t)((uint16_t)val[4] - (buff[9] * 256U)); + buff[11] = (uint8_t)((uint16_t)val[5] / 256U); + buff[10] = (uint8_t)((uint16_t)val[5] - (buff[11] * 256U)); + + return lsm6dso_ln_pg_write(ctx, LSM6DSO_MAG_SI_XX_L, &buff[0], 12); +} + +/** + * @brief Soft-iron (3x3 symmetric) matrix + * correction register (r/w). + * 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 buff buffer that stores data read + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_mag_soft_iron_get(stmdev_ctx_t *ctx, int16_t *val) +{ + uint8_t buff[12]; + int32_t ret; + + ret = lsm6dso_ln_pg_read(ctx, LSM6DSO_MAG_SI_XX_L, &buff[0], 12); + + 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]; + val[3] = (int16_t)buff[7]; + val[3] = (val[3] * 256) + (int16_t)buff[6]; + val[4] = (int16_t)buff[9]; + val[4] = (val[4] * 256) + (int16_t)buff[8]; + val[5] = (int16_t)buff[11]; + val[5] = (val[5] * 256) + (int16_t)buff[10]; + + return ret; +} + +/** + * @brief Magnetometer Z-axis coordinates + * rotation (to be aligned to + * accelerometer/gyroscope axes + * orientation).[set] + * + * @param ctx read / write interface definitions + * @param val change the values of mag_z_axis in reg MAG_CFG_A + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_mag_z_orient_set(stmdev_ctx_t *ctx, + lsm6dso_mag_z_axis_t val) +{ + lsm6dso_mag_cfg_a_t reg; + int32_t ret; + + ret = lsm6dso_ln_pg_read_byte(ctx, LSM6DSO_MAG_CFG_A, (uint8_t *)®); + + if (ret == 0) + { + reg.mag_z_axis = (uint8_t) val; + ret = lsm6dso_ln_pg_write_byte(ctx, LSM6DSO_MAG_CFG_A, (uint8_t *)®); + } + + return ret; +} + +/** + * @brief Magnetometer Z-axis coordinates + * rotation (to be aligned to + * accelerometer/gyroscope axes + * orientation).[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of mag_z_axis in reg MAG_CFG_A + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_mag_z_orient_get(stmdev_ctx_t *ctx, + lsm6dso_mag_z_axis_t *val) +{ + lsm6dso_mag_cfg_a_t reg; + int32_t ret; + + ret = lsm6dso_ln_pg_read_byte(ctx, LSM6DSO_MAG_CFG_A, + (uint8_t *)®); + + switch (reg.mag_z_axis) + { + case LSM6DSO_Z_EQ_Y: + *val = LSM6DSO_Z_EQ_Y; + break; + + case LSM6DSO_Z_EQ_MIN_Y: + *val = LSM6DSO_Z_EQ_MIN_Y; + break; + + case LSM6DSO_Z_EQ_X: + *val = LSM6DSO_Z_EQ_X; + break; + + case LSM6DSO_Z_EQ_MIN_X: + *val = LSM6DSO_Z_EQ_MIN_X; + break; + + case LSM6DSO_Z_EQ_MIN_Z: + *val = LSM6DSO_Z_EQ_MIN_Z; + break; + + case LSM6DSO_Z_EQ_Z: + *val = LSM6DSO_Z_EQ_Z; + break; + + default: + *val = LSM6DSO_Z_EQ_Y; + break; + } + + return ret; +} + +/** + * @brief Magnetometer Y-axis coordinates + * rotation (to be aligned to + * accelerometer/gyroscope axes + * orientation).[set] + * + * @param ctx read / write interface definitions + * @param val change the values of mag_y_axis in reg MAG_CFG_A + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_mag_y_orient_set(stmdev_ctx_t *ctx, + lsm6dso_mag_y_axis_t val) +{ + lsm6dso_mag_cfg_a_t reg; + int32_t ret; + + ret = lsm6dso_ln_pg_read_byte(ctx, LSM6DSO_MAG_CFG_A, (uint8_t *)®); + + if (ret == 0) + { + reg.mag_y_axis = (uint8_t)val; + ret = lsm6dso_ln_pg_write_byte(ctx, LSM6DSO_MAG_CFG_A, (uint8_t *) ®); + } + + return ret; +} + +/** + * @brief Magnetometer Y-axis coordinates + * rotation (to be aligned to + * accelerometer/gyroscope axes + * orientation).[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of mag_y_axis in reg MAG_CFG_A + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_mag_y_orient_get(stmdev_ctx_t *ctx, + lsm6dso_mag_y_axis_t *val) +{ + lsm6dso_mag_cfg_a_t reg; + int32_t ret; + + ret = lsm6dso_ln_pg_read_byte(ctx, LSM6DSO_MAG_CFG_A, + (uint8_t *)®); + + switch (reg.mag_y_axis) + { + case LSM6DSO_Y_EQ_Y: + *val = LSM6DSO_Y_EQ_Y; + break; + + case LSM6DSO_Y_EQ_MIN_Y: + *val = LSM6DSO_Y_EQ_MIN_Y; + break; + + case LSM6DSO_Y_EQ_X: + *val = LSM6DSO_Y_EQ_X; + break; + + case LSM6DSO_Y_EQ_MIN_X: + *val = LSM6DSO_Y_EQ_MIN_X; + break; + + case LSM6DSO_Y_EQ_MIN_Z: + *val = LSM6DSO_Y_EQ_MIN_Z; + break; + + case LSM6DSO_Y_EQ_Z: + *val = LSM6DSO_Y_EQ_Z; + break; + + default: + *val = LSM6DSO_Y_EQ_Y; + break; + } + + return ret; +} + +/** + * @brief Magnetometer X-axis coordinates + * rotation (to be aligned to + * accelerometer/gyroscope axes + * orientation).[set] + * + * @param ctx read / write interface definitions + * @param val change the values of mag_x_axis in reg MAG_CFG_B + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_mag_x_orient_set(stmdev_ctx_t *ctx, + lsm6dso_mag_x_axis_t val) +{ + lsm6dso_mag_cfg_b_t reg; + int32_t ret; + + ret = lsm6dso_ln_pg_read_byte(ctx, LSM6DSO_MAG_CFG_B, (uint8_t *)®); + + if (ret == 0) + { + reg.mag_x_axis = (uint8_t)val; + ret = lsm6dso_ln_pg_write_byte(ctx, LSM6DSO_MAG_CFG_B, (uint8_t *)®); + } + + return ret; +} + +/** + * @brief Magnetometer X-axis coordinates + * rotation (to be aligned to + * accelerometer/gyroscope axes + * orientation).[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of mag_x_axis in reg MAG_CFG_B + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_mag_x_orient_get(stmdev_ctx_t *ctx, + lsm6dso_mag_x_axis_t *val) +{ + lsm6dso_mag_cfg_b_t reg; + int32_t ret; + + ret = lsm6dso_ln_pg_read_byte(ctx, LSM6DSO_MAG_CFG_B, + (uint8_t *)®); + + switch (reg.mag_x_axis) + { + case LSM6DSO_X_EQ_Y: + *val = LSM6DSO_X_EQ_Y; + break; + + case LSM6DSO_X_EQ_MIN_Y: + *val = LSM6DSO_X_EQ_MIN_Y; + break; + + case LSM6DSO_X_EQ_X: + *val = LSM6DSO_X_EQ_X; + break; + + case LSM6DSO_X_EQ_MIN_X: + *val = LSM6DSO_X_EQ_MIN_X; + break; + + case LSM6DSO_X_EQ_MIN_Z: + *val = LSM6DSO_X_EQ_MIN_Z; + break; + + case LSM6DSO_X_EQ_Z: + *val = LSM6DSO_X_EQ_Z; + break; + + default: + *val = LSM6DSO_X_EQ_Y; + break; + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DSO_finite_state_machine + * @brief This section groups all the functions that manage the + * state_machine. + * @{ + * + */ + +/** + * @brief Interrupt status bit for FSM long counter + * timeout interrupt event.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of is_fsm_lc in reg EMB_FUNC_STATUS + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_long_cnt_flag_data_ready_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + lsm6dso_emb_func_status_t reg; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_EMBEDDED_FUNC_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_EMB_FUNC_STATUS, (uint8_t *)®, 1); + *val = reg.is_fsm_lc; + + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @brief Final State Machine enable.[set] + * + * @param ctx read / write interface definitions + * @param val union of registers from FSM_ENABLE_A to FSM_ENABLE_B + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_fsm_enable_set(stmdev_ctx_t *ctx, + lsm6dso_emb_fsm_enable_t *val) +{ + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_EMBEDDED_FUNC_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_write_reg(ctx, LSM6DSO_FSM_ENABLE_A, (uint8_t *)&val->fsm_enable_a, 1); + ret += lsm6dso_write_reg(ctx, LSM6DSO_FSM_ENABLE_B, (uint8_t *)&val->fsm_enable_b, 1); + + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @brief Final State Machine enable.[get] + * + * @param ctx read / write interface definitions + * @param val union of registers from FSM_ENABLE_A to FSM_ENABLE_B + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_fsm_enable_get(stmdev_ctx_t *ctx, + lsm6dso_emb_fsm_enable_t *val) +{ + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_EMBEDDED_FUNC_BANK); + ret += lsm6dso_read_reg(ctx, LSM6DSO_FSM_ENABLE_A, (uint8_t *) val, 2); + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + 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 buff buffer that contains data to write + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_long_cnt_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 = lsm6dso_mem_bank_set(ctx, LSM6DSO_EMBEDDED_FUNC_BANK); + ret += lsm6dso_write_reg(ctx, LSM6DSO_FSM_LONG_COUNTER_L, buff, 2); + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_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 buff buffer that stores data read + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_long_cnt_get(stmdev_ctx_t *ctx, uint16_t *val) +{ + uint8_t buff[2]; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_EMBEDDED_FUNC_BANK); + ret += lsm6dso_read_reg(ctx, LSM6DSO_FSM_LONG_COUNTER_L, buff, 2); + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + *val = buff[1]; + *val = (*val * 256U) + buff[0]; + + return ret; +} + +/** + * @brief Clear FSM long counter value.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of fsm_lc_clr in + * reg FSM_LONG_COUNTER_CLEAR + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_long_clr_set(stmdev_ctx_t *ctx, + lsm6dso_fsm_lc_clr_t val) +{ + lsm6dso_fsm_long_counter_clear_t reg; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_EMBEDDED_FUNC_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_FSM_LONG_COUNTER_CLEAR, (uint8_t *)®, 1); + reg. fsm_lc_clr = (uint8_t)val; + ret += lsm6dso_write_reg(ctx, LSM6DSO_FSM_LONG_COUNTER_CLEAR, (uint8_t *)®, 1); + + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @brief Clear FSM long counter value.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of fsm_lc_clr in + * reg FSM_LONG_COUNTER_CLEAR + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_long_clr_get(stmdev_ctx_t *ctx, + lsm6dso_fsm_lc_clr_t *val) +{ + lsm6dso_fsm_long_counter_clear_t reg; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_EMBEDDED_FUNC_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_FSM_LONG_COUNTER_CLEAR, (uint8_t *)®, 1); + if (ret != 0) { goto exit; } + + switch (reg.fsm_lc_clr) + { + case LSM6DSO_LC_NORMAL: + *val = LSM6DSO_LC_NORMAL; + break; + + case LSM6DSO_LC_CLEAR: + *val = LSM6DSO_LC_CLEAR; + break; + + case LSM6DSO_LC_CLEAR_DONE: + *val = LSM6DSO_LC_CLEAR_DONE; + break; + + default: + *val = LSM6DSO_LC_NORMAL; + break; + } + +exit: + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @brief FSM output registers[get] + * + * @param ctx read / write interface definitions + * @param val struct of registers from FSM_OUTS1 to FSM_OUTS16 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_fsm_out_get(stmdev_ctx_t *ctx, lsm6dso_fsm_out_t *val) +{ + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_EMBEDDED_FUNC_BANK); + ret += lsm6dso_read_reg(ctx, LSM6DSO_FSM_OUTS1, (uint8_t *)val, 16); + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @brief Finite State Machine ODR configuration.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of fsm_odr in reg EMB_FUNC_ODR_CFG_B + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_fsm_data_rate_set(stmdev_ctx_t *ctx, + lsm6dso_fsm_odr_t val) +{ + lsm6dso_emb_func_odr_cfg_b_t reg; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_EMBEDDED_FUNC_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_EMB_FUNC_ODR_CFG_B, (uint8_t *)®, 1); + if (ret != 0) { goto exit; } + + reg.not_used_01 = 3; /* set default values */ + reg.not_used_02 = 2; /* set default values */ + reg.fsm_odr = (uint8_t)val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_EMB_FUNC_ODR_CFG_B, (uint8_t *)®, 1); + +exit: + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @brief Finite State Machine ODR configuration.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of fsm_odr in reg EMB_FUNC_ODR_CFG_B + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_fsm_data_rate_get(stmdev_ctx_t *ctx, + lsm6dso_fsm_odr_t *val) +{ + lsm6dso_emb_func_odr_cfg_b_t reg; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_EMBEDDED_FUNC_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_EMB_FUNC_ODR_CFG_B, (uint8_t *)®, 1); + if (ret != 0) { goto exit; } + + switch (reg.fsm_odr) + { + case LSM6DSO_ODR_FSM_12Hz5: + *val = LSM6DSO_ODR_FSM_12Hz5; + break; + + case LSM6DSO_ODR_FSM_26Hz: + *val = LSM6DSO_ODR_FSM_26Hz; + break; + + case LSM6DSO_ODR_FSM_52Hz: + *val = LSM6DSO_ODR_FSM_52Hz; + break; + + case LSM6DSO_ODR_FSM_104Hz: + *val = LSM6DSO_ODR_FSM_104Hz; + break; + + default: + *val = LSM6DSO_ODR_FSM_12Hz5; + break; + } + +exit: + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @brief FSM initialization request.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of fsm_init in reg FSM_INIT + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_fsm_init_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dso_emb_func_init_b_t reg; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_EMBEDDED_FUNC_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_EMB_FUNC_INIT_B, (uint8_t *)®, 1); + if (ret != 0) { goto exit; } + + reg.fsm_init = val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_EMB_FUNC_INIT_B, (uint8_t *)®, 1); + +exit: + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @brief FSM initialization request.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of fsm_init in reg FSM_INIT + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_fsm_init_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_emb_func_init_b_t reg; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_EMBEDDED_FUNC_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_EMB_FUNC_INIT_B, (uint8_t *)®, 1); + + *val = reg.fsm_init; + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @brief FSM long counter timeout register (r/w). 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 the value of long counter + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_long_cnt_int_value_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 = lsm6dso_ln_pg_write_byte(ctx, LSM6DSO_FSM_LC_TIMEOUT_L, &buff[0]); + ret += lsm6dso_ln_pg_write_byte(ctx, LSM6DSO_FSM_LC_TIMEOUT_H, &buff[1]); + + return ret; +} + +/** + * @brief FSM long counter timeout register (r/w). 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 buffer that stores the value of long counter + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_long_cnt_int_value_get(stmdev_ctx_t *ctx, + uint16_t *val) +{ + uint8_t buff[2]; + int32_t ret; + + ret = lsm6dso_ln_pg_read_byte(ctx, LSM6DSO_FSM_LC_TIMEOUT_L, &buff[0]); + ret += lsm6dso_ln_pg_read_byte(ctx, LSM6DSO_FSM_LC_TIMEOUT_H, &buff[1]); + + *val = buff[1]; + *val = (*val * 256U) + buff[0]; + + return ret; +} + +/** + * @brief FSM number of programs register.[set] + * + * @param ctx read / write interface definitions + * @param val value to write + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_fsm_number_of_programs_set(stmdev_ctx_t *ctx, + uint8_t val) +{ + int32_t ret; + + ret = lsm6dso_ln_pg_write_byte(ctx, LSM6DSO_FSM_PROGRAMS, &val); + + return ret; +} + +/** + * @brief FSM number of programs register.[get] + * + * @param ctx read / write interface definitions + * @param val buffer that stores data read. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_fsm_number_of_programs_get(stmdev_ctx_t *ctx, + uint8_t *val) +{ + int32_t ret; + + ret = lsm6dso_ln_pg_read_byte(ctx, LSM6DSO_FSM_PROGRAMS, val); + + return ret; +} + +/** + * @brief FSM start address register (r/w). + * First available address is 0x033C.[set] + * + * @param ctx read / write interface definitions + * @param val the value of start address + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_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 = lsm6dso_ln_pg_write_byte(ctx, LSM6DSO_FSM_START_ADD_L, &buff[0]); + ret += lsm6dso_ln_pg_write_byte(ctx, LSM6DSO_FSM_START_ADD_H, &buff[1]); + + return ret; +} + +/** + * @brief FSM start address register (r/w). + * First available address is 0x033C.[get] + * + * @param ctx read / write interface definitions + * @param val buffer the value of start address. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_fsm_start_address_get(stmdev_ctx_t *ctx, + uint16_t *val) +{ + uint8_t buff[2]; + int32_t ret; + + ret = lsm6dso_ln_pg_read_byte(ctx, LSM6DSO_FSM_START_ADD_L, &buff[0]); + ret += lsm6dso_ln_pg_read_byte(ctx, LSM6DSO_FSM_START_ADD_H, &buff[1]); + + *val = buff[1]; + *val = (*val * 256U) + buff[0]; + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup LSM6DSO_Sensor_hub + * @brief This section groups all the functions that manage the + * sensor hub. + * @{ + * + */ + +/** + * @brief Sensor hub output registers.[get] + * + * @param ctx read / write interface definitions + * @param val values read from registers SENSOR_HUB_1 to SENSOR_HUB_18 + * @param len number of consecutive register to read (max 18) + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_sh_read_data_raw_get(stmdev_ctx_t *ctx, uint8_t *val, + uint8_t len) +{ + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_SENSOR_HUB_BANK); + ret += lsm6dso_read_reg(ctx, LSM6DSO_SENSOR_HUB_1, (uint8_t *) val, len); + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @brief Number of external sensors to be read by the sensor hub.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of aux_sens_on in reg MASTER_CONFIG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_sh_slave_connected_set(stmdev_ctx_t *ctx, + lsm6dso_aux_sens_on_t val) +{ + lsm6dso_master_config_t reg; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_SENSOR_HUB_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_MASTER_CONFIG, (uint8_t *)®, 1); + reg.aux_sens_on = (uint8_t)val; + ret += lsm6dso_write_reg(ctx, LSM6DSO_MASTER_CONFIG, (uint8_t *)®, 1); + + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @brief Number of external sensors to be read by the sensor hub.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of aux_sens_on in reg MASTER_CONFIG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_sh_slave_connected_get(stmdev_ctx_t *ctx, + lsm6dso_aux_sens_on_t *val) +{ + lsm6dso_master_config_t reg; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_SENSOR_HUB_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_MASTER_CONFIG, (uint8_t *)®, 1); + + switch (reg.aux_sens_on) + { + case LSM6DSO_SLV_0: + *val = LSM6DSO_SLV_0; + break; + + case LSM6DSO_SLV_0_1: + *val = LSM6DSO_SLV_0_1; + break; + + case LSM6DSO_SLV_0_1_2: + *val = LSM6DSO_SLV_0_1_2; + break; + + case LSM6DSO_SLV_0_1_2_3: + *val = LSM6DSO_SLV_0_1_2_3; + break; + + default: + *val = LSM6DSO_SLV_0; + break; + } + + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @brief Sensor hub I2C master enable.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of master_on in reg MASTER_CONFIG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_sh_master_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dso_master_config_t reg; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_SENSOR_HUB_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_MASTER_CONFIG, (uint8_t *)®, 1); + reg.master_on = val; + ret += lsm6dso_write_reg(ctx, LSM6DSO_MASTER_CONFIG, (uint8_t *)®, 1); + + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @brief Sensor hub I2C master enable.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of master_on in reg MASTER_CONFIG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_sh_master_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_master_config_t reg; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_SENSOR_HUB_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_MASTER_CONFIG, (uint8_t *)®, 1); + *val = reg.master_on; + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @brief Master I2C pull-up enable.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of shub_pu_en in reg MASTER_CONFIG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_sh_pin_mode_set(stmdev_ctx_t *ctx, + lsm6dso_shub_pu_en_t val) +{ + lsm6dso_master_config_t reg; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_SENSOR_HUB_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_MASTER_CONFIG, (uint8_t *)®, 1); + reg.shub_pu_en = (uint8_t)val; + ret += lsm6dso_write_reg(ctx, LSM6DSO_MASTER_CONFIG, (uint8_t *)®, 1); + + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @brief Master I2C pull-up enable.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of shub_pu_en in reg MASTER_CONFIG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_sh_pin_mode_get(stmdev_ctx_t *ctx, + lsm6dso_shub_pu_en_t *val) +{ + lsm6dso_master_config_t reg; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_SENSOR_HUB_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_MASTER_CONFIG, (uint8_t *)®, 1); + + switch (reg.shub_pu_en) + { + case LSM6DSO_EXT_PULL_UP: + *val = LSM6DSO_EXT_PULL_UP; + break; + + case LSM6DSO_INTERNAL_PULL_UP: + *val = LSM6DSO_INTERNAL_PULL_UP; + break; + + default: + *val = LSM6DSO_EXT_PULL_UP; + break; + } + + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @brief I2C interface pass-through.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of pass_through_mode in + * reg MASTER_CONFIG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_sh_pass_through_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dso_master_config_t reg; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_SENSOR_HUB_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_MASTER_CONFIG, (uint8_t *)®, 1); + reg.pass_through_mode = val; + ret += lsm6dso_write_reg(ctx, LSM6DSO_MASTER_CONFIG, (uint8_t *)®, 1); + + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @brief I2C interface pass-through.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of pass_through_mode in + * reg MASTER_CONFIG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_sh_pass_through_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_master_config_t reg; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_SENSOR_HUB_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_MASTER_CONFIG, (uint8_t *)®, 1); + *val = reg.pass_through_mode; + + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @brief Sensor hub trigger signal selection.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of start_config in reg MASTER_CONFIG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_sh_syncro_mode_set(stmdev_ctx_t *ctx, + lsm6dso_start_config_t val) +{ + lsm6dso_master_config_t reg; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_SENSOR_HUB_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_MASTER_CONFIG, (uint8_t *)®, 1); + reg.start_config = (uint8_t)val; + ret += lsm6dso_write_reg(ctx, LSM6DSO_MASTER_CONFIG, (uint8_t *)®, 1); + + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @brief Sensor hub trigger signal selection.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of start_config in reg MASTER_CONFIG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_sh_syncro_mode_get(stmdev_ctx_t *ctx, + lsm6dso_start_config_t *val) +{ + lsm6dso_master_config_t reg; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_SENSOR_HUB_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_MASTER_CONFIG, (uint8_t *)®, 1); + + switch (reg.start_config) + { + case LSM6DSO_EXT_ON_INT2_PIN: + *val = LSM6DSO_EXT_ON_INT2_PIN; + break; + + case LSM6DSO_XL_GY_DRDY: + *val = LSM6DSO_XL_GY_DRDY; + break; + + default: + *val = LSM6DSO_EXT_ON_INT2_PIN; + break; + } + + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + 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 change the values of write_once in reg MASTER_CONFIG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_sh_write_mode_set(stmdev_ctx_t *ctx, + lsm6dso_write_once_t val) +{ + lsm6dso_master_config_t reg; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_SENSOR_HUB_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_MASTER_CONFIG, (uint8_t *)®, 1); + reg.write_once = (uint8_t)val; + ret += lsm6dso_write_reg(ctx, LSM6DSO_MASTER_CONFIG, (uint8_t *)®, 1); + + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_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 Get the values of write_once in reg MASTER_CONFIG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_sh_write_mode_get(stmdev_ctx_t *ctx, + lsm6dso_write_once_t *val) +{ + lsm6dso_master_config_t reg; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_SENSOR_HUB_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_MASTER_CONFIG, (uint8_t *)®, 1); + + switch (reg.write_once) + { + case LSM6DSO_EACH_SH_CYCLE: + *val = LSM6DSO_EACH_SH_CYCLE; + break; + + case LSM6DSO_ONLY_FIRST_CYCLE: + *val = LSM6DSO_ONLY_FIRST_CYCLE; + break; + + default: + *val = LSM6DSO_EACH_SH_CYCLE; + break; + } + + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @brief Reset Master logic and output registers.[set] + * + * @param ctx read / write interface definitions + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_sh_reset_set(stmdev_ctx_t *ctx) +{ + lsm6dso_master_config_t reg; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_SENSOR_HUB_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_MASTER_CONFIG, (uint8_t *)®, 1); + reg.rst_master_regs = PROPERTY_ENABLE; + ret += lsm6dso_write_reg(ctx, LSM6DSO_MASTER_CONFIG, (uint8_t *)®, 1); + if (ret != 0) { return ret; } + + reg.rst_master_regs = PROPERTY_DISABLE; + ret = lsm6dso_write_reg(ctx, LSM6DSO_MASTER_CONFIG, (uint8_t *)®, 1); + + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @brief Reset Master logic and output registers.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of rst_master_regs in reg MASTER_CONFIG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_sh_reset_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dso_master_config_t reg; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_SENSOR_HUB_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_MASTER_CONFIG, (uint8_t *)®, 1); + *val = reg.rst_master_regs; + + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @brief Rate at which the master communicates.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of shub_odr in reg slv1_CONFIG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_sh_data_rate_set(stmdev_ctx_t *ctx, + lsm6dso_shub_odr_t val) +{ + lsm6dso_slv0_config_t reg; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_SENSOR_HUB_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_SLV0_CONFIG, (uint8_t *)®, 1); + reg.shub_odr = (uint8_t)val; + ret += lsm6dso_write_reg(ctx, LSM6DSO_SLV0_CONFIG, (uint8_t *)®, 1); + + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @brief Rate at which the master communicates.[get] + * + * @param ctx read / write interface definitions + * @param val Get the values of shub_odr in reg slv1_CONFIG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_sh_data_rate_get(stmdev_ctx_t *ctx, + lsm6dso_shub_odr_t *val) +{ + lsm6dso_slv0_config_t reg; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_SENSOR_HUB_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_SLV0_CONFIG, (uint8_t *)®, 1); + + switch (reg.shub_odr) + { + case LSM6DSO_SH_ODR_104Hz: + *val = LSM6DSO_SH_ODR_104Hz; + break; + + case LSM6DSO_SH_ODR_52Hz: + *val = LSM6DSO_SH_ODR_52Hz; + break; + + case LSM6DSO_SH_ODR_26Hz: + *val = LSM6DSO_SH_ODR_26Hz; + break; + + case LSM6DSO_SH_ODR_13Hz: + *val = LSM6DSO_SH_ODR_13Hz; + break; + + default: + *val = LSM6DSO_SH_ODR_104Hz; + break; + } + + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_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 lsm6dso_sh_cfg_write(stmdev_ctx_t *ctx, + lsm6dso_sh_cfg_write_t *val) +{ + lsm6dso_slv0_add_t reg; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_SENSOR_HUB_BANK); + if (ret != 0) { return ret; } + + reg.slave0 = val->slv0_add; + reg.rw_0 = 0; + ret = lsm6dso_write_reg(ctx, LSM6DSO_SLV0_ADD, (uint8_t *)®, 1); + if (ret != 0) { goto exit; } + + ret = lsm6dso_write_reg(ctx, LSM6DSO_SLV0_SUBADD, &(val->slv0_subadd), 1); + if (ret != 0) { goto exit; } + + ret = lsm6dso_write_reg(ctx, LSM6DSO_DATAWRITE_SLV0, &(val->slv0_data), 1); + +exit: + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @brief Configure slave idx to 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 lsm6dso_sh_slv_cfg_read(stmdev_ctx_t *ctx, uint8_t idx, + lsm6dso_sh_cfg_read_t *val) +{ + lsm6dso_slv0_add_t slv0_add; + lsm6dso_slv0_config_t slv0_config; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_SENSOR_HUB_BANK); + if (ret != 0) { return ret; } + + slv0_add.slave0 = val->slv_add; + slv0_add.rw_0 = 1; + ret = lsm6dso_write_reg(ctx, LSM6DSO_SLV0_ADD + idx*3U, (uint8_t *)&slv0_add, 1); + if (ret != 0) { goto exit; } + + ret = lsm6dso_write_reg(ctx, LSM6DSO_SLV0_SUBADD + idx*3U, &(val->slv_subadd), 1); + if (ret != 0) { goto exit; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_SLV0_CONFIG + idx*3U, (uint8_t *)&slv0_config, 1); + slv0_config.slave0_numop = val->slv_len; + ret += lsm6dso_write_reg(ctx, LSM6DSO_SLV0_CONFIG + idx*3U, (uint8_t *)&slv0_config, 1); + +exit: + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @brief Sensor hub source register.[get] + * + * @param ctx read / write interface definitions + * @param val union of registers from STATUS_MASTER to + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_sh_status_get(stmdev_ctx_t *ctx, + lsm6dso_status_master_t *val) +{ + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_STATUS_MASTER_MAINPAGE, (uint8_t *) val, 1); + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup Basic configuration + * @brief This section groups all the functions concerning + * device basic configuration. + * @{ + * + */ + +/** + * @brief Device "Who am I".[get] + * + * @param ctx communication interface handler. Use NULL to ignore + * this interface.(ptr) + * @param aux_ctx auxiliary communication interface handler. Use NULL + * to ignore this interface.(ptr) + * @param val ID values read from the two interfaces. ID values + * will be the same.(ptr) + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_id_get(stmdev_ctx_t *ctx, stmdev_ctx_t *aux_ctx, + lsm6dso_id_t *val) +{ + int32_t ret = 0; + + if (ctx != NULL) + { + ret = lsm6dso_read_reg(ctx, LSM6DSO_WHO_AM_I, (uint8_t *) & (val->ui), 1); + } + + if (aux_ctx != NULL) + { + ret += lsm6dso_read_reg(aux_ctx, LSM6DSO_WHO_AM_I, (uint8_t *) & (val->aux), 1); + } + + return ret; +} + +/** + * @brief Re-initialize the device.[set] + * + * @param ctx communication interface handler.(ptr) + * @param val re-initialization mode. Refer to datasheet + * and application note for more information + * about differencies between boot and sw_reset + * procedure. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_init_set(stmdev_ctx_t *ctx, lsm6dso_init_t val) +{ + lsm6dso_emb_func_init_a_t emb_func_init_a; + lsm6dso_emb_func_init_b_t emb_func_init_b; + lsm6dso_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_EMBEDDED_FUNC_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_EMB_FUNC_INIT_B, (uint8_t *)&emb_func_init_b, 1); + emb_func_init_b.fifo_compr_init = (uint8_t)val & ((uint8_t)LSM6DSO_FIFO_COMP >> 2); + emb_func_init_b.fsm_init = (uint8_t)val & ((uint8_t)LSM6DSO_FSM >> 3); + ret += lsm6dso_write_reg(ctx, LSM6DSO_EMB_FUNC_INIT_B, (uint8_t *)&emb_func_init_b, 1); + + ret += lsm6dso_read_reg(ctx, LSM6DSO_EMB_FUNC_INIT_A, (uint8_t *)&emb_func_init_a, 1); + emb_func_init_a.step_det_init = ((uint8_t)val & (uint8_t)LSM6DSO_PEDO) >> 5; + emb_func_init_a.tilt_init = ((uint8_t)val & (uint8_t)LSM6DSO_TILT) >> 6; + emb_func_init_a.sig_mot_init = ((uint8_t)val & (uint8_t)LSM6DSO_SMOTION) >> 7; + ret += lsm6dso_write_reg(ctx, LSM6DSO_EMB_FUNC_INIT_A, (uint8_t *)&emb_func_init_a, 1); + + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + + if (((val == LSM6DSO_BOOT) || (val == LSM6DSO_RESET)) && (ret == 0)) + { + ctrl3_c.boot = (uint8_t)val & (uint8_t)LSM6DSO_BOOT; + ctrl3_c.sw_reset = ((uint8_t)val & (uint8_t)LSM6DSO_RESET) >> 1; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + } + + if ((val == LSM6DSO_DRV_RDY) + && ((ctrl3_c.bdu == PROPERTY_DISABLE) + || (ctrl3_c.if_inc == PROPERTY_DISABLE)) && (ret == 0)) + { + ctrl3_c.bdu = PROPERTY_ENABLE; + ctrl3_c.if_inc = PROPERTY_ENABLE; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + } + + return ret; +} + +/** + * @brief Configures the bus operating mode.[set] + * + * @param ctx communication interface handler. Use NULL to ignore + * this interface.(ptr) + * @param aux_ctx auxiliary communication interface handler. Use NULL + * to ignore this interface.(ptr) + * @param val configures the bus operating mode for both the + * main and the auxiliary interface. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_bus_mode_set(stmdev_ctx_t *ctx, stmdev_ctx_t *aux_ctx, + lsm6dso_bus_mode_t val) +{ + lsm6dso_ctrl1_ois_t ctrl1_ois; + lsm6dso_i3c_bus_avb_t i3c_bus_avb; + lsm6dso_ctrl9_xl_t ctrl9_xl; + lsm6dso_ctrl3_c_t ctrl3_c; + lsm6dso_ctrl4_c_t ctrl4_c; + uint8_t bit_val; + int32_t ret; + + ret = 0; + + if (aux_ctx != NULL) + { + ret = lsm6dso_read_reg(aux_ctx, LSM6DSO_CTRL1_OIS, + (uint8_t *)&ctrl1_ois, 1); + bit_val = ((uint8_t)val.aux_bus_md & 0x04U) >> 2; + + if ((ret == 0) && (ctrl1_ois.sim_ois != bit_val)) + { + ctrl1_ois.sim_ois = bit_val; + ret = lsm6dso_write_reg(aux_ctx, LSM6DSO_CTRL1_OIS, + (uint8_t *)&ctrl1_ois, 1); + } + } + + if (ctx != NULL) + { + if (ret == 0) + { + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL9_XL, + (uint8_t *)&ctrl9_xl, 1); + } + + bit_val = ((uint8_t)val.ui_bus_md & 0x04U) >> 2; + + if ((ret == 0) && (ctrl9_xl.i3c_disable != bit_val)) + { + ctrl9_xl.i3c_disable = bit_val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL9_XL, + (uint8_t *)&ctrl9_xl, 1); + } + + if (ret == 0) + { + ret = lsm6dso_read_reg(ctx, LSM6DSO_I3C_BUS_AVB, + (uint8_t *)&i3c_bus_avb, 1); + } + + bit_val = ((uint8_t)val.ui_bus_md & 0x30U) >> 4; + + if ((ret == 0) && (i3c_bus_avb.i3c_bus_avb_sel != bit_val)) + { + i3c_bus_avb.i3c_bus_avb_sel = bit_val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_I3C_BUS_AVB, + (uint8_t *)&i3c_bus_avb, 1); + } + + if (ret == 0) + { + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL4_C, + (uint8_t *)&ctrl4_c, 1); + } + + bit_val = ((uint8_t)val.ui_bus_md & 0x02U) >> 1; + + if ((ret == 0) && (ctrl4_c.i2c_disable != bit_val)) + { + ctrl4_c.i2c_disable = bit_val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL4_C, + (uint8_t *)&ctrl4_c, 1); + } + + if (ret == 0) + { + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL3_C, + (uint8_t *)&ctrl3_c, 1); + } + + bit_val = (uint8_t)val.ui_bus_md & 0x01U; + + if ((ret == 0) && (ctrl3_c.sim != bit_val)) + { + ctrl3_c.sim = bit_val; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL3_C, + (uint8_t *)&ctrl3_c, 1); + } + } + + return ret; +} + +/** + * @brief Get the bus operating mode.[get] + * + * @param ctx communication interface handler. Use NULL to ignore + * this interface.(ptr) + * @param aux_ctx auxiliary communication interface handler. Use NULL + * to ignore this interface.(ptr) + * @param val retrieves the bus operating mode for both the main + * and the auxiliary interface.(ptr) + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_bus_mode_get(stmdev_ctx_t *ctx, stmdev_ctx_t *aux_ctx, + lsm6dso_bus_mode_t *val) +{ + lsm6dso_ctrl1_ois_t ctrl1_ois; + lsm6dso_i3c_bus_avb_t i3c_bus_avb; + lsm6dso_ctrl9_xl_t ctrl9_xl; + lsm6dso_ctrl3_c_t ctrl3_c; + lsm6dso_ctrl4_c_t ctrl4_c; + int32_t ret = 0; + + if (aux_ctx != NULL) + { + ret = lsm6dso_read_reg(aux_ctx, LSM6DSO_CTRL1_OIS, + (uint8_t *)&ctrl1_ois, 1); + + switch (ctrl1_ois.sim_ois) + { + case LSM6DSO_SPI_4W_AUX: + val->aux_bus_md = LSM6DSO_SPI_4W_AUX; + break; + + case LSM6DSO_SPI_3W_AUX: + val->aux_bus_md = LSM6DSO_SPI_3W_AUX; + break; + + default: + val->aux_bus_md = LSM6DSO_SPI_4W_AUX; + break; + } + } + + if (ctx != NULL) + { + if (ret == 0) + { + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL9_XL, + (uint8_t *)&ctrl9_xl, 1); + } + + if (ret == 0) + { + ret = lsm6dso_read_reg(ctx, LSM6DSO_I3C_BUS_AVB, + (uint8_t *)&i3c_bus_avb, 1); + } + + if (ret == 0) + { + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL4_C, + (uint8_t *)&ctrl4_c, 1); + } + + if (ret == 0) + { + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL3_C, + (uint8_t *)&ctrl3_c, 1); + + switch ((i3c_bus_avb.i3c_bus_avb_sel << 4) & + (ctrl9_xl.i3c_disable << 2) & + (ctrl4_c.i2c_disable << 1) & ctrl3_c.sim) + { + case LSM6DSO_SEL_BY_HW: + val->ui_bus_md = LSM6DSO_SEL_BY_HW; + break; + + case LSM6DSO_SPI_4W: + val->ui_bus_md = LSM6DSO_SPI_4W; + break; + + case LSM6DSO_SPI_3W: + val->ui_bus_md = LSM6DSO_SPI_3W; + break; + + case LSM6DSO_I2C: + val->ui_bus_md = LSM6DSO_I2C; + break; + + case LSM6DSO_I3C_T_50us: + val->ui_bus_md = LSM6DSO_I3C_T_50us; + break; + + case LSM6DSO_I3C_T_2us: + val->ui_bus_md = LSM6DSO_I3C_T_2us; + break; + + case LSM6DSO_I3C_T_1ms: + val->ui_bus_md = LSM6DSO_I3C_T_1ms; + break; + + case LSM6DSO_I3C_T_25ms: + val->ui_bus_md = LSM6DSO_I3C_T_25ms; + break; + + default: + val->ui_bus_md = LSM6DSO_SEL_BY_HW; + break; + } + } + } + + return ret; +} + +/** + * @brief Get the status of the device.[get] + * + * @param ctx communication interface handler. Use NULL to ignore + * this interface.(ptr) + * @param aux_ctx auxiliary communication interface handler. Use NULL + * to ignore this interface.(ptr) + * @param val the status of the device.(ptr) + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_status_get(stmdev_ctx_t *ctx, stmdev_ctx_t *aux_ctx, + lsm6dso_status_t *val) +{ + lsm6dso_status_spiaux_t status_spiaux; + lsm6dso_status_reg_t status_reg; + lsm6dso_ctrl3_c_t ctrl3_c; + int32_t ret; + ret = 0; + + if (aux_ctx != NULL) + { + ret = lsm6dso_read_reg(aux_ctx, LSM6DSO_STATUS_SPIAUX, + (uint8_t *)&status_spiaux, 1); + val->ois_drdy_xl = status_spiaux.xlda; + val->ois_drdy_g = status_spiaux.gda; + val->ois_gyro_settling = status_spiaux.gyro_settling; + } + + if (ctx != NULL) + { + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + val->sw_reset = ctrl3_c.sw_reset; + val->boot = ctrl3_c.boot; + + if ((ret == 0) && (ctrl3_c.sw_reset == PROPERTY_DISABLE) && + (ctrl3_c.boot == PROPERTY_DISABLE)) + { + ret = lsm6dso_read_reg(ctx, LSM6DSO_STATUS_REG, + (uint8_t *)&status_reg, 1); + val->drdy_xl = status_reg.xlda; + val->drdy_g = status_reg.gda; + val->drdy_temp = status_reg.tda; + } + } + + return ret; +} + +/** + * @brief Electrical pin configuration.[set] + * + * @param ctx communication interface handler.(ptr) + * @param val the electrical settings for the configurable + * pins. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_pin_conf_set(stmdev_ctx_t *ctx, + lsm6dso_pin_conf_t val) +{ + lsm6dso_i3c_bus_avb_t i3c_bus_avb; + lsm6dso_pin_ctrl_t pin_ctrl; + lsm6dso_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_PIN_CTRL, (uint8_t *)&pin_ctrl, 1); + pin_ctrl.ois_pu_dis = ~val.aux_sdo_ocs_pull_up; + pin_ctrl.sdo_pu_en = val.sdo_sa0_pull_up; + ret += lsm6dso_write_reg(ctx, LSM6DSO_PIN_CTRL, (uint8_t *)&pin_ctrl, 1); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + ctrl3_c.pp_od = ~val.int1_int2_push_pull; + ret += lsm6dso_write_reg(ctx, LSM6DSO_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_I3C_BUS_AVB, (uint8_t *)&i3c_bus_avb, 1); + i3c_bus_avb.pd_dis_int1 = ~val.int1_pull_down; + ret += lsm6dso_write_reg(ctx, LSM6DSO_I3C_BUS_AVB, (uint8_t *)&i3c_bus_avb, 1); + + return ret; +} + +/** + * @brief Electrical pin configuration.[get] + * + * @param ctx communication interface handler.(ptr) + * @param val the electrical settings for the configurable + * pins.(ptr) + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_pin_conf_get(stmdev_ctx_t *ctx, + lsm6dso_pin_conf_t *val) +{ + lsm6dso_i3c_bus_avb_t i3c_bus_avb; + lsm6dso_pin_ctrl_t pin_ctrl; + lsm6dso_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_PIN_CTRL, (uint8_t *)&pin_ctrl, 1); + if (ret != 0) { return ret; } + + val->aux_sdo_ocs_pull_up = ~pin_ctrl.ois_pu_dis; + val->aux_sdo_ocs_pull_up = pin_ctrl.sdo_pu_en; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + if (ret != 0) { return ret; } + + val->int1_int2_push_pull = ~ctrl3_c.pp_od; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_I3C_BUS_AVB, (uint8_t *)&i3c_bus_avb, 1); + val->int1_pull_down = ~i3c_bus_avb.pd_dis_int1; + + return ret; +} + +/** + * @brief Interrupt pins hardware signal configuration.[set] + * + * @param ctx communication interface handler.(ptr) + * @param val the pins hardware signal settings. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_interrupt_mode_set(stmdev_ctx_t *ctx, + lsm6dso_int_mode_t val) +{ + lsm6dso_tap_cfg0_t tap_cfg0; + lsm6dso_page_rw_t page_rw; + lsm6dso_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + ctrl3_c.h_lactive = val.active_low; + ret += lsm6dso_write_reg(ctx, LSM6DSO_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_TAP_CFG0, (uint8_t *) &tap_cfg0, 1); + tap_cfg0.lir = val.base_latched; + tap_cfg0.int_clr_on_read = val.base_latched | val.emb_latched; + ret += lsm6dso_write_reg(ctx, LSM6DSO_TAP_CFG0, (uint8_t *) &tap_cfg0, 1); + if (ret != 0) { return ret; } + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_EMBEDDED_FUNC_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_PAGE_RW, (uint8_t *) &page_rw, 1); + page_rw.emb_func_lir = val.emb_latched; + ret += lsm6dso_write_reg(ctx, LSM6DSO_PAGE_RW, (uint8_t *) &page_rw, 1); + + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @brief Interrupt pins hardware signal configuration.[get] + * + * @param ctx communication interface handler.(ptr) + * @param val the pins hardware signal settings.(ptr) + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_interrupt_mode_get(stmdev_ctx_t *ctx, + lsm6dso_int_mode_t *val) +{ + lsm6dso_tap_cfg0_t tap_cfg0; + lsm6dso_page_rw_t page_rw; + lsm6dso_ctrl3_c_t ctrl3_c; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL3_C, (uint8_t *)&ctrl3_c, 1); + if (ret != 0) { return ret; } + + val->active_low = ctrl3_c.h_lactive; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_TAP_CFG0, (uint8_t *) &tap_cfg0, 1); + if (ret != 0) { return ret; } + + val->base_latched = (tap_cfg0.lir & tap_cfg0.int_clr_on_read); + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_EMBEDDED_FUNC_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_PAGE_RW, (uint8_t *) &page_rw, 1); + val->emb_latched = (page_rw.emb_func_lir & tap_cfg0.int_clr_on_read); + + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @brief Route interrupt signals on int1 pin.[set] + * + * @param ctx communication interface handler.(ptr) + * @param val the signals to route on int1 pin. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_pin_int1_route_set(stmdev_ctx_t *ctx, + lsm6dso_pin_int1_route_t val) +{ + lsm6dso_pin_int2_route_t pin_int2_route; + lsm6dso_emb_func_int1_t emb_func_int1; + lsm6dso_fsm_int1_a_t fsm_int1_a; + lsm6dso_fsm_int1_b_t fsm_int1_b; + lsm6dso_int1_ctrl_t int1_ctrl = {0}; + lsm6dso_int2_ctrl_t int2_ctrl; + lsm6dso_tap_cfg2_t tap_cfg2; + lsm6dso_md2_cfg_t md2_cfg; + lsm6dso_md1_cfg_t md1_cfg; + lsm6dso_ctrl4_c_t ctrl4_c; + int32_t ret; + + /* INT1_CTRL */ + int1_ctrl.int1_drdy_xl = val.drdy_xl; + int1_ctrl.int1_drdy_g = val.drdy_g; + int1_ctrl.int1_boot = val.boot; + 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.fifo_bdr; + int1_ctrl.den_drdy_flag = val.den_flag; + ret = lsm6dso_write_reg(ctx, LSM6DSO_INT1_CTRL, (uint8_t *)&int1_ctrl, 1); + if (ret != 0) { return ret; } + + /* DRDY for temperature and/or timestamp */ + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + if (ret != 0) { return ret; } + + if ((val.drdy_temp | val.timestamp) != PROPERTY_DISABLE) + { + ctrl4_c.int2_on_int1 = PROPERTY_ENABLE; + } + else + { + ctrl4_c.int2_on_int1 = PROPERTY_DISABLE; + } + + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_INT2_CTRL, (uint8_t *)&int2_ctrl, 1); + int2_ctrl.int2_drdy_temp = val.drdy_temp; + ret += lsm6dso_write_reg(ctx, LSM6DSO_INT2_CTRL, (uint8_t *)&int2_ctrl, 1); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_MD2_CFG, (uint8_t *)&md2_cfg, 1); + md2_cfg.int2_timestamp = val.timestamp; + ret += lsm6dso_write_reg(ctx, LSM6DSO_MD2_CFG, (uint8_t *)&md2_cfg, 1); + if (ret != 0) { return ret; } + + /* emmbedded and FSM events */ + emb_func_int1.not_used_01 = 0; + emb_func_int1.int1_step_detector = val.step_detector; + emb_func_int1.int1_tilt = val.tilt; + emb_func_int1.int1_sig_mot = val.sig_mot; + emb_func_int1.not_used_02 = 0; + emb_func_int1.int1_fsm_lc = val.fsm_lc; + fsm_int1_a.int1_fsm1 = val.fsm1; + fsm_int1_a.int1_fsm2 = val.fsm2; + fsm_int1_a.int1_fsm3 = val.fsm3; + fsm_int1_a.int1_fsm4 = val.fsm4; + fsm_int1_a.int1_fsm5 = val.fsm5; + fsm_int1_a.int1_fsm6 = val.fsm6; + fsm_int1_a.int1_fsm7 = val.fsm7; + fsm_int1_a.int1_fsm8 = val.fsm8; + fsm_int1_b.int1_fsm9 = val.fsm9 ; + fsm_int1_b.int1_fsm10 = val.fsm10; + fsm_int1_b.int1_fsm11 = val.fsm11; + fsm_int1_b.int1_fsm12 = val.fsm12; + fsm_int1_b.int1_fsm13 = val.fsm13; + fsm_int1_b.int1_fsm14 = val.fsm14; + fsm_int1_b.int1_fsm15 = val.fsm15; + fsm_int1_b.int1_fsm16 = val.fsm16; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_EMBEDDED_FUNC_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_write_reg(ctx, LSM6DSO_EMB_FUNC_INT1, (uint8_t *)&emb_func_int1, 1); + ret += lsm6dso_write_reg(ctx, LSM6DSO_FSM_INT1_A, (uint8_t *)&fsm_int1_a, 1); + ret += lsm6dso_write_reg(ctx, LSM6DSO_FSM_INT1_B, (uint8_t *)&fsm_int1_b, 1); + + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + if (ret != 0) { return ret; } + + /* MD1_CFG */ + md1_cfg.int1_shub = val.sh_endop; + md1_cfg.int1_6d = val.six_d; + md1_cfg.int1_double_tap = val.double_tap; + md1_cfg.int1_ff = val.free_fall; + md1_cfg.int1_wu = val.wake_up; + md1_cfg.int1_single_tap = val.single_tap; + md1_cfg.int1_sleep_change = val.sleep_change; + + if ((emb_func_int1.int1_fsm_lc + | emb_func_int1.int1_sig_mot + | emb_func_int1.int1_step_detector + | emb_func_int1.int1_tilt + | fsm_int1_a.int1_fsm1 + | fsm_int1_a.int1_fsm2 + | fsm_int1_a.int1_fsm3 + | fsm_int1_a.int1_fsm4 + | fsm_int1_a.int1_fsm5 + | fsm_int1_a.int1_fsm6 + | fsm_int1_a.int1_fsm7 + | fsm_int1_a.int1_fsm8 + | fsm_int1_b.int1_fsm9 + | fsm_int1_b.int1_fsm10 + | fsm_int1_b.int1_fsm11 + | fsm_int1_b.int1_fsm12 + | fsm_int1_b.int1_fsm13 + | fsm_int1_b.int1_fsm14 + | fsm_int1_b.int1_fsm15 + | fsm_int1_b.int1_fsm16) != PROPERTY_DISABLE) + { + md1_cfg.int1_emb_func = PROPERTY_ENABLE; + } + else + { + md1_cfg.int1_emb_func = PROPERTY_DISABLE; + } + + ret = lsm6dso_write_reg(ctx, LSM6DSO_MD1_CFG, (uint8_t *)&md1_cfg, 1); + if (ret != 0) { return ret; } + + /* set interrupts_enable = 1 in TAP_CFG2 if it is the case */ + ret = lsm6dso_read_reg(ctx, LSM6DSO_TAP_CFG2, (uint8_t *) &tap_cfg2, 1); + ret += lsm6dso_pin_int2_route_get(ctx, NULL, &pin_int2_route); + if (ret != 0) { return ret; } + + if ((pin_int2_route.fifo_bdr + | pin_int2_route.drdy_g + | pin_int2_route.drdy_temp + | pin_int2_route.drdy_xl + | pin_int2_route.fifo_full + | pin_int2_route.fifo_ovr + | pin_int2_route.fifo_th + | pin_int2_route.six_d + | pin_int2_route.double_tap + | pin_int2_route.free_fall + | pin_int2_route.wake_up + | pin_int2_route.single_tap + | pin_int2_route.sleep_change + | int1_ctrl.den_drdy_flag + | int1_ctrl.int1_boot + | int1_ctrl.int1_cnt_bdr + | int1_ctrl.int1_drdy_g + | int1_ctrl.int1_drdy_xl + | int1_ctrl.int1_fifo_full + | int1_ctrl.int1_fifo_ovr + | int1_ctrl.int1_fifo_th + | md1_cfg.int1_shub + | md1_cfg.int1_6d + | md1_cfg.int1_double_tap + | md1_cfg.int1_ff + | md1_cfg.int1_wu + | md1_cfg.int1_single_tap + | md1_cfg.int1_sleep_change) != PROPERTY_DISABLE) + { + tap_cfg2.interrupts_enable = PROPERTY_ENABLE; + } + else + { + tap_cfg2.interrupts_enable = PROPERTY_DISABLE; + } + + ret = lsm6dso_write_reg(ctx, LSM6DSO_TAP_CFG2, (uint8_t *) &tap_cfg2, 1); + + return ret; +} + +/** + * @brief Route interrupt signals on int1 pin.[get] + * + * @param ctx communication interface handler.(ptr) + * @param val the signals that are routed on int1 pin.(ptr) + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_pin_int1_route_get(stmdev_ctx_t *ctx, + lsm6dso_pin_int1_route_t *val) +{ + lsm6dso_emb_func_int1_t emb_func_int1; + lsm6dso_fsm_int1_a_t fsm_int1_a; + lsm6dso_fsm_int1_b_t fsm_int1_b; + lsm6dso_int1_ctrl_t int1_ctrl; + lsm6dso_int2_ctrl_t int2_ctrl; + lsm6dso_md2_cfg_t md2_cfg; + lsm6dso_md1_cfg_t md1_cfg; + lsm6dso_ctrl4_c_t ctrl4_c; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_EMBEDDED_FUNC_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_EMB_FUNC_INT1, (uint8_t *)&emb_func_int1, 1); + ret += lsm6dso_read_reg(ctx, LSM6DSO_FSM_INT1_A, (uint8_t *)&fsm_int1_a, 1); + ret += lsm6dso_read_reg(ctx, LSM6DSO_FSM_INT1_B, (uint8_t *)&fsm_int1_b, 1); + + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_INT1_CTRL, (uint8_t *)&int1_ctrl, 1); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_MD1_CFG, (uint8_t *)&md1_cfg, 1); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + if (ret != 0) { return ret; } + + if (ctrl4_c.int2_on_int1 == PROPERTY_ENABLE) + { + ret = lsm6dso_read_reg(ctx, LSM6DSO_INT2_CTRL, (uint8_t *)&int2_ctrl, 1); + val->drdy_temp = int2_ctrl.int2_drdy_temp; + + ret += lsm6dso_read_reg(ctx, LSM6DSO_MD2_CFG, (uint8_t *)&md2_cfg, 1); + val->timestamp = md2_cfg.int2_timestamp; + } + else + { + val->drdy_temp = PROPERTY_DISABLE; + val->timestamp = PROPERTY_DISABLE; + } + if (ret != 0) { return ret; } + + val->drdy_xl = int1_ctrl.int1_drdy_xl; + val->drdy_g = int1_ctrl.int1_drdy_g; + val->boot = int1_ctrl.int1_boot; + 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->fifo_bdr = int1_ctrl.int1_cnt_bdr; + val->den_flag = int1_ctrl.den_drdy_flag; + val->sh_endop = md1_cfg.int1_shub; + val->six_d = md1_cfg.int1_6d; + val->double_tap = md1_cfg.int1_double_tap; + val->free_fall = md1_cfg.int1_ff; + val->wake_up = md1_cfg.int1_wu; + val->single_tap = md1_cfg.int1_single_tap; + val->sleep_change = md1_cfg.int1_sleep_change; + val->step_detector = emb_func_int1.int1_step_detector; + val->tilt = emb_func_int1.int1_tilt; + val->sig_mot = emb_func_int1.int1_sig_mot; + val->fsm_lc = emb_func_int1.int1_fsm_lc; + val->fsm1 = fsm_int1_a.int1_fsm1; + val->fsm2 = fsm_int1_a.int1_fsm2; + val->fsm3 = fsm_int1_a.int1_fsm3; + val->fsm4 = fsm_int1_a.int1_fsm4; + val->fsm5 = fsm_int1_a.int1_fsm5; + val->fsm6 = fsm_int1_a.int1_fsm6; + val->fsm7 = fsm_int1_a.int1_fsm7; + val->fsm8 = fsm_int1_a.int1_fsm8; + val->fsm9 = fsm_int1_b.int1_fsm9; + val->fsm10 = fsm_int1_b.int1_fsm10; + val->fsm11 = fsm_int1_b.int1_fsm11; + val->fsm12 = fsm_int1_b.int1_fsm12; + val->fsm13 = fsm_int1_b.int1_fsm13; + val->fsm14 = fsm_int1_b.int1_fsm14; + val->fsm15 = fsm_int1_b.int1_fsm15; + val->fsm16 = fsm_int1_b.int1_fsm16; + + return ret; +} + +/** + * @brief Route interrupt signals on int2 pin.[set] + * + * @param ctx communication interface handler. Use NULL to ignore + * this interface.(ptr) + * @param aux_ctx auxiliary communication interface handler. Use NULL + * to ignore this interface.(ptr) + * @param val the signals to route on int2 pin. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_pin_int2_route_set(stmdev_ctx_t *ctx, + stmdev_ctx_t *aux_ctx, + lsm6dso_pin_int2_route_t val) +{ + lsm6dso_pin_int1_route_t pin_int1_route; + lsm6dso_emb_func_int2_t emb_func_int2; + lsm6dso_fsm_int2_a_t fsm_int2_a; + lsm6dso_fsm_int2_b_t fsm_int2_b; + lsm6dso_int2_ctrl_t int2_ctrl; + lsm6dso_tap_cfg2_t tap_cfg2; + lsm6dso_md2_cfg_t md2_cfg; + lsm6dso_ctrl4_c_t ctrl4_c; + lsm6dso_int_ois_t int_ois; + int32_t ret; + ret = 0; + + if (aux_ctx != NULL) + { + ret = lsm6dso_read_reg(aux_ctx, LSM6DSO_INT_OIS, (uint8_t *)&int_ois, 1); + int_ois.int2_drdy_ois = val.drdy_ois; + ret += lsm6dso_write_reg(aux_ctx, LSM6DSO_INT_OIS, (uint8_t *)&int_ois, 1); + if (ret != 0) { return ret; } + } + + if (ctx != NULL) + { + int2_ctrl.int2_drdy_xl = val.drdy_xl; + int2_ctrl.int2_drdy_g = val.drdy_g; + int2_ctrl.int2_drdy_temp = val.drdy_temp; + 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.fifo_bdr; + int2_ctrl.not_used_01 = 0; + ret = lsm6dso_write_reg(ctx, LSM6DSO_INT2_CTRL, (uint8_t *)&int2_ctrl, 1); + if (ret != 0) { return ret; } + + md2_cfg.int2_timestamp = val.timestamp; + md2_cfg.int2_6d = val.six_d; + md2_cfg.int2_double_tap = val.double_tap; + md2_cfg.int2_ff = val.free_fall; + md2_cfg.int2_wu = val.wake_up; + md2_cfg.int2_single_tap = val.single_tap; + md2_cfg.int2_sleep_change = val.sleep_change; + emb_func_int2.not_used_01 = 0; + emb_func_int2. int2_step_detector = val.step_detector; + emb_func_int2.int2_tilt = val.tilt; + emb_func_int2.int2_sig_mot = val.sig_mot; + emb_func_int2.not_used_02 = 0; + emb_func_int2.int2_fsm_lc = val.fsm_lc; + fsm_int2_a.int2_fsm1 = val.fsm1; + fsm_int2_a.int2_fsm2 = val.fsm2; + fsm_int2_a.int2_fsm3 = val.fsm3; + fsm_int2_a.int2_fsm4 = val.fsm4; + fsm_int2_a.int2_fsm5 = val.fsm5; + fsm_int2_a.int2_fsm6 = val.fsm6; + fsm_int2_a.int2_fsm7 = val.fsm7; + fsm_int2_a.int2_fsm8 = val.fsm8; + fsm_int2_b.int2_fsm9 = val.fsm9 ; + fsm_int2_b.int2_fsm10 = val.fsm10; + fsm_int2_b.int2_fsm11 = val.fsm11; + fsm_int2_b.int2_fsm12 = val.fsm12; + fsm_int2_b.int2_fsm13 = val.fsm13; + fsm_int2_b.int2_fsm14 = val.fsm14; + fsm_int2_b.int2_fsm15 = val.fsm15; + fsm_int2_b.int2_fsm16 = val.fsm16; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + + if ((val.drdy_temp | val.timestamp) != PROPERTY_DISABLE) + { + ctrl4_c.int2_on_int1 = PROPERTY_DISABLE; + } + + ret += lsm6dso_write_reg(ctx, LSM6DSO_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + if (ret != 0) { return ret; } + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_EMBEDDED_FUNC_BANK); + + ret += lsm6dso_write_reg(ctx, LSM6DSO_EMB_FUNC_INT2, (uint8_t *)&emb_func_int2, 1); + ret += lsm6dso_write_reg(ctx, LSM6DSO_FSM_INT2_A, (uint8_t *)&fsm_int2_a, 1); + ret += lsm6dso_write_reg(ctx, LSM6DSO_FSM_INT2_B, (uint8_t *)&fsm_int2_b, 1); + + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + if (ret != 0) { return ret; } + + if ((emb_func_int2.int2_fsm_lc + | emb_func_int2.int2_sig_mot + | emb_func_int2.int2_step_detector + | emb_func_int2.int2_tilt + | fsm_int2_a.int2_fsm1 + | fsm_int2_a.int2_fsm2 + | fsm_int2_a.int2_fsm3 + | fsm_int2_a.int2_fsm4 + | fsm_int2_a.int2_fsm5 + | fsm_int2_a.int2_fsm6 + | fsm_int2_a.int2_fsm7 + | fsm_int2_a.int2_fsm8 + | fsm_int2_b.int2_fsm9 + | fsm_int2_b.int2_fsm10 + | fsm_int2_b.int2_fsm11 + | fsm_int2_b.int2_fsm12 + | fsm_int2_b.int2_fsm13 + | fsm_int2_b.int2_fsm14 + | fsm_int2_b.int2_fsm15 + | fsm_int2_b.int2_fsm16) != PROPERTY_DISABLE) + { + md2_cfg.int2_emb_func = PROPERTY_ENABLE; + } + else + { + md2_cfg.int2_emb_func = PROPERTY_DISABLE; + } + + ret = lsm6dso_write_reg(ctx, LSM6DSO_MD2_CFG, (uint8_t *)&md2_cfg, 1); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_TAP_CFG2, (uint8_t *) &tap_cfg2, 1); + ret += lsm6dso_pin_int1_route_get(ctx, &pin_int1_route); + + if ((val.fifo_bdr + | val.drdy_g + | val.drdy_temp + | val.drdy_xl + | val.fifo_full + | val.fifo_ovr + | val.fifo_th + | val.six_d + | val.double_tap + | val.free_fall + | val.wake_up + | val.single_tap + | val.sleep_change + | pin_int1_route.den_flag + | pin_int1_route.boot + | pin_int1_route.fifo_bdr + | pin_int1_route.drdy_g + | pin_int1_route.drdy_xl + | pin_int1_route.fifo_full + | pin_int1_route.fifo_ovr + | pin_int1_route.fifo_th + | pin_int1_route.six_d + | pin_int1_route.double_tap + | pin_int1_route.free_fall + | pin_int1_route.wake_up + | pin_int1_route.single_tap + | pin_int1_route.sleep_change) != PROPERTY_DISABLE) + { + tap_cfg2.interrupts_enable = PROPERTY_ENABLE; + } + else + { + tap_cfg2.interrupts_enable = PROPERTY_DISABLE; + } + + ret += lsm6dso_write_reg(ctx, LSM6DSO_TAP_CFG2, (uint8_t *) &tap_cfg2, 1); + } + + return ret; +} + +/** + * @brief Route interrupt signals on int2 pin.[get] + * + * @param ctx communication interface handler. Use NULL to ignore + * this interface.(ptr) + * @param aux_ctx auxiliary communication interface handler. Use NULL + * to ignore this interface.(ptr) + * @param val the signals that are routed on int2 pin.(ptr) + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_pin_int2_route_get(stmdev_ctx_t *ctx, + stmdev_ctx_t *aux_ctx, + lsm6dso_pin_int2_route_t *val) +{ + lsm6dso_emb_func_int2_t emb_func_int2; + lsm6dso_fsm_int2_a_t fsm_int2_a; + lsm6dso_fsm_int2_b_t fsm_int2_b; + lsm6dso_int2_ctrl_t int2_ctrl; + lsm6dso_md2_cfg_t md2_cfg; + lsm6dso_ctrl4_c_t ctrl4_c; + lsm6dso_int_ois_t int_ois; + int32_t ret; + ret = 0; + + if (aux_ctx != NULL) + { + ret = lsm6dso_read_reg(aux_ctx, LSM6DSO_INT_OIS, + (uint8_t *)&int_ois, 1); + val->drdy_ois = int_ois.int2_drdy_ois; + } + + if (ctx != NULL) + { + if (ret == 0) + { + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_EMBEDDED_FUNC_BANK); + } + + if (ret == 0) + { + ret = lsm6dso_read_reg(ctx, LSM6DSO_EMB_FUNC_INT2, + (uint8_t *)&emb_func_int2, 1); + } + + if (ret == 0) + { + ret = lsm6dso_read_reg(ctx, LSM6DSO_FSM_INT2_A, + (uint8_t *)&fsm_int2_a, 1); + } + + if (ret == 0) + { + ret = lsm6dso_read_reg(ctx, LSM6DSO_FSM_INT2_B, + (uint8_t *)&fsm_int2_b, 1); + } + + if (ret == 0) + { + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + } + + if (ret == 0) + { + ret = lsm6dso_read_reg(ctx, LSM6DSO_INT2_CTRL, + (uint8_t *)&int2_ctrl, 1); + } + + if (ret == 0) + { + ret = lsm6dso_read_reg(ctx, LSM6DSO_MD2_CFG, + (uint8_t *)&md2_cfg, 1); + } + + if (ret == 0) + { + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL4_C, (uint8_t *)&ctrl4_c, 1); + } + + if (ctrl4_c.int2_on_int1 == PROPERTY_DISABLE) + { + if (ret == 0) + { + ret = lsm6dso_read_reg(ctx, LSM6DSO_INT2_CTRL, + (uint8_t *)&int2_ctrl, 1); + val->drdy_temp = int2_ctrl.int2_drdy_temp; + } + + if (ret == 0) + { + ret = lsm6dso_read_reg(ctx, LSM6DSO_MD2_CFG, (uint8_t *)&md2_cfg, 1); + val->timestamp = md2_cfg.int2_timestamp; + } + } + + else + { + val->drdy_temp = PROPERTY_DISABLE; + val->timestamp = PROPERTY_DISABLE; + } + + val->drdy_xl = int2_ctrl.int2_drdy_xl; + val->drdy_g = int2_ctrl.int2_drdy_g; + val->drdy_temp = int2_ctrl.int2_drdy_temp; + 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->fifo_bdr = int2_ctrl.int2_cnt_bdr; + val->timestamp = md2_cfg.int2_timestamp; + val->six_d = md2_cfg.int2_6d; + val->double_tap = md2_cfg.int2_double_tap; + val->free_fall = md2_cfg.int2_ff; + val->wake_up = md2_cfg.int2_wu; + val->single_tap = md2_cfg.int2_single_tap; + val->sleep_change = md2_cfg.int2_sleep_change; + val->step_detector = emb_func_int2. int2_step_detector; + val->tilt = emb_func_int2.int2_tilt; + val->fsm_lc = emb_func_int2.int2_fsm_lc; + val->fsm1 = fsm_int2_a.int2_fsm1; + val->fsm2 = fsm_int2_a.int2_fsm2; + val->fsm3 = fsm_int2_a.int2_fsm3; + val->fsm4 = fsm_int2_a.int2_fsm4; + val->fsm5 = fsm_int2_a.int2_fsm5; + val->fsm6 = fsm_int2_a.int2_fsm6; + val->fsm7 = fsm_int2_a.int2_fsm7; + val->fsm8 = fsm_int2_a.int2_fsm8; + val->fsm9 = fsm_int2_b.int2_fsm9; + val->fsm10 = fsm_int2_b.int2_fsm10; + val->fsm11 = fsm_int2_b.int2_fsm11; + val->fsm12 = fsm_int2_b.int2_fsm12; + val->fsm13 = fsm_int2_b.int2_fsm13; + val->fsm14 = fsm_int2_b.int2_fsm14; + val->fsm15 = fsm_int2_b.int2_fsm15; + val->fsm16 = fsm_int2_b.int2_fsm16; + } + + return ret; +} + +/** + * @brief Get the status of all the interrupt sources.[get] + * + * @param ctx communication interface handler.(ptr) + * @param val the status of all the interrupt sources.(ptr) + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_all_sources_get(stmdev_ctx_t *ctx, + lsm6dso_all_sources_t *val) +{ + lsm6dso_emb_func_status_mainpage_t emb_func_status_mainpage; + lsm6dso_status_master_mainpage_t status_master_mainpage; + lsm6dso_fsm_status_a_mainpage_t fsm_status_a_mainpage; + lsm6dso_fsm_status_b_mainpage_t fsm_status_b_mainpage; + lsm6dso_fifo_status1_t fifo_status1; + lsm6dso_fifo_status2_t fifo_status2; + lsm6dso_all_int_src_t all_int_src; + lsm6dso_wake_up_src_t wake_up_src; + lsm6dso_status_reg_t status_reg; + lsm6dso_tap_src_t tap_src; + lsm6dso_d6d_src_t d6d_src; + uint8_t reg[5]; + int32_t ret; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_ALL_INT_SRC, reg, 5); + if (ret != 0) { return ret; } + + bytecpy((uint8_t *)&all_int_src, ®[0]); + bytecpy((uint8_t *)&wake_up_src, ®[1]); + bytecpy((uint8_t *)&tap_src, ®[2]); + bytecpy((uint8_t *)&d6d_src, ®[3]); + bytecpy((uint8_t *)&status_reg, ®[4]); + val->timestamp = all_int_src.timestamp_endcount; + val->wake_up_z = wake_up_src.z_wu; + val->wake_up_y = wake_up_src.y_wu; + val->wake_up_x = wake_up_src.x_wu; + val->wake_up = wake_up_src.wu_ia; + val->sleep_state = wake_up_src.sleep_state; + val->free_fall = wake_up_src.ff_ia; + val->sleep_change = wake_up_src.sleep_change_ia; + 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_xl = d6d_src.xl; + val->six_d_xh = d6d_src.xh; + val->six_d_yl = d6d_src.yl; + val->six_d_yh = d6d_src.yh; + val->six_d_zl = d6d_src.zl; + val->six_d_zh = d6d_src.zh; + val->six_d = d6d_src.d6d_ia; + val->den_flag = d6d_src.den_drdy; + val->drdy_xl = status_reg.xlda; + val->drdy_g = status_reg.gda; + val->drdy_temp = status_reg.tda; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_EMB_FUNC_STATUS_MAINPAGE, reg, 3); + if (ret != 0) { return ret; } + + bytecpy((uint8_t *)&emb_func_status_mainpage, ®[0]); + bytecpy((uint8_t *)&fsm_status_a_mainpage, ®[1]); + bytecpy((uint8_t *)&fsm_status_b_mainpage, ®[2]); + 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_a_mainpage.is_fsm1; + val->fsm2 = fsm_status_a_mainpage.is_fsm2; + val->fsm3 = fsm_status_a_mainpage.is_fsm3; + val->fsm4 = fsm_status_a_mainpage.is_fsm4; + val->fsm5 = fsm_status_a_mainpage.is_fsm5; + val->fsm6 = fsm_status_a_mainpage.is_fsm6; + val->fsm7 = fsm_status_a_mainpage.is_fsm7; + val->fsm8 = fsm_status_a_mainpage.is_fsm8; + val->fsm9 = fsm_status_b_mainpage.is_fsm9; + val->fsm10 = fsm_status_b_mainpage.is_fsm10; + val->fsm11 = fsm_status_b_mainpage.is_fsm11; + val->fsm12 = fsm_status_b_mainpage.is_fsm12; + val->fsm13 = fsm_status_b_mainpage.is_fsm13; + val->fsm14 = fsm_status_b_mainpage.is_fsm14; + val->fsm15 = fsm_status_b_mainpage.is_fsm15; + val->fsm16 = fsm_status_b_mainpage.is_fsm16; + + ret = lsm6dso_read_reg(ctx, LSM6DSO_STATUS_MASTER_MAINPAGE, reg, 3); + if (ret != 0) { return ret; } + + bytecpy((uint8_t *)&status_master_mainpage, ®[0]); + bytecpy((uint8_t *)&fifo_status1, ®[1]); + bytecpy((uint8_t *)&fifo_status2, ®[2]); + val->sh_endop = status_master_mainpage.sens_hub_endop; + val->sh_slave0_nack = status_master_mainpage.slave0_nack; + val->sh_slave1_nack = status_master_mainpage.slave1_nack; + val->sh_slave2_nack = status_master_mainpage.slave2_nack; + val->sh_slave3_nack = status_master_mainpage.slave3_nack; + val->sh_wr_once = status_master_mainpage.wr_once_done; + val->fifo_diff = (256U * fifo_status2.diff_fifo) + + fifo_status1.diff_fifo; + val->fifo_ovr_latched = fifo_status2.over_run_latched; + val->fifo_bdr = fifo_status2.counter_bdr_ia; + val->fifo_full = fifo_status2.fifo_full_ia; + val->fifo_ovr = fifo_status2.fifo_ovr_ia; + val->fifo_th = fifo_status2.fifo_wtm_ia; + + return ret; +} + +/** + * @brief Sensor conversion parameters selection.[set] + * + * @param ctx communication interface handler. Use NULL to ignore + * this interface.(ptr) + * @param aux_ctx auxiliary communication interface handler. Use NULL + * to ignore this interface.(ptr) + * @param val set the sensor conversion parameters by checking + * the constraints of the device.(ptr) + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_mode_set(stmdev_ctx_t *ctx, stmdev_ctx_t *aux_ctx, + lsm6dso_md_t *val) +{ + lsm6dso_func_cfg_access_t func_cfg_access; + lsm6dso_ctrl1_ois_t ctrl1_ois; + lsm6dso_ctrl2_ois_t ctrl2_ois; + lsm6dso_ctrl3_ois_t ctrl3_ois; + lsm6dso_ctrl1_xl_t ctrl1_xl; + lsm6dso_ctrl8_xl_t ctrl8_xl; + lsm6dso_ctrl2_g_t ctrl2_g; + lsm6dso_ctrl3_c_t ctrl3_c; + lsm6dso_ctrl4_c_t ctrl4_c; + lsm6dso_ctrl5_c_t ctrl5_c; + lsm6dso_ctrl6_c_t ctrl6_c; + lsm6dso_ctrl7_g_t ctrl7_g; + uint8_t xl_hm_mode; + uint8_t g_hm_mode; + uint8_t xl_ulp_en; + uint8_t odr_gy; + uint8_t odr_xl; + uint8_t reg[8]; + int32_t ret; + + ret = 0; + /* FIXME: Remove warnings with STM32CubeIDE */ + ctrl3_c.not_used_01 = 0; + ctrl4_c.not_used_01 = 0; + /* reading input configuration */ + xl_hm_mode = ((uint8_t)val->ui.xl.odr & 0x10U) >> 4; + xl_ulp_en = ((uint8_t)val->ui.xl.odr & 0x20U) >> 5; + odr_xl = (uint8_t)val->ui.xl.odr & 0x0FU; + + /* if enable xl ultra low power mode disable gy and OIS chain */ + if (xl_ulp_en == PROPERTY_ENABLE) + { + val->ois.xl.odr = LSM6DSO_XL_OIS_OFF; + val->ois.gy.odr = LSM6DSO_GY_OIS_OFF; + val->ui.gy.odr = LSM6DSO_GY_UI_OFF; + } + + /* if OIS xl is enabled also gyro OIS is enabled */ + if (val->ois.xl.odr == LSM6DSO_XL_OIS_6667Hz_HP) + { + val->ois.gy.odr = LSM6DSO_GY_OIS_6667Hz_HP; + } + + g_hm_mode = ((uint8_t)val->ui.gy.odr & 0x10U) >> 4; + odr_gy = (uint8_t)val->ui.gy.odr & 0x0FU; + + /* reading registers to be configured */ + if (ctx != NULL) + { + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL1_XL, reg, 8); + if (ret != 0) { return ret; } + + bytecpy((uint8_t *)&ctrl1_xl, ®[0]); + bytecpy((uint8_t *)&ctrl2_g, ®[1]); + bytecpy((uint8_t *)&ctrl3_c, ®[2]); + bytecpy((uint8_t *)&ctrl4_c, ®[3]); + bytecpy((uint8_t *)&ctrl5_c, ®[4]); + bytecpy((uint8_t *)&ctrl6_c, ®[5]); + bytecpy((uint8_t *)&ctrl7_g, ®[6]); + bytecpy((uint8_t *)&ctrl8_xl, ®[7]); + + ret = lsm6dso_read_reg(ctx, LSM6DSO_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); + + /* if toggle xl ultra low power mode, turn off xl before reconfigure */ + if (ctrl5_c.xl_ulp_en != xl_ulp_en) + { + ctrl1_xl.odr_xl = (uint8_t) 0x00U; + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL1_XL, (uint8_t *)&ctrl1_xl, 1); + } + } + + /* reading OIS registers to be configured */ + if (aux_ctx != NULL) + { + ret = lsm6dso_read_reg(aux_ctx, LSM6DSO_CTRL1_OIS, reg, 3); + if (ret != 0) { return ret; } + + bytecpy((uint8_t *)&ctrl1_ois, ®[0]); + bytecpy((uint8_t *)&ctrl2_ois, ®[1]); + bytecpy((uint8_t *)&ctrl3_ois, ®[2]); + } + else + { + if (ctx != NULL) + { + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL1_OIS, reg, 3); + if (ret != 0) { return ret; } + + bytecpy((uint8_t *)&ctrl1_ois, ®[0]); + bytecpy((uint8_t *)&ctrl2_ois, ®[1]); + bytecpy((uint8_t *)&ctrl3_ois, ®[2]); + } + } + + /* Check the Finite State Machine data rate constraints */ + if (val->fsm.sens != LSM6DSO_FSM_DISABLE) + { + switch (val->fsm.odr) + { + case LSM6DSO_FSM_12Hz5: + if ((val->fsm.sens != LSM6DSO_FSM_GY) && (odr_xl == 0x00U)) + { + odr_xl = 0x01U; + } + + if ((val->fsm.sens != LSM6DSO_FSM_XL) && (odr_gy == 0x00U)) + { + xl_ulp_en = PROPERTY_DISABLE; + odr_gy = 0x01U; + } + + break; + + case LSM6DSO_FSM_26Hz: + if ((val->fsm.sens != LSM6DSO_FSM_GY) && (odr_xl < 0x02U)) + { + odr_xl = 0x02U; + } + + if ((val->fsm.sens != LSM6DSO_FSM_XL) && (odr_gy < 0x02U)) + { + xl_ulp_en = PROPERTY_DISABLE; + odr_gy = 0x02U; + } + + break; + + case LSM6DSO_FSM_52Hz: + if ((val->fsm.sens != LSM6DSO_FSM_GY) && (odr_xl < 0x03U)) + { + odr_xl = 0x03U; + } + + if ((val->fsm.sens != LSM6DSO_FSM_XL) && (odr_gy < 0x03U)) + { + xl_ulp_en = PROPERTY_DISABLE; + odr_gy = 0x03U; + } + + break; + + case LSM6DSO_FSM_104Hz: + if ((val->fsm.sens != LSM6DSO_FSM_GY) && (odr_xl < 0x04U)) + { + odr_xl = 0x04U; + } + + if ((val->fsm.sens != LSM6DSO_FSM_XL) && (odr_gy < 0x04U)) + { + xl_ulp_en = PROPERTY_DISABLE; + odr_gy = 0x04U; + } + + break; + + default: + odr_xl = 0x00U; + odr_gy = 0x00U; + break; + } + } + + /* Updating the accelerometer data rate configuration */ + switch ((ctrl5_c.xl_ulp_en << 5) | (ctrl6_c.xl_hm_mode << 4) | + ctrl1_xl.odr_xl) + { + case LSM6DSO_XL_UI_OFF: + val->ui.xl.odr = LSM6DSO_XL_UI_OFF; + break; + + case LSM6DSO_XL_UI_12Hz5_HP: + val->ui.xl.odr = LSM6DSO_XL_UI_12Hz5_HP; + break; + + case LSM6DSO_XL_UI_26Hz_HP: + val->ui.xl.odr = LSM6DSO_XL_UI_26Hz_HP; + break; + + case LSM6DSO_XL_UI_52Hz_HP: + val->ui.xl.odr = LSM6DSO_XL_UI_52Hz_HP; + break; + + case LSM6DSO_XL_UI_104Hz_HP: + val->ui.xl.odr = LSM6DSO_XL_UI_104Hz_HP; + break; + + case LSM6DSO_XL_UI_208Hz_HP: + val->ui.xl.odr = LSM6DSO_XL_UI_208Hz_HP; + break; + + case LSM6DSO_XL_UI_416Hz_HP: + val->ui.xl.odr = LSM6DSO_XL_UI_416Hz_HP; + break; + + case LSM6DSO_XL_UI_833Hz_HP: + val->ui.xl.odr = LSM6DSO_XL_UI_833Hz_HP; + break; + + case LSM6DSO_XL_UI_1667Hz_HP: + val->ui.xl.odr = LSM6DSO_XL_UI_1667Hz_HP; + break; + + case LSM6DSO_XL_UI_3333Hz_HP: + val->ui.xl.odr = LSM6DSO_XL_UI_3333Hz_HP; + break; + + case LSM6DSO_XL_UI_6667Hz_HP: + val->ui.xl.odr = LSM6DSO_XL_UI_6667Hz_HP; + break; + + case LSM6DSO_XL_UI_1Hz6_LP: + val->ui.xl.odr = LSM6DSO_XL_UI_1Hz6_LP; + break; + + case LSM6DSO_XL_UI_12Hz5_LP: + val->ui.xl.odr = LSM6DSO_XL_UI_12Hz5_LP; + break; + + case LSM6DSO_XL_UI_26Hz_LP: + val->ui.xl.odr = LSM6DSO_XL_UI_26Hz_LP; + break; + + case LSM6DSO_XL_UI_52Hz_LP: + val->ui.xl.odr = LSM6DSO_XL_UI_52Hz_LP; + break; + + case LSM6DSO_XL_UI_104Hz_NM: + val->ui.xl.odr = LSM6DSO_XL_UI_104Hz_NM; + break; + + case LSM6DSO_XL_UI_208Hz_NM: + val->ui.xl.odr = LSM6DSO_XL_UI_208Hz_NM; + break; + + case LSM6DSO_XL_UI_1Hz6_ULP: + val->ui.xl.odr = LSM6DSO_XL_UI_1Hz6_ULP; + break; + + case LSM6DSO_XL_UI_12Hz5_ULP: + val->ui.xl.odr = LSM6DSO_XL_UI_12Hz5_ULP; + break; + + case LSM6DSO_XL_UI_26Hz_ULP: + val->ui.xl.odr = LSM6DSO_XL_UI_26Hz_ULP; + break; + + case LSM6DSO_XL_UI_52Hz_ULP: + val->ui.xl.odr = LSM6DSO_XL_UI_52Hz_ULP; + break; + + case LSM6DSO_XL_UI_104Hz_ULP: + val->ui.xl.odr = LSM6DSO_XL_UI_104Hz_ULP; + break; + + case LSM6DSO_XL_UI_208Hz_ULP: + val->ui.xl.odr = LSM6DSO_XL_UI_208Hz_ULP; + break; + + default: + val->ui.xl.odr = LSM6DSO_XL_UI_OFF; + break; + } + + /* Updating the accelerometer data rate configuration */ + switch ((ctrl7_g.g_hm_mode << 4) | ctrl2_g.odr_g) + { + case LSM6DSO_GY_UI_OFF: + val->ui.gy.odr = LSM6DSO_GY_UI_OFF; + break; + + case LSM6DSO_GY_UI_12Hz5_LP: + val->ui.gy.odr = LSM6DSO_GY_UI_12Hz5_LP; + break; + + case LSM6DSO_GY_UI_12Hz5_HP: + val->ui.gy.odr = LSM6DSO_GY_UI_12Hz5_HP; + break; + + case LSM6DSO_GY_UI_26Hz_LP: + val->ui.gy.odr = LSM6DSO_GY_UI_26Hz_LP; + break; + + case LSM6DSO_GY_UI_26Hz_HP: + val->ui.gy.odr = LSM6DSO_GY_UI_26Hz_HP; + break; + + case LSM6DSO_GY_UI_52Hz_LP: + val->ui.gy.odr = LSM6DSO_GY_UI_52Hz_LP; + break; + + case LSM6DSO_GY_UI_52Hz_HP: + val->ui.gy.odr = LSM6DSO_GY_UI_52Hz_HP; + break; + + case LSM6DSO_GY_UI_104Hz_NM: + val->ui.gy.odr = LSM6DSO_GY_UI_104Hz_NM; + break; + + case LSM6DSO_GY_UI_104Hz_HP: + val->ui.gy.odr = LSM6DSO_GY_UI_104Hz_HP; + break; + + case LSM6DSO_GY_UI_208Hz_NM: + val->ui.gy.odr = LSM6DSO_GY_UI_208Hz_NM; + break; + + case LSM6DSO_GY_UI_208Hz_HP: + val->ui.gy.odr = LSM6DSO_GY_UI_208Hz_HP; + break; + + case LSM6DSO_GY_UI_416Hz_HP: + val->ui.gy.odr = LSM6DSO_GY_UI_416Hz_HP; + break; + + case LSM6DSO_GY_UI_833Hz_HP: + val->ui.gy.odr = LSM6DSO_GY_UI_833Hz_HP; + break; + + case LSM6DSO_GY_UI_1667Hz_HP: + val->ui.gy.odr = LSM6DSO_GY_UI_1667Hz_HP; + break; + + case LSM6DSO_GY_UI_3333Hz_HP: + val->ui.gy.odr = LSM6DSO_GY_UI_3333Hz_HP; + break; + + case LSM6DSO_GY_UI_6667Hz_HP: + val->ui.gy.odr = LSM6DSO_GY_UI_6667Hz_HP; + break; + + default: + val->ui.gy.odr = LSM6DSO_GY_UI_OFF; + break; + } + + /* Check accelerometer full scale constraints */ + /* Full scale of 16g must be the same for UI and OIS */ + if ((val->ui.xl.fs == LSM6DSO_XL_UI_16g) || + (val->ois.xl.fs == LSM6DSO_XL_OIS_16g)) + { + val->ui.xl.fs = LSM6DSO_XL_UI_16g; + val->ois.xl.fs = LSM6DSO_XL_OIS_16g; + } + + /* prapare new configuration */ + + /* Full scale of 16g must be the same for UI and OIS */ + if (val->ui.xl.fs == LSM6DSO_XL_UI_16g) + { + ctrl8_xl.xl_fs_mode = PROPERTY_DISABLE; + } + + else + { + ctrl8_xl.xl_fs_mode = PROPERTY_ENABLE; + } + + /* OIS new configuration */ + ctrl7_g.ois_on_en = (val->ois.ctrl_md == LSM6DSO_OIS_MIXED) ? 1U : 0U; + + switch (val->ois.ctrl_md) + { + case LSM6DSO_OIS_ONLY_AUX: + ctrl1_ois.fs_g_ois = (uint8_t)val->ois.gy.fs; + ctrl1_ois.ois_en_spi2 = (uint8_t)val->ois.gy.odr | + (uint8_t)val->ois.xl.odr; + ctrl1_ois.mode4_en = (uint8_t) val->ois.xl.odr; + ctrl3_ois.fs_xl_ois = (uint8_t)val->ois.xl.fs; + break; + + case LSM6DSO_OIS_MIXED: + ctrl1_ois.fs_g_ois = (uint8_t)val->ois.gy.fs; + ctrl7_g.ois_on = (uint8_t)val->ois.gy.odr | (uint8_t)val->ois.xl.odr; + ctrl1_ois.mode4_en = (uint8_t) val->ois.xl.odr; + ctrl3_ois.fs_xl_ois = (uint8_t)val->ois.xl.fs; + break; + + default: + ctrl1_ois.fs_g_ois = (uint8_t)val->ois.gy.fs; + ctrl1_ois.ois_en_spi2 = (uint8_t)val->ois.gy.odr | + (uint8_t)val->ois.xl.odr; + ctrl1_ois.mode4_en = (uint8_t) val->ois.xl.odr; + ctrl3_ois.fs_xl_ois = (uint8_t)val->ois.xl.fs; + break; + } + + /* UI new configuration */ + ctrl1_xl.odr_xl = odr_xl; + ctrl1_xl.fs_xl = (uint8_t)val->ui.xl.fs; + ctrl5_c.xl_ulp_en = xl_ulp_en; + ctrl6_c.xl_hm_mode = xl_hm_mode; + ctrl7_g.g_hm_mode = g_hm_mode; + ctrl2_g.odr_g = odr_gy; + ctrl2_g.fs_g = (uint8_t) val->ui.gy.fs; + + /* writing checked configuration */ + if (ctx != NULL) + { + bytecpy(®[0], (uint8_t *)&ctrl1_xl); + bytecpy(®[1], (uint8_t *)&ctrl2_g); + bytecpy(®[2], (uint8_t *)&ctrl3_c); + bytecpy(®[3], (uint8_t *)&ctrl4_c); + bytecpy(®[4], (uint8_t *)&ctrl5_c); + bytecpy(®[5], (uint8_t *)&ctrl6_c); + bytecpy(®[6], (uint8_t *)&ctrl7_g); + bytecpy(®[7], (uint8_t *)&ctrl8_xl); + + ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL1_XL, (uint8_t *)®, 8); + ret += lsm6dso_write_reg(ctx, LSM6DSO_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); + if (ret != 0) { return ret; } + } + + /* writing OIS checked configuration */ + if (aux_ctx != NULL) + { + bytecpy(®[0], (uint8_t *)&ctrl1_ois); + bytecpy(®[1], (uint8_t *)&ctrl2_ois); + bytecpy(®[2], (uint8_t *)&ctrl3_ois); + + ret = lsm6dso_write_reg(aux_ctx, LSM6DSO_CTRL1_OIS, reg, 3); + } + + return ret; +} + +/** + * @brief Sensor conversion parameters selection.[get] + * + * @param ctx communication interface handler. Use NULL to ignore + * this interface.(ptr) + * @param aux_ctx auxiliary communication interface handler. Use NULL + * to ignore this interface.(ptr) + * @param val get the sensor conversion parameters.(ptr) + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_mode_get(stmdev_ctx_t *ctx, stmdev_ctx_t *aux_ctx, + lsm6dso_md_t *val) +{ + lsm6dso_emb_func_odr_cfg_b_t emb_func_odr_cfg_b; + lsm6dso_func_cfg_access_t func_cfg_access; + lsm6dso_emb_func_en_b_t emb_func_en_b; + lsm6dso_fsm_enable_a_t fsm_enable_a; + lsm6dso_fsm_enable_b_t fsm_enable_b; + lsm6dso_ctrl1_ois_t ctrl1_ois; + lsm6dso_ctrl2_ois_t ctrl2_ois; + lsm6dso_ctrl3_ois_t ctrl3_ois; + lsm6dso_ctrl1_xl_t ctrl1_xl; + lsm6dso_ctrl2_g_t ctrl2_g; + lsm6dso_ctrl3_c_t ctrl3_c; + lsm6dso_ctrl4_c_t ctrl4_c; + lsm6dso_ctrl5_c_t ctrl5_c; + lsm6dso_ctrl6_c_t ctrl6_c; + lsm6dso_ctrl7_g_t ctrl7_g; + uint8_t reg[8]; + int32_t ret; + + ret = 0; + + /* reading the registers of the device */ + if (ctx != NULL) + { + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL1_XL, reg, 7); + if (ret != 0) { return ret; } + + bytecpy((uint8_t *)&ctrl1_xl, ®[0]); + bytecpy((uint8_t *)&ctrl2_g, ®[1]); + bytecpy((uint8_t *)&ctrl3_c, ®[2]); + bytecpy((uint8_t *)&ctrl4_c, ®[3]); + bytecpy((uint8_t *)&ctrl5_c, ®[4]); + bytecpy((uint8_t *)&ctrl6_c, ®[5]); + bytecpy((uint8_t *)&ctrl7_g, ®[6]); + + ret = lsm6dso_read_reg(ctx, LSM6DSO_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); + if (ret != 0) { return ret; } + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_EMBEDDED_FUNC_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_EMB_FUNC_ODR_CFG_B, reg, 1); + bytecpy((uint8_t *)&emb_func_odr_cfg_b, ®[0]); + + ret += lsm6dso_read_reg(ctx, LSM6DSO_EMB_FUNC_EN_B, (uint8_t *)&emb_func_en_b, 1); + bytecpy((uint8_t *)&fsm_enable_b, ®[1]); + + ret += lsm6dso_read_reg(ctx, LSM6DSO_FSM_ENABLE_A, reg, 2); + bytecpy((uint8_t *)&fsm_enable_a, ®[0]); + + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + if (ret != 0) { return ret; } + } + + if (aux_ctx != NULL) + { + ret = lsm6dso_read_reg(aux_ctx, LSM6DSO_CTRL1_OIS, reg, 3); + if (ret != 0) { return ret; } + + bytecpy((uint8_t *)&ctrl1_ois, ®[0]); + bytecpy((uint8_t *)&ctrl2_ois, ®[1]); + bytecpy((uint8_t *)&ctrl3_ois, ®[2]); + } + + else + { + if (ctx != NULL) + { + ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL1_OIS, reg, 3); + if (ret != 0) { return ret; } + + bytecpy((uint8_t *)&ctrl1_ois, ®[0]); + bytecpy((uint8_t *)&ctrl2_ois, ®[1]); + bytecpy((uint8_t *)&ctrl3_ois, ®[2]); + } + } + + /* fill the input structure */ + + /* get accelerometer configuration */ + switch ((ctrl5_c.xl_ulp_en << 5) | (ctrl6_c.xl_hm_mode << 4) | + ctrl1_xl.odr_xl) + { + case LSM6DSO_XL_UI_OFF: + val->ui.xl.odr = LSM6DSO_XL_UI_OFF; + break; + + case LSM6DSO_XL_UI_12Hz5_HP: + val->ui.xl.odr = LSM6DSO_XL_UI_12Hz5_HP; + break; + + case LSM6DSO_XL_UI_26Hz_HP: + val->ui.xl.odr = LSM6DSO_XL_UI_26Hz_HP; + break; + + case LSM6DSO_XL_UI_52Hz_HP: + val->ui.xl.odr = LSM6DSO_XL_UI_52Hz_HP; + break; + + case LSM6DSO_XL_UI_104Hz_HP: + val->ui.xl.odr = LSM6DSO_XL_UI_104Hz_HP; + break; + + case LSM6DSO_XL_UI_208Hz_HP: + val->ui.xl.odr = LSM6DSO_XL_UI_208Hz_HP; + break; + + case LSM6DSO_XL_UI_416Hz_HP: + val->ui.xl.odr = LSM6DSO_XL_UI_416Hz_HP; + break; + + case LSM6DSO_XL_UI_833Hz_HP: + val->ui.xl.odr = LSM6DSO_XL_UI_833Hz_HP; + break; + + case LSM6DSO_XL_UI_1667Hz_HP: + val->ui.xl.odr = LSM6DSO_XL_UI_1667Hz_HP; + break; + + case LSM6DSO_XL_UI_3333Hz_HP: + val->ui.xl.odr = LSM6DSO_XL_UI_3333Hz_HP; + break; + + case LSM6DSO_XL_UI_6667Hz_HP: + val->ui.xl.odr = LSM6DSO_XL_UI_6667Hz_HP; + break; + + case LSM6DSO_XL_UI_1Hz6_LP: + val->ui.xl.odr = LSM6DSO_XL_UI_1Hz6_LP; + break; + + case LSM6DSO_XL_UI_12Hz5_LP: + val->ui.xl.odr = LSM6DSO_XL_UI_12Hz5_LP; + break; + + case LSM6DSO_XL_UI_26Hz_LP: + val->ui.xl.odr = LSM6DSO_XL_UI_26Hz_LP; + break; + + case LSM6DSO_XL_UI_52Hz_LP: + val->ui.xl.odr = LSM6DSO_XL_UI_52Hz_LP; + break; + + case LSM6DSO_XL_UI_104Hz_NM: + val->ui.xl.odr = LSM6DSO_XL_UI_104Hz_NM; + break; + + case LSM6DSO_XL_UI_208Hz_NM: + val->ui.xl.odr = LSM6DSO_XL_UI_208Hz_NM; + break; + + case LSM6DSO_XL_UI_1Hz6_ULP: + val->ui.xl.odr = LSM6DSO_XL_UI_1Hz6_ULP; + break; + + case LSM6DSO_XL_UI_12Hz5_ULP: + val->ui.xl.odr = LSM6DSO_XL_UI_12Hz5_ULP; + break; + + case LSM6DSO_XL_UI_26Hz_ULP: + val->ui.xl.odr = LSM6DSO_XL_UI_26Hz_ULP; + break; + + case LSM6DSO_XL_UI_52Hz_ULP: + val->ui.xl.odr = LSM6DSO_XL_UI_52Hz_ULP; + break; + + case LSM6DSO_XL_UI_104Hz_ULP: + val->ui.xl.odr = LSM6DSO_XL_UI_104Hz_ULP; + break; + + case LSM6DSO_XL_UI_208Hz_ULP: + val->ui.xl.odr = LSM6DSO_XL_UI_208Hz_ULP; + break; + + default: + val->ui.xl.odr = LSM6DSO_XL_UI_OFF; + break; + } + + switch (ctrl1_xl.fs_xl) + { + case LSM6DSO_XL_UI_2g: + val->ui.xl.fs = LSM6DSO_XL_UI_2g; + break; + + case LSM6DSO_XL_UI_4g: + val->ui.xl.fs = LSM6DSO_XL_UI_4g; + break; + + case LSM6DSO_XL_UI_8g: + val->ui.xl.fs = LSM6DSO_XL_UI_8g; + break; + + case LSM6DSO_XL_UI_16g: + val->ui.xl.fs = LSM6DSO_XL_UI_16g; + break; + + default: + val->ui.xl.fs = LSM6DSO_XL_UI_2g; + break; + } + + /* get gyroscope configuration */ + switch ((ctrl7_g.g_hm_mode << 4) | ctrl2_g.odr_g) + { + case LSM6DSO_GY_UI_OFF: + val->ui.gy.odr = LSM6DSO_GY_UI_OFF; + break; + + case LSM6DSO_GY_UI_12Hz5_LP: + val->ui.gy.odr = LSM6DSO_GY_UI_12Hz5_LP; + break; + + case LSM6DSO_GY_UI_12Hz5_HP: + val->ui.gy.odr = LSM6DSO_GY_UI_12Hz5_HP; + break; + + case LSM6DSO_GY_UI_26Hz_LP: + val->ui.gy.odr = LSM6DSO_GY_UI_26Hz_LP; + break; + + case LSM6DSO_GY_UI_26Hz_HP: + val->ui.gy.odr = LSM6DSO_GY_UI_26Hz_HP; + break; + + case LSM6DSO_GY_UI_52Hz_LP: + val->ui.gy.odr = LSM6DSO_GY_UI_52Hz_LP; + break; + + case LSM6DSO_GY_UI_52Hz_HP: + val->ui.gy.odr = LSM6DSO_GY_UI_52Hz_HP; + break; + + case LSM6DSO_GY_UI_104Hz_NM: + val->ui.gy.odr = LSM6DSO_GY_UI_104Hz_NM; + break; + + case LSM6DSO_GY_UI_104Hz_HP: + val->ui.gy.odr = LSM6DSO_GY_UI_104Hz_HP; + break; + + case LSM6DSO_GY_UI_208Hz_NM: + val->ui.gy.odr = LSM6DSO_GY_UI_208Hz_NM; + break; + + case LSM6DSO_GY_UI_208Hz_HP: + val->ui.gy.odr = LSM6DSO_GY_UI_208Hz_HP; + break; + + case LSM6DSO_GY_UI_416Hz_HP: + val->ui.gy.odr = LSM6DSO_GY_UI_416Hz_HP; + break; + + case LSM6DSO_GY_UI_833Hz_HP: + val->ui.gy.odr = LSM6DSO_GY_UI_833Hz_HP; + break; + + case LSM6DSO_GY_UI_1667Hz_HP: + val->ui.gy.odr = LSM6DSO_GY_UI_1667Hz_HP; + break; + + case LSM6DSO_GY_UI_3333Hz_HP: + val->ui.gy.odr = LSM6DSO_GY_UI_3333Hz_HP; + break; + + case LSM6DSO_GY_UI_6667Hz_HP: + val->ui.gy.odr = LSM6DSO_GY_UI_6667Hz_HP; + break; + + default: + val->ui.gy.odr = LSM6DSO_GY_UI_OFF; + break; + } + + switch (ctrl2_g.fs_g) + { + case LSM6DSO_GY_UI_125dps: + val->ui.gy.fs = LSM6DSO_GY_UI_125dps; + break; + + case LSM6DSO_GY_UI_250dps: + val->ui.gy.fs = LSM6DSO_GY_UI_250dps; + break; + + case LSM6DSO_GY_UI_500dps: + val->ui.gy.fs = LSM6DSO_GY_UI_500dps; + break; + + case LSM6DSO_GY_UI_1000dps: + val->ui.gy.fs = LSM6DSO_GY_UI_1000dps; + break; + + case LSM6DSO_GY_UI_2000dps: + val->ui.gy.fs = LSM6DSO_GY_UI_2000dps; + break; + + default: + val->ui.gy.fs = LSM6DSO_GY_UI_125dps; + break; + } + + /* get finite state machine configuration */ + if ((fsm_enable_a.fsm1_en | fsm_enable_a.fsm2_en | + fsm_enable_a.fsm3_en | + fsm_enable_a.fsm4_en | fsm_enable_a.fsm5_en | fsm_enable_a.fsm6_en | + fsm_enable_a.fsm7_en | fsm_enable_a.fsm8_en | fsm_enable_b.fsm9_en | + fsm_enable_b.fsm10_en | fsm_enable_b.fsm11_en | + fsm_enable_b.fsm12_en | fsm_enable_b.fsm13_en | + fsm_enable_b.fsm14_en | fsm_enable_b.fsm15_en | + fsm_enable_b.fsm16_en) == PROPERTY_ENABLE) + { + switch (emb_func_odr_cfg_b.fsm_odr) + { + case LSM6DSO_FSM_12Hz5: + val->fsm.odr = LSM6DSO_FSM_12Hz5; + break; + + case LSM6DSO_FSM_26Hz: + val->fsm.odr = LSM6DSO_FSM_26Hz; + break; + + case LSM6DSO_FSM_52Hz: + val->fsm.odr = LSM6DSO_FSM_52Hz; + break; + + case LSM6DSO_FSM_104Hz: + val->fsm.odr = LSM6DSO_FSM_104Hz; + break; + + default: + val->fsm.odr = LSM6DSO_FSM_12Hz5; + break; + } + + val->fsm.sens = LSM6DSO_FSM_XL_GY; + + if (val->ui.gy.odr == LSM6DSO_GY_UI_OFF) + { + val->fsm.sens = LSM6DSO_FSM_XL; + } + + if (val->ui.xl.odr == LSM6DSO_XL_UI_OFF) + { + val->fsm.sens = LSM6DSO_FSM_GY; + } + } + + else + { + val->fsm.sens = LSM6DSO_FSM_DISABLE; + } + + /* get ois configuration */ + + /* OIS configuration mode */ + switch (ctrl7_g.ois_on_en) + { + case LSM6DSO_OIS_ONLY_AUX: + switch (ctrl3_ois.fs_xl_ois) + { + case LSM6DSO_XL_OIS_2g: + val->ois.xl.fs = LSM6DSO_XL_OIS_2g; + break; + + case LSM6DSO_XL_OIS_4g: + val->ois.xl.fs = LSM6DSO_XL_OIS_4g; + break; + + case LSM6DSO_XL_OIS_8g: + val->ois.xl.fs = LSM6DSO_XL_OIS_8g; + break; + + case LSM6DSO_XL_OIS_16g: + val->ois.xl.fs = LSM6DSO_XL_OIS_16g; + break; + + default: + val->ois.xl.fs = LSM6DSO_XL_OIS_2g; + break; + } + + switch (ctrl1_ois.mode4_en) + { + case LSM6DSO_XL_OIS_OFF: + val->ois.xl.odr = LSM6DSO_XL_OIS_OFF; + break; + + case LSM6DSO_XL_OIS_6667Hz_HP: + val->ois.xl.odr = LSM6DSO_XL_OIS_6667Hz_HP; + break; + + default: + val->ois.xl.odr = LSM6DSO_XL_OIS_OFF; + break; + } + + switch (ctrl1_ois.fs_g_ois) + { + case LSM6DSO_GY_OIS_250dps: + val->ois.gy.fs = LSM6DSO_GY_OIS_250dps; + break; + + case LSM6DSO_GY_OIS_500dps: + val->ois.gy.fs = LSM6DSO_GY_OIS_500dps; + break; + + case LSM6DSO_GY_OIS_1000dps: + val->ois.gy.fs = LSM6DSO_GY_OIS_1000dps; + break; + + case LSM6DSO_GY_OIS_2000dps: + val->ois.gy.fs = LSM6DSO_GY_OIS_2000dps; + break; + + default: + val->ois.gy.fs = LSM6DSO_GY_OIS_250dps; + break; + } + + switch (ctrl1_ois.ois_en_spi2) + { + case LSM6DSO_GY_OIS_OFF: + val->ois.gy.odr = LSM6DSO_GY_OIS_OFF; + break; + + case LSM6DSO_GY_OIS_6667Hz_HP: + val->ois.gy.odr = LSM6DSO_GY_OIS_6667Hz_HP; + break; + + default: + val->ois.gy.odr = LSM6DSO_GY_OIS_OFF; + break; + } + + val->ois.ctrl_md = LSM6DSO_OIS_ONLY_AUX; + break; + + case LSM6DSO_OIS_MIXED: + switch (ctrl3_ois.fs_xl_ois) + { + case LSM6DSO_XL_OIS_2g: + val->ois.xl.fs = LSM6DSO_XL_OIS_2g; + break; + + case LSM6DSO_XL_OIS_4g: + val->ois.xl.fs = LSM6DSO_XL_OIS_4g; + break; + + case LSM6DSO_XL_OIS_8g: + val->ois.xl.fs = LSM6DSO_XL_OIS_8g; + break; + + case LSM6DSO_XL_OIS_16g: + val->ois.xl.fs = LSM6DSO_XL_OIS_16g; + break; + + default: + val->ois.xl.fs = LSM6DSO_XL_OIS_2g; + break; + } + + switch (ctrl1_ois.mode4_en) + { + case LSM6DSO_XL_OIS_OFF: + val->ois.xl.odr = LSM6DSO_XL_OIS_OFF; + break; + + case LSM6DSO_XL_OIS_6667Hz_HP: + val->ois.xl.odr = LSM6DSO_XL_OIS_6667Hz_HP; + break; + + default: + val->ois.xl.odr = LSM6DSO_XL_OIS_OFF; + break; + } + + switch (ctrl1_ois.fs_g_ois) + { + case LSM6DSO_GY_OIS_250dps: + val->ois.gy.fs = LSM6DSO_GY_OIS_250dps; + break; + + case LSM6DSO_GY_OIS_500dps: + val->ois.gy.fs = LSM6DSO_GY_OIS_500dps; + break; + + case LSM6DSO_GY_OIS_1000dps: + val->ois.gy.fs = LSM6DSO_GY_OIS_1000dps; + break; + + case LSM6DSO_GY_OIS_2000dps: + val->ois.gy.fs = LSM6DSO_GY_OIS_2000dps; + break; + + default: + val->ois.gy.fs = LSM6DSO_GY_OIS_250dps; + break; + } + + switch (ctrl1_ois.ois_en_spi2) + { + case LSM6DSO_GY_OIS_OFF: + val->ois.gy.odr = LSM6DSO_GY_OIS_OFF; + break; + + case LSM6DSO_GY_OIS_6667Hz_HP: + val->ois.gy.odr = LSM6DSO_GY_OIS_6667Hz_HP; + break; + + default: + val->ois.gy.odr = LSM6DSO_GY_OIS_OFF; + break; + } + + val->ois.ctrl_md = LSM6DSO_OIS_MIXED; + break; + + default: + val->ois.gy.fs = LSM6DSO_GY_OIS_250dps; + val->ois.gy.odr = LSM6DSO_GY_OIS_OFF; + val->ois.xl.odr = LSM6DSO_XL_OIS_OFF; + val->ois.xl.fs = LSM6DSO_XL_OIS_2g; + val->ois.ctrl_md = LSM6DSO_OIS_ONLY_AUX; + break; + } + + return ret; +} + +/** + * @brief Read data in engineering unit.[get] + * + * @param ctx communication interface handler.(ptr) + * @param md the sensor conversion parameters.(ptr) + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_data_get(stmdev_ctx_t *ctx, stmdev_ctx_t *aux_ctx, + lsm6dso_md_t *md, lsm6dso_data_t *data) +{ + uint8_t buff[14]; + int32_t ret; + + uint8_t i; + uint8_t j; + ret = 0; + + /* read data */ + if (ctx != NULL) + { + ret = lsm6dso_read_reg(ctx, LSM6DSO_OUT_TEMP_L, buff, 14); + if (ret != 0) { return ret; } + } + + j = 0; + /* temperature conversion */ + data->ui.heat.raw = (int16_t)buff[j + 1U]; + data->ui.heat.raw = (((int16_t)data->ui.heat.raw * (int16_t)256) + + (int16_t)buff[j]); + j += 2U; + data->ui.heat.deg_c = lsm6dso_from_lsb_to_celsius(( + int16_t)data->ui.heat.raw); + + /* angular rate conversion */ + for (i = 0U; i < 3U; i++) + { + data->ui.gy.raw[i] = (int16_t)buff[j + 1U]; + data->ui.gy.raw[i] = (data->ui.gy.raw[i] * 256) + (int16_t) buff[j]; + j += 2U; + + switch (md->ui.gy.fs) + { + case LSM6DSO_GY_UI_250dps: + data->ui.gy.mdps[i] = lsm6dso_from_fs250_to_mdps(data->ui.gy.raw[i]); + break; + + case LSM6DSO_GY_UI_125dps: + data->ui.gy.mdps[i] = lsm6dso_from_fs125_to_mdps(data->ui.gy.raw[i]); + break; + + case LSM6DSO_GY_UI_500dps: + data->ui.gy.mdps[i] = lsm6dso_from_fs500_to_mdps(data->ui.gy.raw[i]); + break; + + case LSM6DSO_GY_UI_1000dps: + data->ui.gy.mdps[i] = lsm6dso_from_fs1000_to_mdps(data->ui.gy.raw[i]); + break; + + case LSM6DSO_GY_UI_2000dps: + data->ui.gy.mdps[i] = lsm6dso_from_fs2000_to_mdps(data->ui.gy.raw[i]); + break; + + default: + data->ui.gy.mdps[i] = 0.0f; + break; + } + } + + /* acceleration conversion */ + for (i = 0U; i < 3U; i++) + { + data->ui.xl.raw[i] = (int16_t)buff[j + 1U]; + data->ui.xl.raw[i] = (data->ui.xl.raw[i] * 256) + (int16_t) buff[j]; + j += 2U; + + switch (md->ui.xl.fs) + { + case LSM6DSO_XL_UI_2g: + data->ui.xl.mg[i] = lsm6dso_from_fs2_to_mg(data->ui.xl.raw[i]); + break; + + case LSM6DSO_XL_UI_4g: + data->ui.xl.mg[i] = lsm6dso_from_fs4_to_mg(data->ui.xl.raw[i]); + break; + + case LSM6DSO_XL_UI_8g: + data->ui.xl.mg[i] = lsm6dso_from_fs8_to_mg(data->ui.xl.raw[i]); + break; + + case LSM6DSO_XL_UI_16g: + data->ui.xl.mg[i] = lsm6dso_from_fs16_to_mg(data->ui.xl.raw[i]); + break; + + default: + data->ui.xl.mg[i] = 0.0f; + break; + } + } + + /* read data from ois chain */ + if (aux_ctx != NULL) + { + ret = lsm6dso_read_reg(aux_ctx, LSM6DSO_OUTX_L_G, buff, 12); + if (ret != 0) { return ret; } + } + + j = 0; + + /* ois angular rate conversion */ + for (i = 0U; i < 3U; i++) + { + data->ois.gy.raw[i] = (int16_t) buff[j + 1U]; + data->ois.gy.raw[i] = (data->ois.gy.raw[i] * 256) + (int16_t) buff[j]; + j += 2U; + + switch (md->ois.gy.fs) + { + case LSM6DSO_GY_UI_250dps: + data->ois.gy.mdps[i] = lsm6dso_from_fs250_to_mdps(data->ois.gy.raw[i]); + break; + + case LSM6DSO_GY_UI_125dps: + data->ois.gy.mdps[i] = lsm6dso_from_fs125_to_mdps(data->ois.gy.raw[i]); + break; + + case LSM6DSO_GY_UI_500dps: + data->ois.gy.mdps[i] = lsm6dso_from_fs500_to_mdps(data->ois.gy.raw[i]); + break; + + case LSM6DSO_GY_UI_1000dps: + data->ois.gy.mdps[i] = lsm6dso_from_fs1000_to_mdps(data->ois.gy.raw[i]); + break; + + case LSM6DSO_GY_UI_2000dps: + data->ois.gy.mdps[i] = lsm6dso_from_fs2000_to_mdps(data->ois.gy.raw[i]); + break; + + default: + data->ois.gy.mdps[i] = 0.0f; + break; + } + } + + /* ois acceleration conversion */ + for (i = 0U; i < 3U; i++) + { + data->ois.xl.raw[i] = (int16_t) buff[j + 1U]; + data->ois.xl.raw[i] = (data->ois.xl.raw[i] * 256) + (int16_t) buff[j]; + j += 2U; + + switch (md->ois.xl.fs) + { + case LSM6DSO_XL_UI_2g: + data->ois.xl.mg[i] = lsm6dso_from_fs2_to_mg(data->ois.xl.raw[i]); + break; + + case LSM6DSO_XL_UI_4g: + data->ois.xl.mg[i] = lsm6dso_from_fs4_to_mg(data->ois.xl.raw[i]); + break; + + case LSM6DSO_XL_UI_8g: + data->ois.xl.mg[i] = lsm6dso_from_fs8_to_mg(data->ois.xl.raw[i]); + break; + + case LSM6DSO_XL_UI_16g: + data->ois.xl.mg[i] = lsm6dso_from_fs16_to_mg(data->ois.xl.raw[i]); + break; + + default: + data->ois.xl.mg[i] = 0.0f; + break; + } + } + + return ret; +} +/** + * @brief Embedded functions.[set] + * + * @param ctx read / write interface definitions + * @param val change the values of registers + * EMB_FUNC_EN_A e EMB_FUNC_EN_B. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_embedded_sens_set(stmdev_ctx_t *ctx, + lsm6dso_emb_sens_t *val) +{ + lsm6dso_emb_func_en_a_t emb_func_en_a; + lsm6dso_emb_func_en_b_t emb_func_en_b; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_EMBEDDED_FUNC_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); + ret += lsm6dso_read_reg(ctx, LSM6DSO_EMB_FUNC_EN_B, (uint8_t *)&emb_func_en_b, 1); + if (ret != 0) { goto exit; } + + emb_func_en_b.fsm_en = val->fsm; + emb_func_en_a.tilt_en = val->tilt; + emb_func_en_a.pedo_en = val->step; + emb_func_en_b.pedo_adv_en = val->step_adv; + emb_func_en_a.sign_motion_en = val->sig_mot; + emb_func_en_b.fifo_compr_en = val->fifo_compr; + + ret = lsm6dso_write_reg(ctx, LSM6DSO_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); + ret += lsm6dso_write_reg(ctx, LSM6DSO_EMB_FUNC_EN_B, (uint8_t *)&emb_func_en_b, 1); + +exit: + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @brief Embedded functions.[get] + * + * @param ctx read / write interface definitions + * @param val get the values of registers + * EMB_FUNC_EN_A e EMB_FUNC_EN_B. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_embedded_sens_get(stmdev_ctx_t *ctx, + lsm6dso_emb_sens_t *emb_sens) +{ + lsm6dso_emb_func_en_a_t emb_func_en_a; + lsm6dso_emb_func_en_b_t emb_func_en_b; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_EMBEDDED_FUNC_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); + ret += lsm6dso_read_reg(ctx, LSM6DSO_EMB_FUNC_EN_B, (uint8_t *)&emb_func_en_b, 1); + + emb_sens->fsm = emb_func_en_b.fsm_en; + emb_sens->tilt = emb_func_en_a.tilt_en; + emb_sens->step = emb_func_en_a.pedo_en; + emb_sens->step_adv = emb_func_en_b.pedo_adv_en; + emb_sens->sig_mot = emb_func_en_a.sign_motion_en; + emb_sens->fifo_compr = emb_func_en_b.fifo_compr_en; + + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @brief turn off all embedded functions.[get] + * + * @param ctx read / write interface definitions + * @param val get the values of registers + * EMB_FUNC_EN_A e EMB_FUNC_EN_B. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dso_embedded_sens_off(stmdev_ctx_t *ctx) +{ + lsm6dso_emb_func_en_a_t emb_func_en_a; + lsm6dso_emb_func_en_b_t emb_func_en_b; + int32_t ret; + + ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_EMBEDDED_FUNC_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dso_read_reg(ctx, LSM6DSO_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); + ret += lsm6dso_read_reg(ctx, LSM6DSO_EMB_FUNC_EN_B, (uint8_t *)&emb_func_en_b, 1); + if (ret != 0) { goto exit; } + + emb_func_en_b.fsm_en = PROPERTY_DISABLE; + emb_func_en_a.tilt_en = PROPERTY_DISABLE; + emb_func_en_a.pedo_en = PROPERTY_DISABLE; + emb_func_en_b.pedo_adv_en = PROPERTY_DISABLE; + emb_func_en_a.sign_motion_en = PROPERTY_DISABLE; + emb_func_en_b.fifo_compr_en = PROPERTY_DISABLE; + + ret = lsm6dso_write_reg(ctx, LSM6DSO_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); + ret += lsm6dso_write_reg(ctx, LSM6DSO_EMB_FUNC_EN_B, (uint8_t *)&emb_func_en_b, 1); + +exit: + ret += lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK); + + return ret; +} + +/** + * @} + * + */ + +/** + * @} + * + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ \ No newline at end of file