diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/LICENSE b/lib/main/BoschSensortec/BMI270-Sensor-API/LICENSE index df241f252..315dfa230 100644 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/LICENSE +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/LICENSE @@ -6,15 +6,15 @@ Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 1. Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. + notice, this list of conditions and the following disclaimer. 2. Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. 3. Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/OIS_README.md b/lib/main/BoschSensortec/BMI270-Sensor-API/OIS_README.md new file mode 100644 index 000000000..2983d0699 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/OIS_README.md @@ -0,0 +1,170 @@ +# Sensor API for the BMI2's OIS interface + +## Table of Contents + - [Introduction](#Intro) + - [Integration details](#Integration) + - [Driver files information](#file) + - [Sensor interfaces](#interface) + - [Integration Examples](#examples) + +### Introduction + This package contains Bosch Sensortec's BMI2 Sensor API. + +### Integration details +- Integrate _bmi2.c_, _bmi2.h_, _bmi2_ois.c_, _bmi2_ois.h_, _bmi2_defs.h_ and the required variant files in your project. +- User has to include _bmi2_ois.h_ in the code to call OIS related APIs and a _variant header_ for initialization as +well as BMI2 related API calls, as shown below: +``` c +#include "bmi270.h" +#include "bmi2_ois.h" +```` +### Driver files information +- *_bmi2_ois.c_* + * This file has function definitions of OIS related API interfaces. +- *_bmi2_ois.h_* + * This header file has necessary include files, function declarations, required to make OIS related API calls. + +### Sensor interfaces +#### _Host Interface_ +- I2C interface +- SPI interface +_Note: By default, the interface is I2C._ + +#### _OIS Interface_ +- SPI interface + +### Integration examples +#### Configuring SPI/I2C for host interface. +To configure host interface, an instance of the bmi2_dev structure should be +created for initializing BMI2 sensor. "_Refer **README** for initializing BMI2 +sensor._" + +#### Configuring SPI for OIS interface. +To configure OIS interface, an instance of the bmi2_ois_dev structure should be +created. The following parameters are required to be updated in the structure, +by the user. + +Parameters | Details +--------------|----------------------------------- +_intf_ptr_ | device address reference of SPI interface +_ois_read_ | read through SPI interface +_ois_write_ | read through SPI interface +_ois_delay_us_| delay in micro seconds +_acc_en_ | for enabling accelerometer +_gyr_en_ | for enabling gyroscope + +``` c +int8_t rslt = 0; + +struct bmi2_ois_dev ois_dev = { + .intf_ptr = intf_ptr will contain the chip selection info of SPI CS pin, + .ois_read = user_spi_reg_read, + .ois_write = user_spi_reg_write, + .ois_delay_us = user_delay_us +}; +``` +>**_Important Note_**: For initializing and configuring BMI2 sensors, which is +done through host interface, the API's are to be used from bmi2.c file. Rest +of the API's, for OIS configurations and the reading of OIS data, which is done +through OIS interface, are to be used from bmi2_ois.c file. + +##### Get accelerometer and gyroscope data through OIS interface +``` c +int8_t rslt; +/* Array to enable sensor through host interface */ +uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO}; +/* Array to enable sensor through OIS interface */ +uint8_t sens_sel[2] = {BMI2_OIS_ACCEL, BMI2_OIS_GYRO}; +/* Initialize the configuration structure */ +struct bmi2_sens_config sens_cfg = {0}; + +/* Initialize BMI2 */ +rslt = bmi2_init(&dev); +if (rslt != BMI2_OK) { + printf("Error: %d\n", rslt); + return; +} + +/* Enable accelerometer and gyroscope through host interface */ +rslt = bmi270_sensor_enable(sens_list, 2, &dev); +if (rslt != BMI2_OK) { + printf("Error: %d\n", rslt); + return; +} + +/* Setting of OIS Range is done through host interface */ +/* Select the gyroscope sensor for OIS Range configuration */ +sens_cfg.type = BMI2_GYRO; + +/* Get gyroscope configuration */ +rslt = bmi2_get_sensor_config(&sens_cfg, 1, &dev); +if (rslt != BMI2_OK) { + printf("Error: %d\n", rslt); + return; +} + +/* Set the desired OIS Range */ +sens_cfg.cfg.gyr.ois_range = BMI2_GYR_OIS_2000; + +/* Set gyroscope configuration for default values */ +rslt = bmi2_set_sensor_config(&sens_cfg, 1, &dev); +if (rslt != BMI2_OK) { + printf("Error: %d\n", rslt); + return; +} + +/* Enable OIS through host interface */ +rslt = bmi2_set_ois_interface(BMI2_ENABLE, &dev); +if (rslt != BMI2_OK) { + printf("Error: %d\n", rslt); + return; +} + +/* Disable Advance Power Save Mode through host interface */ +rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &dev); +if (rslt != BMI2_OK) { + printf("Error: %d\n", rslt); + return; +} + +/* Get configurations for OIS through OIS interface for default values */ +rslt = bmi2_ois_get_config(&ois_dev); +if (rslt != BMI2_OK) { + printf("Error: %d\n", rslt); + return; +} + +/* Enable accelerometer and gyroscope for reading OIS data */ +ois_dev.acc_en = BMI2_ENABLE; +ois_dev.gyr_en = BMI2_ENABLE; + +/* Set configurations for OIS through OIS interface */ +rslt = bmi2_ois_set_config(&ois_dev); +if (rslt == BMI2_OK) { + /* Get OIS accelerometer and gyroscope data through OIS interface */ + rslt = bmi2_ois_read_data(sens_sel, 2, &ois_dev); + if (rslt == BMI2_OK) { + /* Print accelerometer data */ + printf("OIS Accel x-axis = %d\t", ois_dev.acc_data.x); + printf("OIS Accel y-axis= %d\t", ois_dev.acc_data.y); + printf("OIS Accel z-axis = %d\r\n", ois_dev.acc_data.z); + + /* Print gyroscope data */ + printf("OIS Gyro x-axis = %d\t", ois_dev.gyr_data.x); + printf("OIS Gyro y-axis= %d\t", ois_dev.gyr_data.y); + printf("OIS Gyro z-axis = %d\r\n", ois_dev.gyr_data.z); + } +} + +if (rslt != BMI2_OK) { + printf("Error code: %d\n", rslt); + return; +} + +/* Enable Advance Power Save Mode through host interface */ +rslt = bmi2_set_adv_power_save(BMI2_ENABLE, &dev); +if (rslt != BMI2_OK) { + printf("Error: %d\n", rslt); + return; +} +``` \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/README.md b/lib/main/BoschSensortec/BMI270-Sensor-API/README.md index 2bf3b9eb6..193b03821 100644 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/README.md +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/README.md @@ -2,18 +2,13 @@ BMI2xy Sensor API > This package contains BMI2xy sensor API -**Note : When updating from 2.19.0 to 2.34.0, the gyroscope offset calibration data cannot be reused. In case of a permanently stored gyroscope offset (inside the sensor NVM or on the host), please perform calibration again with API version 2.34.0. - ## Sensor Overview - The BMI2xy is a small, low power, low noise inertial measurement unit designed for use in mobile applications like augmented reality or indoor navigation which require highly accurate, real-time sensor data. ### Features - Indoor navigation, pedestrian dead-reckoning, step-counting -For more information refer [product page](https://www.bosch-sensortec.com/bst/products/all_products/bmi270) +For more information refer product page [Link](https://www.bosch-sensortec.com/products/motion-sensors/imus/bmi270.html) ---- - -#### Copyright (C) 2019 Bosch Sensortec GmbH +--- \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/betaflight_info.txt b/lib/main/BoschSensortec/BMI270-Sensor-API/betaflight_info.txt index ac8e2ac29..9f8947c9d 100644 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/betaflight_info.txt +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/betaflight_info.txt @@ -3,4 +3,7 @@ This library is used to support the Bosch BMI270 gyro sensor (see drivers/accgyr Library download location: https://github.com/BoschSensortec/BMI270-Sensor-API -The only file that is compiled as part of Betaflight is bmi270.c. This file contains the device microcode that must be uploaded during initialization. +Version: + 2.63.1 + +The only file that is compiled as part of Betaflight is bmi270_maximum_fifo.c. This file contains the device microcode that must be uploaded during initialization. diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2.c b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2.c index f04e04d1d..556039a1a 100644 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2.c +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2.c @@ -30,375 +30,25 @@ * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * -* @file bmi2.c -* @date 2020-01-10 -* @version v2.46.1 +* @file bmi2.c +* @date 2020-11-04 +* @version v2.63.1 * -*//******************************************************************************/ +*/ + +/******************************************************************************/ /*! @name Header Files */ /******************************************************************************/ #include "bmi2.h" -/******************************************************************************/ -/*! @name Macros */ -/******************************************************************************/ -/*! @name Offsets from feature start address for BMI2 feature enable/disable */ -#define ANY_MOT_FEAT_EN_OFFSET UINT8_C(0x03) -#define NO_MOT_FEAT_EN_OFFSET UINT8_C(0x03) -#define SIG_MOT_FEAT_EN_OFFSET UINT8_C(0x0A) -#define STEP_COUNT_FEAT_EN_OFFSET UINT8_C(0x01) -#define GYR_USER_GAIN_FEAT_EN_OFFSET UINT8_C(0x05) -#define HIGH_G_FEAT_EN_OFFSET UINT8_C(0x03) -#define LOW_G_FEAT_EN_OFFSET UINT8_C(0x03) - -/*! @name Mask definitions for BMI2 feature enable/disable */ -#define ANY_NO_MOT_EN_MASK UINT8_C(0x80) -#define TILT_FEAT_EN_MASK UINT8_C(0x01) -#define ORIENT_FEAT_EN_MASK UINT8_C(0x01) -#define SIG_MOT_FEAT_EN_MASK UINT8_C(0x01) -#define STEP_DET_FEAT_EN_MASK UINT8_C(0x08) -#define STEP_COUNT_FEAT_EN_MASK UINT8_C(0x10) -#define STEP_ACT_FEAT_EN_MASK UINT8_C(0x20) -#define GYR_USER_GAIN_FEAT_EN_MASK UINT8_C(0x08) -#define UP_HOLD_TO_WAKE_FEAT_EN_MASK UINT8_C(0x01) -#define GLANCE_FEAT_EN_MASK UINT8_C(0x01) -#define WAKE_UP_FEAT_EN_MASK UINT8_C(0x01) -#define HIGH_G_FEAT_EN_MASK UINT8_C(0x80) -#define LOW_G_FEAT_EN_MASK UINT8_C(0x10) -#define FLAT_FEAT_EN_MASK UINT8_C(0x01) -#define EXT_SENS_SYNC_FEAT_EN_MASK UINT8_C(0x01) -#define GYR_SELF_OFF_CORR_FEAT_EN_MASK UINT8_C(0x02) -#define WRIST_GEST_FEAT_EN_MASK UINT8_C(0x20) -#define WRIST_WEAR_WAKE_UP_FEAT_EN_MASK UINT8_C(0x10) -#define ACTIVITY_RECOG_EN_MASK UINT8_C(0x01) -#define ACC_SELF_TEST_FEAT_EN_MASK UINT8_C(0x02) -#define GYRO_SELF_TEST_CRT_EN_MASK UINT8_C(0x01) -#define ABORT_FEATURE_EN_MASK UINT8_C(0x02) -#define NVM_PREP_FEATURE_EN_MASK UINT8_C(0x04) -#define FREE_FALL_DET_FEAT_EN_MASK UINT8_C(0x01) - -/*! @name Bit position definitions for BMI2 feature enable/disable */ -#define ANY_NO_MOT_EN_POS UINT8_C(0x07) -#define STEP_DET_FEAT_EN_POS UINT8_C(0x03) -#define STEP_COUNT_FEAT_EN_POS UINT8_C(0x04) -#define STEP_ACT_FEAT_EN_POS UINT8_C(0x05) -#define GYR_USER_GAIN_FEAT_EN_POS UINT8_C(0x03) -#define HIGH_G_FEAT_EN_POS UINT8_C(0x07) -#define LOW_G_FEAT_EN_POS UINT8_C(0x04) -#define GYR_SELF_OFF_CORR_FEAT_EN_POS UINT8_C(0x01) -#define WRIST_GEST_FEAT_EN_POS UINT8_C(0x05) -#define WRIST_WEAR_WAKE_UP_FEAT_EN_POS UINT8_C(0x04) -#define ACC_SELF_TEST_FEAT_EN_POS UINT8_C(0x01) -#define ABORT_FEATURE_EN_POS UINT8_C(0x1) -#define NVM_PREP_FEATURE_EN_POS UINT8_C(0x02) - -/*! Primary OIS low pass filter configuration position and mask */ -#define LP_FILTER_EN_MASK UINT8_C(0x01) - -#define LP_FILTER_CONFIG_POS UINT8_C(0x01) -#define LP_FILTER_CONFIG_MASK UINT8_C(0x06) - -#define PRIMARY_OIS_GYR_EN_POS UINT8_C(0x06) -#define PRIMARY_OIS_GYR_EN_MASK UINT8_C(0x40) - -#define PRIMARY_OIS_ACC_EN_POS UINT8_C(0x07) -#define PRIMARY_OIS_ACC_EN_MASK UINT8_C(0x80) - -/*! @name Mask definitions for BMI2 any and no-motion feature configuration */ -#define ANY_NO_MOT_DUR_MASK UINT16_C(0x1FFF) -#define ANY_NO_MOT_X_SEL_MASK UINT16_C(0x2000) -#define ANY_NO_MOT_Y_SEL_MASK UINT16_C(0x4000) -#define ANY_NO_MOT_Z_SEL_MASK UINT16_C(0x8000) -#define ANY_NO_MOT_THRES_MASK UINT16_C(0x07FF) -#define ANY_NO_MOT_OUT_CONF_MASK UINT16_C(0x7800) - -/*! @name Bit position definitions for BMI2 any and no-motion feature - * configuration - */ -#define ANY_NO_MOT_X_SEL_POS UINT8_C(0x0D) -#define ANY_NO_MOT_Y_SEL_POS UINT8_C(0x0E) -#define ANY_NO_MOT_Z_SEL_POS UINT8_C(0x0F) -#define ANY_NO_MOT_OUT_CONF_POS UINT8_C(0x0B) - -/*! @name Mask definitions for BMI2 tilt feature configuration */ -#define TILT_OUT_CONF_MASK UINT16_C(0x001E) - -/*! @name Bit position definitions for BMI2 tilt feature configuration */ -#define TILT_OUT_CONF_POS UINT8_C(0x01) - -/*! @name Mask definitions for BMI2 orientation feature configuration */ -#define ORIENT_UP_DOWN_MASK UINT16_C(0x0002) -#define ORIENT_SYMM_MODE_MASK UINT16_C(0x000C) -#define ORIENT_BLOCK_MODE_MASK UINT16_C(0x0030) -#define ORIENT_THETA_MASK UINT16_C(0x0FC0) -#define ORIENT_HYST_MASK UINT16_C(0x07FF) -#define ORIENT_OUT_CONF_MASK UINT16_C(0x7800) - -/*! @name Bit position definitions for BMI2 orientation feature configuration */ -#define ORIENT_UP_DOWN_POS UINT8_C(0x01) -#define ORIENT_SYMM_MODE_POS UINT8_C(0x02) -#define ORIENT_BLOCK_MODE_POS UINT8_C(0x04) -#define ORIENT_THETA_POS UINT8_C(0x06) -#define ORIENT_OUT_CONF_POS UINT8_C(0x0B) - -/*! @name Mask definitions for BMI2 sig-motion feature configuration */ -#define SIG_MOT_PARAM_1_MASK UINT16_C(0xFFFF) -#define SIG_MOT_PARAM_2_MASK UINT16_C(0xFFFF) -#define SIG_MOT_PARAM_3_MASK UINT16_C(0xFFFF) -#define SIG_MOT_PARAM_4_MASK UINT16_C(0xFFFF) -#define SIG_MOT_PARAM_5_MASK UINT16_C(0xFFFF) -#define SIG_MOT_OUT_CONF_MASK UINT16_C(0x001E) - -/*! @name Bit position definitions for BMI2 sig-motion feature configuration */ -#define SIG_MOT_OUT_CONF_POS UINT8_C(0x01) - -/*! @name Mask definitions for BMI2 parameter configurations */ -#define STEP_COUNT_PARAMS_MASK UINT16_C(0xFFFF) - -/*! @name Mask definitions for BMI2 step-counter/detector feature configuration */ -#define STEP_COUNT_WM_LEVEL_MASK UINT16_C(0x03FF) -#define STEP_COUNT_RST_CNT_MASK UINT16_C(0x0400) -#define STEP_DET_OUT_CONF_MASK UINT16_C(0x000F) -#define STEP_ACT_OUT_CONF_MASK UINT16_C(0x00F0) -#define STEP_BUFFER_SIZE_MASK UINT16_C(0XFF00) - -/*! @name Bit position definitions for BMI2 step-counter/detector feature - * configuration - */ -#define STEP_COUNT_RST_CNT_POS UINT8_C(0x0A) -#define STEP_ACT_OUT_CONF_POS UINT8_C(0x04) -#define STEP_BUFFER_SIZE_POS UINT8_C(0X08) - -/*! @name Mask definitions for BMI2 gyroscope user gain feature - * configuration - */ -#define GYR_USER_GAIN_RATIO_X_MASK UINT16_C(0x07FF) -#define GYR_USER_GAIN_RATIO_Y_MASK UINT16_C(0x07FF) -#define GYR_USER_GAIN_RATIO_Z_MASK UINT16_C(0x07FF) - -/*! @name Mask definitions for BMI2 gyroscope user gain saturation status */ -#define GYR_USER_GAIN_SAT_STAT_X_MASK UINT8_C(0x01) -#define GYR_USER_GAIN_SAT_STAT_Y_MASK UINT8_C(0x02) -#define GYR_USER_GAIN_SAT_STAT_Z_MASK UINT8_C(0x04) -#define G_TRIGGER_STAT_MASK UINT8_C(0x38) - -/*! @name Bit position definitions for BMI2 gyroscope user gain saturation status */ -#define GYR_USER_GAIN_SAT_STAT_Y_POS UINT8_C(0x01) -#define GYR_USER_GAIN_SAT_STAT_Z_POS UINT8_C(0x02) -#define G_TRIGGER_STAT_POS UINT8_C(0x03) - -/*! @name Mask definitions for MSB values of BMI2 gyroscope compensation */ -#define GYR_OFF_COMP_MSB_X_MASK UINT8_C(0x03) -#define GYR_OFF_COMP_MSB_Y_MASK UINT8_C(0x0C) -#define GYR_OFF_COMP_MSB_Z_MASK UINT8_C(0x30) - -/*! @name Bit positions for MSB values of BMI2 gyroscope compensation */ -#define GYR_OFF_COMP_MSB_Y_POS UINT8_C(0x02) -#define GYR_OFF_COMP_MSB_Z_POS UINT8_C(0x04) - -/*! @name Mask definitions for MSB values of BMI2 gyroscope compensation from user input */ -#define GYR_OFF_COMP_MSB_MASK UINT16_C(0x0300) -#define GYR_OFF_COMP_LSB_MASK UINT16_C(0x00FF) - -/*! @name Mask definitions for BMI2 orientation status */ -#define BMI2_ORIENT_DETECT_MASK UINT8_C(0x03) -#define BMI2_ORIENT_FACE_UP_DWN_MASK UINT8_C(0x04) - -/*! @name Bit position definitions for BMI2 orientation status */ -#define BMI2_ORIENT_FACE_UP_DWN_POS UINT8_C(0x02) - -/*! @name Mask definitions for NVM-VFRM error status */ -#define NVM_LOAD_ERR_STATUS_MASK UINT8_C(0x01) -#define NVM_PROG_ERR_STATUS_MASK UINT8_C(0x02) -#define NVM_ERASE_ERR_STATUS_MASK UINT8_C(0x04) -#define NVM_END_EXCEED_STATUS_MASK UINT8_C(0x08) -#define NVM_PRIV_ERR_STATUS_MASK UINT8_C(0x10) -#define VFRM_LOCK_ERR_STATUS_MASK UINT8_C(0x20) -#define VFRM_WRITE_ERR_STATUS_MASK UINT8_C(0x40) -#define VFRM_FATAL_ERR_STATUS_MASK UINT8_C(0x80) - -/*! @name Bit positions for NVM-VFRM error status */ -#define NVM_PROG_ERR_STATUS_POS UINT8_C(0x01) -#define NVM_ERASE_ERR_STATUS_POS UINT8_C(0x02) -#define NVM_END_EXCEED_STATUS_POS UINT8_C(0x03) -#define NVM_PRIV_ERR_STATUS_POS UINT8_C(0x04) -#define VFRM_LOCK_ERR_STATUS_POS UINT8_C(0x05) -#define VFRM_WRITE_ERR_STATUS_POS UINT8_C(0x06) -#define VFRM_FATAL_ERR_STATUS_POS UINT8_C(0x07) - -/*! @name Mask definitions for accelerometer self-test status */ -#define ACC_SELF_TEST_DONE_MASK UINT8_C(0x01) -#define ACC_X_OK_MASK UINT8_C(0x02) -#define ACC_Y_OK_MASK UINT8_C(0x04) -#define ACC_Z_OK_MASK UINT8_C(0x08) - -/*! @name Bit Positions for accelerometer self-test status */ -#define ACC_X_OK_POS UINT8_C(0x01) -#define ACC_Y_OK_POS UINT8_C(0x02) -#define ACC_Z_OK_POS UINT8_C(0x03) - -/*! @name Mask definitions for BMI2 uphold to wake feature configuration */ -#define UP_HOLD_TO_WAKE_OUT_CONF_MASK UINT16_C(0x001E) - -/*! @name Bit position definitions for BMI2 uphold to wake feature configuration */ -#define UP_HOLD_TO_WAKE_OUT_CONF_POS UINT8_C(0x01) - -/*! @name Mask definitions for BMI2 glance detector feature configuration */ -#define GLANCE_DET_OUT_CONF_MASK UINT16_C(0x001E) - -/*! @name Bit position definitions for BMI2 glance detector feature - * configuration - */ -#define GLANCE_DET_OUT_CONF_POS UINT8_C(0x01) - -/*! @name Mask definitions for BMI2 wake-up feature configuration */ -#define WAKE_UP_SENSITIVITY_MASK UINT16_C(0x000E) -#define WAKE_UP_SINGLE_TAP_EN_MASK UINT16_C(0x0010) -#define WAKE_UP_OUT_CONF_MASK UINT16_C(0x01E0) - -/*! @name Bit position definitions for BMI2 wake-up feature configuration */ -#define WAKE_UP_SENSITIVITY_POS UINT8_C(0x01) -#define WAKE_UP_SINGLE_TAP_EN_POS UINT8_C(0x04) -#define WAKE_UP_OUT_CONF_POS UINT8_C(0x05) - -/*! @name Mask definitions for BMI2 high-g feature configuration */ -#define HIGH_G_THRES_MASK UINT16_C(0x7FFF) -#define HIGH_G_HYST_MASK UINT16_C(0x0FFF) -#define HIGH_G_X_SEL_MASK UINT16_C(0x1000) -#define HIGH_G_Y_SEL_MASK UINT16_C(0x2000) -#define HIGH_G_Z_SEL_MASK UINT16_C(0x4000) -#define HIGH_G_DUR_MASK UINT16_C(0x0FFF) -#define HIGH_G_OUT_CONF_MASK UINT16_C(0xF000) - -/*! @name Bit position definitions for BMI2 high-g feature configuration */ -#define HIGH_G_OUT_CONF_POS UINT8_C(0x0C) -#define HIGH_G_X_SEL_POS UINT8_C(0x0C) -#define HIGH_G_Y_SEL_POS UINT8_C(0x0D) -#define HIGH_G_Z_SEL_POS UINT8_C(0x0E) - -/*! @name Mask definitions for BMI2 low-g feature configuration */ -#define LOW_G_THRES_MASK UINT16_C(0x7FFF) -#define LOW_G_HYST_MASK UINT16_C(0x0FFF) -#define LOW_G_DUR_MASK UINT16_C(0x0FFF) -#define LOW_G_OUT_CONF_MASK UINT16_C(0xF000) - -/*! @name Mask definitions for BMI2 free-fall detection feature configuration */ -#define FREE_FALL_OUT_CONF_MASK UINT16_C(0x001E) -#define FREE_FALL_ACCEL_SETT_MASK UINT16_C(0xFFFF) - -/*! @name Bit position definitions for BMI2 free-fall detection feature configuration */ -#define FREE_FALL_OUT_CONF_POS UINT8_C(0x01) - -/*! @name Bit position definitions for BMI2 low-g feature configuration */ -#define LOW_G_OUT_CONF_POS UINT8_C(0x0C) - -/*! @name Mask definitions for BMI2 flat feature configuration */ -#define FLAT_THETA_MASK UINT16_C(0x007E) -#define FLAT_BLOCK_MASK UINT16_C(0x0180) -#define FLAT_OUT_CONF_MASK UINT16_C(0x1E00) -#define FLAT_HYST_MASK UINT16_C(0x003F) -#define FLAT_HOLD_TIME_MASK UINT16_C(0x3FC0) - -/*! @name Bit position definitions for BMI2 flat feature configuration */ -#define FLAT_THETA_POS UINT8_C(0x01) -#define FLAT_BLOCK_POS UINT8_C(0x07) -#define FLAT_OUT_CONF_POS UINT8_C(0x09) -#define FLAT_HOLD_TIME_POS UINT8_C(0x06) - -/*! @name Mask definitions for BMI2 external sensor sync configuration */ -#define EXT_SENS_SYNC_OUT_CONF_MASK UINT16_C(0x001E) - -/*! @name Bit position definitions for external sensor sync configuration */ -#define EXT_SENS_SYNC_OUT_CONF_POS UINT8_C(0x01) - -/*! @name Mask definitions for BMI2 wrist gesture configuration */ -#define WRIST_GEST_WEAR_ARM_MASK UINT16_C(0x0010) -#define WRIST_GEST_OUT_CONF_MASK UINT16_C(0x000F) - -/*! @name Bit position definitions for wrist gesture configuration */ -#define WRIST_GEST_WEAR_ARM_POS UINT8_C(0x04) - -/*! @name Mask definitions for BMI2 wrist wear wake-up configuration */ -#define WRIST_WAKE_UP_OUT_CONF_MASK UINT16_C(0x000F) - -/*! @name Mask definition for BMI2 wrist wear wake-up configuration for wearable variant */ -#define WRIST_WAKE_UP_ANGLE_LR_MASK UINT16_C(0x00FF) -#define WRIST_WAKE_UP_ANGLE_LL_MASK UINT16_C(0xFF00) -#define WRIST_WAKE_UP_ANGLE_PD_MASK UINT16_C(0x00FF) -#define WRIST_WAKE_UP_ANGLE_PU_MASK UINT16_C(0xFF00) -#define WRIST_WAKE_UP_MIN_DUR_MOVED_MASK UINT16_C(0x00FF) -#define WRIST_WAKE_UP_MIN_DUR_QUITE_MASK UINT16_C(0xFF00) - -/*! @name Bit position definition for BMI2 wrist wear wake-up configuration for wearable variant */ -#define WRIST_WAKE_UP_ANGLE_LL_POS UINT16_C(0x0008) -#define WRIST_WAKE_UP_ANGLE_PU_POS UINT16_C(0x0008) -#define WRIST_WAKE_UP_MIN_DUR_QUITE_POS UINT16_C(0x0008) - -/*! @name Macros to define values of BMI2 axis and its sign for re-map - * settings - */ -#define MAP_X_AXIS UINT8_C(0x00) -#define MAP_Y_AXIS UINT8_C(0x01) -#define MAP_Z_AXIS UINT8_C(0x02) -#define MAP_POSITIVE UINT8_C(0x00) -#define MAP_NEGATIVE UINT8_C(0x01) - -/*! @name Mask definitions of BMI2 axis re-mapping */ -#define X_AXIS_MASK UINT8_C(0x03) -#define X_AXIS_SIGN_MASK UINT8_C(0x04) -#define Y_AXIS_MASK UINT8_C(0x18) -#define Y_AXIS_SIGN_MASK UINT8_C(0x20) -#define Z_AXIS_MASK UINT8_C(0xC0) -#define Z_AXIS_SIGN_MASK UINT8_C(0x01) - -/*! @name Bit position definitions of BMI2 axis re-mapping */ -#define X_AXIS_SIGN_POS UINT8_C(0x02) -#define Y_AXIS_POS UINT8_C(0x03) -#define Y_AXIS_SIGN_POS UINT8_C(0x05) -#define Z_AXIS_POS UINT8_C(0x06) - -/*! @name Macros to define polarity */ -#define NEG_SIGN INT16_C(-1) -#define POS_SIGN INT16_C(1) - -/*! @name Macro to define related to CRT */ -#define CRT_READY_FOR_DOWNLOAD_US UINT16_C(2000) -#define CRT_READY_FOR_DOWNLOAD_RETRY UINT8_C(100) - -#define CRT_WAIT_RUNNING_US UINT16_C(10000) -#define CRT_WAIT_RUNNING_RETRY_EXECUTION UINT8_C(200) - -#define CRT_MIN_BURST_WORD_LENGTH UINT8_C(2) -#define CRT_MAX_BURST_WORD_LENGTH UINT16_C(255) - -#define ACC_FOC_2G_REF UINT16_C(16384) -#define ACC_FOC_4G_REF UINT16_C(8192) -#define ACC_FOC_8G_REF UINT16_C(4096) -#define ACC_FOC_16G_REF UINT16_C(2048) - -#define GYRO_FOC_NOISE_LIMIT_NEGATIVE INT8_C(-20) -#define GYRO_FOC_NOISE_LIMIT_POSITIVE INT8_C(20) - -/* reference value with positive and negative noise range in lsb */ -#define ACC_2G_MAX_NOISE_LIMIT (ACC_FOC_2G_REF + UINT16_C(255)) -#define ACC_2G_MIN_NOISE_LIMIT (ACC_FOC_2G_REF - UINT16_C(255)) -#define ACC_4G_MAX_NOISE_LIMIT (ACC_FOC_4G_REF + UINT16_C(255)) -#define ACC_4G_MIN_NOISE_LIMIT (ACC_FOC_4G_REF - UINT16_C(255)) -#define ACC_8G_MAX_NOISE_LIMIT (ACC_FOC_8G_REF + UINT16_C(255)) -#define ACC_8G_MIN_NOISE_LIMIT (ACC_FOC_8G_REF - UINT16_C(255)) -#define ACC_16G_MAX_NOISE_LIMIT (ACC_FOC_16G_REF + UINT16_C(255)) -#define ACC_16G_MIN_NOISE_LIMIT (ACC_FOC_16G_REF - UINT16_C(255)) - -#define BMI2_FOC_SAMPLE_LIMIT UINT8_C(128) - /***************************************************************************/ /*! Local structures ****************************************************************************/ /*! @name Structure to define the difference in accelerometer values */ -struct selftest_delta_limit +struct bmi2_selftest_delta_limit { /*! X data */ int32_t x; @@ -410,32 +60,8 @@ struct selftest_delta_limit int32_t z; }; -/*! @name Structure to store the local copy of the re-mapped axis and - * the value of its sign for register settings - */ -struct axes_remap -{ - /*! Re-mapped x-axis */ - uint8_t x_axis; - - /*! Re-mapped y-axis */ - uint8_t y_axis; - - /*! Re-mapped z-axis */ - uint8_t z_axis; - - /*! Re-mapped x-axis sign */ - uint8_t x_axis_sign; - - /*! Re-mapped y-axis sign */ - uint8_t y_axis_sign; - - /*! Re-mapped z-axis sign */ - uint8_t z_axis_sign; -}; - /*! @name Structure to store temporary accelerometer/gyroscope values */ -struct foc_temp_value +struct bmi2_foc_temp_value { /*! X data */ int32_t x; @@ -448,7 +74,7 @@ struct foc_temp_value }; /*! @name Structure to store accelerometer data deviation from ideal value */ -struct offset_delta +struct bmi2_offset_delta { /*! X axis */ int16_t x; @@ -461,7 +87,7 @@ struct offset_delta }; /*! @name Structure to store accelerometer offset values */ -struct accel_offset +struct bmi2_accel_offset { /*! offset X data */ uint8_t x; @@ -484,9 +110,8 @@ struct accel_offset * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t write_config_file(struct bmi2_dev *dev); @@ -498,9 +123,8 @@ static int8_t write_config_file(struct bmi2_dev *dev); * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t set_config_load(uint8_t enable, struct bmi2_dev *dev); @@ -512,540 +136,11 @@ static int8_t set_config_load(uint8_t enable, struct bmi2_dev *dev); * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t upload_file(const uint8_t *config_data, uint16_t index, uint16_t write_len, struct bmi2_dev *dev); -/*! - * @brief This internal API enables the selected sensor/features. - * - * @param[in] sensor_sel : Selects the desired sensor. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_SET_APS_FAIL - Error: Set Advance Power Save Fail - */ -static int8_t sensor_enable(uint32_t sensor_sel, struct bmi2_dev *dev); - -/*! - * @brief This internal API disables the selected sensor/features. - * - * @param[in] sensor_sel : Selects the desired sensor. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_SET_APS_FAIL - Error: Set Advance Power Save Fail - */ -static int8_t sensor_disable(uint32_t sensor_sel, struct bmi2_dev *dev); - -/*! - * @brief This internal API selects the sensors/features to be enabled or - * disabled. - * - * @param[in] sens_list : Pointer to select the sensor. - * @param[in] n_sens : Number of sensors selected. - * @param[out] sensor_sel : Gets the selected sensor. - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - */ -static int8_t select_sensor(const uint8_t *sens_list, uint8_t n_sens, uint32_t *sensor_sel); - -/*! - * @brief This internal API is used to enable/disable any-motion feature. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables any-motion. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables any-motion. - * BMI2_ENABLE | Enables any-motion. - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_any_motion(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to enable/disable no-motion feature. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables no-motion. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables no-motion. - * BMI2_ENABLE | Enables no-motion. - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_no_motion(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to enable/disable sig-motion feature. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables sig-motion. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables sig-motion. - * BMI2_ENABLE | Enables sig-motion. - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_sig_motion(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to enable/disable step detector feature. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables step-detector. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables step detector - * BMI2_ENABLE | Enables step detector - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_step_detector(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to enable/disable step counter feature. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables step counter. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables step counter - * BMI2_ENABLE | Enables step counter - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_step_counter(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to enable/disable step activity detection. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables step activity. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables step activity - * BMI2_ENABLE | Enables step activity - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_step_activity(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to enable/disable tilt feature. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables tilt. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables tilt - * BMI2_ENABLE | Enables tilt - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_tilt(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to enable/disable uphold to wake feature. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables uphold to wake. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables uphold to wake - * BMI2_ENABLE | Enables uphold to wake - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_up_hold_to_wake(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to enable/disable glance detector feature. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables glance. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables glance detector - * BMI2_ENABLE | Enables glance detector - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_glance_detector(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to enable/disable wake-up feature through - * single or double tap. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables single or double tap. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables single/double tap. - * BMI2_ENABLE | Enables single/double tap - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_wake_up(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to enable/disable orientation feature. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables orientation. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables orientation - * BMI2_ENABLE | Enables orientation - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_orientation(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to enable/disable high-g feature. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables high-g. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables high-g - * BMI2_ENABLE | Enables high-g - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_high_g(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to enable/disable low-g feature. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables low-g. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables low-g - * BMI2_ENABLE | Enables low-g - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_low_g(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to enable/disable flat feature. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables flat. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables flat - * BMI2_ENABLE | Enables flat - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_flat(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to enable/disable ext-sensor-sync feature. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables ext-sensor-sync. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables ext-sensor-sync - * BMI2_ENABLE | Enables ext-sensor-sync - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_ext_sens_sync(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API gives an option to enable offset correction - * feature of gyroscope, either internally or by the host. - * - * @param[in] enable : Enables/Disables self-offset correction. - * @param[in] dev : Structure instance of bmi2_dev. - * - * enable | Description - * -------------|--------------- - * BMI2_ENABLE | gyroscope offset correction values are set internally - * BMI2_DISABLE | gyroscope offset correction values has to be set by host - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_gyro_self_offset_corr(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to enable/disable gyroscope user gain - * feature. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables gyroscope user gain. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables gyroscope user gain - * BMI2_ENABLE | Enables gyroscope user gain - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_gyro_user_gain(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API enables the wrist gesture feature. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables wrist gesture. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables wrist gesture - * BMI2_ENABLE | Enables wrist gesture - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_wrist_gesture(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API enables the wrist wear wake up feature. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables wrist wear wake up. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables wrist wear wake up - * BMI2_ENABLE | Enables wrist wear wake up - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_wrist_wear_wake_up(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API enables the wrist gesture feature for wearable variant. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables wrist gesture. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables wrist gesture - * BMI2_ENABLE | Enables wrist gesture - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_wrist_gesture_wh(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API enables the wrist wear wake up feature for wearable variant. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables wrist wear wake up. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables wrist wear wake up - * BMI2_ENABLE | Enables wrist wear wake up - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_wrist_wear_wake_up_wh(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API enables/disables the activity recognition feature. - * - * @param[in] enable : Enables/Disables activity recognition. - * @param[in] dev : Structure instance of bmi2_dev. - * - * enable | Description - * -------------|--------------- - * BMI2_ENABLE | Enables activity recognition. - * BMI2_DISABLE | Disables activity recognition. - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_act_recog(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API gives an option to enable accelerometer self-test - * in the feature register. - * - * @param[in] enable : Enables/Disables accelerometer self-test. - * @param[in] dev : Structure instance of bmi2_dev. - * - * enable | Description - * -------------|--------------- - * BMI2_ENABLE | Enables accelerometer self-test. - * BMI2_DISABLE | Disables accelerometer self-test. - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_feat_accel_self_test(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API gives an option to enable low pass filter - * in the feature register. - * - * @param[in] enable : Enables/Disables accelerometer self-test. - * @param[in] dev : Structure instance of bmi2_dev. - * - * enable | Description - * -------------|--------------- - * BMI2_ENABLE | Enables low pass filter. - * BMI2_DISABLE | Disables low pass filter. - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_primary_ois_low_pass_filter(uint8_t enable, struct bmi2_dev *dev); - /*! * @brief This internal API sets accelerometer configurations like ODR, * bandwidth, performance mode and g-range. @@ -1054,13 +149,8 @@ static int8_t set_primary_ois_low_pass_filter(uint8_t enable, struct bmi2_dev *d * @param[in,out] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_ACC_INVALID_CFG - Error: Invalid accelerometer configuration - * @retval BMI2_E_ACC_GYR_INVALID_CFG - Error: Invalid accelerometer and - * gyroscope configuration - * + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t set_accel_config(struct bmi2_accel_config *config, struct bmi2_dev *dev); @@ -1072,13 +162,11 @@ static int8_t set_accel_config(struct bmi2_accel_config *config, struct bmi2_dev * @param[in, out] perf_mode : Pointer to performance mode set by the user. * @param[in, out] dev : Structure instance of bmi2_dev. * - * @return Result of API execution status - * * @note dev->info contains two warnings: BMI2_I_MIN_VALUE and BMI2_I_MAX_VALUE * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t validate_bw_perf_mode(uint8_t *bandwidth, uint8_t *perf_mode, struct bmi2_dev *dev); @@ -1090,12 +178,11 @@ static int8_t validate_bw_perf_mode(uint8_t *bandwidth, uint8_t *perf_mode, stru * @param[in, out] range : Pointer to range value set by the user. * @param[in, out] dev : Structure instance of bmi2_dev. * - * @return Result of API execution status - * * @note dev->info contains two warnings: BMI2_I_MIN_VALUE and BMI2_I_MAX_VALUE * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t validate_odr_range(uint8_t *odr, uint8_t *range, struct bmi2_dev *dev); @@ -1107,12 +194,8 @@ static int8_t validate_odr_range(uint8_t *odr, uint8_t *range, struct bmi2_dev * * @param[in,out] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_GYRO_INVALID_CFG - Error: Invalid accelerometer configuration - * @retval BMI2_E_ACC_GYR_INVALID_CFG - Error: Invalid accelerometer and - * gyroscope configuration + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t set_gyro_config(struct bmi2_gyro_config *config, struct bmi2_dev *dev); @@ -1123,12 +206,11 @@ static int8_t set_gyro_config(struct bmi2_gyro_config *config, struct bmi2_dev * * @param[in] config : Structure instance of bmi2_gyro_config. * @param[in] dev : Structure instance of bmi2_dev. * - * @return Result of API execution status - * * @note dev->info contains two warnings: BMI2_I_MIN_VALUE and BMI2_I_MAX_VALUE * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t validate_gyro_config(struct bmi2_gyro_config *config, struct bmi2_dev *dev); @@ -1139,15 +221,10 @@ static int8_t validate_gyro_config(struct bmi2_gyro_config *config, struct bmi2_ * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_ACC_INVALID_CFG - Error: Invalid accelerometer configuration - * @retval BMI2_E_GYRO_INVALID_CFG - Error: Invalid accelerometer configuration - * @retval BMI2_E_ACC_GYR_INVALID_CFG - Error: Invalid accelerometer and - * gyroscope configuration + * @retval 0 -> Success + * @retval < 0 -> Fail */ -static int8_t cfg_error_status(const struct bmi2_dev *dev); +static int8_t cfg_error_status(struct bmi2_dev *dev); /*! * @brief This internal API: @@ -1161,231 +238,11 @@ static int8_t cfg_error_status(const struct bmi2_dev *dev); * @param[in, out] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_AUX_BUSY - Error: Auxiliary sensor is busy + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t set_aux_config(struct bmi2_aux_config *config, struct bmi2_dev *dev); -/*! - * @brief This internal API enables/disables auxiliary interface. - * - * @param[in] config : Structure instance of bmi2_aux_config. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - */ -static int8_t set_aux_interface(const struct bmi2_aux_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets auxiliary configurations like manual/auto mode - * FCU write command enable and read burst length for both data and manual mode. - * - * @param[in] config : Structure instance of bmi2_aux_config. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @note Auxiliary sensor should not be busy when configuring aux_i2c_addr, - * man_rd_burst_len, aux_rd_burst_len and aux_rd_addr. - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_AUX_BUSY - Error: Auxiliary sensor is busy - */ -static int8_t config_aux_interface(const struct bmi2_aux_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API triggers read out offset and sets ODR of the - * auxiliary sensor. - * - * @param[in] config : Structure instance of bmi2_aux_config. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - */ -static int8_t config_aux(const struct bmi2_aux_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API validates auxiliary configuration set by the user. - * - * @param[in, out] config : Structure instance of bmi2_aux_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * - * @note dev->info contains two warnings: BMI2_I_MIN_VALUE and BMI2_I_MAX_VALUE - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - */ -static int8_t validate_aux_config(struct bmi2_aux_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets any-motion configurations like axes select, - * duration, threshold and output-configuration. - * - * @param[in] config : Structure instance of bmi2_any_motion_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *---------------------------------------------------------------------------- - * bmi2_any_motion_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * | Defines the number of consecutive data points for - * | which the threshold condition must be respected, - * | for interrupt assertion. It is expressed in 50 Hz - * duration | samples (20 msec). - * | Range is 0 to 163sec. - * | Default value is 5 = 100ms. - * -------------------------|--------------------------------------------------- - * | Slope threshold value for in 5.11g format. - * threshold | Range is 0 to 1g. - * | Default value is 0xAA = 83mg. - * -------------------------|--------------------------------------------------- - * x_sel, y_sel, z_sel | Selects the feature on a per-axis basis - * -------------------------|--------------------------------------------------- - * | Enable bits for enabling output into the - * out_conf | register status bits and, if desired, onto the - * | interrupt pin. - * @endverbatim - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_any_motion_config(const struct bmi2_any_motion_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets no-motion configurations like axes select, - * duration, threshold and output-configuration. - * - * @param[in] config : Structure instance of bmi2_no_motion_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *---------------------------------------------------------------------------- - * bmi2_no_motion_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * | Defines the number of consecutive data points for - * | which the threshold condition must be respected, - * | for interrupt assertion. It is expressed in 50 Hz - * duration | samples (20 msec). - * | Range is 0 to 163sec. - * | Default value is 5 = 100ms. - * -------------------------|--------------------------------------------------- - * | Slope threshold value for in 5.11g format. - * threshold | Range is 0 to 1g. - * | Default value is 0xAA = 83mg. - * -------------------------|--------------------------------------------------- - * x_sel, y_sel, z_sel | Selects the feature on a per-axis basis - * -------------------------|--------------------------------------------------- - * | Enable bits for enabling output into the - * out_conf | register status bits and, if desired, onto the - * | interrupt pin. - * @endverbatim - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_no_motion_config(const struct bmi2_no_motion_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets sig-motion configurations like block-size, - * output-configuration and other parameters. - * - * @param[in] config : Structure instance of bmi2_sig_motion_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - *---------------------------------------------------------------------------- - * bmi2_sig_motion_config | - * Structure parameters | Description - * -------------------------|--------------------------------------------------- - * | Defines the duration after which the significant - * block_size | motion interrupt is triggered. It is expressed in - * | 50 Hz samples (20 ms). Default value is 0xFA=5sec. - *--------------------------|--------------------------------------------------- - * | Enable bits for enabling output into the - * out_conf | register status bits and, if desired, onto the - * | interrupt pin. - *--------------------------|--------------------------------------------------- - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_sig_motion_config(const struct bmi2_sig_motion_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets step counter parameter configurations. - * - * @param[in] step_count_params : Array that stores parameters 1 to 25. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_step_count_params_config(const uint16_t *step_count_params, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets step counter/detector/activity configurations. - * - * @param[in] config : Structure instance of bmi2_step_config. - * @param[in] dev : Structure instance of bmi2_dev. - * - *--------------------------------------------------------------------------- - * bmi2_step_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * | The Step-counter will trigger output every time - * | the number of steps are counted. Holds implicitly - * water-mark level | a 20x factor, so the range is 0 to 10230, - * | with resolution of 20 steps. - * -------------------------|--------------------------------------------------- - * reset counter | Flag to reset the counted steps. - * -------------------------|--------------------------------------------------- - * | Enable bits for enabling output into the - * step_det_out_conf | register status bits and, if desired, onto the - * | interrupt pin for step detector. - * -------------------------|--------------------------------------------------- - * | Enable bits for enabling output into the - * step_act_out_conf | register status bits and, if desired, onto the - * | interrupt pin for step activity. - * -------------------------|--------------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_step_config(const struct bmi2_step_config *config, struct bmi2_dev *dev); - /*! * @brief This internal API sets gyroscope user-gain configurations like gain * update value for x, y and z-axis. @@ -1406,453 +263,65 @@ static int8_t set_step_config(const struct bmi2_step_config *config, struct bmi2 * @endverbatim * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t set_gyro_user_gain_config(const struct bmi2_gyro_user_gain_config *config, struct bmi2_dev *dev); /*! - * @brief This internal API sets tilt configurations like output-configuration. + * @brief This internal API enables/disables auxiliary interface. * - * @param[in] config : Structure instance of bmi2_tilt_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *---------------------------------------------------------------------------- - * bmi2_tilt_config | - * Structure parameters | Description - *------------------------- |-------------------------------------------------- - * | Enable bits for enabling output into the - * out_conf | register status bits and, if desired, onto the - * | interrupt pin. - * ------------------------ |--------------------------------------------------- - * @endverbatim + * @param[in] config : Structure instance of bmi2_aux_config. + * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page + * @retval 0 -> Success + * @retval < 0 -> Fail */ -static int8_t set_tilt_config(const struct bmi2_tilt_config *config, struct bmi2_dev *dev); +static int8_t set_aux_interface(const struct bmi2_aux_config *config, struct bmi2_dev *dev); /*! - * @brief This internal API sets uphold to wake configurations like output - * configuration. + * @brief This internal API sets auxiliary configurations like manual/auto mode + * FCU write command enable and read burst length for both data and manual mode. * - * @param[in] config : Structure instance of bmi2_uphold_to_wake_config. - * @param[in, out] dev : Structure instance of bmi2_dev. + * @param[in] config : Structure instance of bmi2_aux_config. + * @param[in] dev : Structure instance of bmi2_dev. * - * @verbatim - *---------------------------------------------------------------------------- - * bmi2_uphold_to_wake_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * | Enable bits for enabling output into the - * out_conf | register status bits and, if desired, onto the - * | interrupt pin. - * -------------------------|--------------------------------------------------- - * @endverbatim + * @note Auxiliary sensor should not be busy when configuring aux_i2c_addr, + * man_rd_burst_len, aux_rd_burst_len and aux_rd_addr. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page + * @retval 0 -> Success + * @retval < 0 -> Fail */ -static int8_t set_up_hold_to_wake_config(const struct bmi2_up_hold_to_wake_config *config, struct bmi2_dev *dev); +static int8_t config_aux_interface(const struct bmi2_aux_config *config, struct bmi2_dev *dev); /*! - * @brief This internal API sets glance detector configurations like output - * configuration. + * @brief This internal API triggers read out offset and sets ODR of the + * auxiliary sensor. * - * @param[in] config : Structure instance of bmi2_glance_det_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *---------------------------------------------------------------------------- - * bmi2_glance_det_config| - * Structure parameters | Description - *-------------------------|-------------------------------------------------- - * | Enable bits for enabling output into the - * out_conf | register status bits and, if desired, onto the - * | interrupt pin. - * ------------------------|---------------------------------------------------- - * @endverbatim + * @param[in] config : Structure instance of bmi2_aux_config. + * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page + * @retval 0 -> Success + * @retval < 0 -> Fail */ -static int8_t set_glance_detect_config(const struct bmi2_glance_det_config *config, struct bmi2_dev *dev); +static int8_t config_aux(const struct bmi2_aux_config *config, struct bmi2_dev *dev); /*! - * @brief This internal API sets wake-up configurations like sensitivity, - * single/double tap enable and output-configuration. + * @brief This internal API validates auxiliary configuration set by the user. * - * @param[in] config : Structure instance of bmi2_wake_up_config. - * @param[in, out] dev : Structure instance of bmi2_dev. + * @param[in, out] config : Structure instance of bmi2_aux_config. + * @param[in, out] dev : Structure instance of bmi2_dev. * - * @verbatim - *----------------------------------------------------------------------------- - * bmi2_wake_up_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * | Configures wake-up sensitivity, the range goes - * sensitivity | from 0 (high sensitive) to 7 (low sensitive). - * -------------------------|--------------------------------------------------- - * | Enable - Single Tap detection - * single_tap_en | Disable- Double Tap detection (Default value) - * -------------------------|--------------------------------------------------- - * | Enable bits for enabling output into the - * out_conf | register status bits and, if desired, onto the - * | interrupt pin. - * -------------------------|--------------------------------------------------- - * @endverbatim + * @note dev->info contains two warnings: BMI2_I_MIN_VALUE and BMI2_I_MAX_VALUE * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page + * @retval 0 -> Success + * @retval < 0 -> Fail */ -static int8_t set_wake_up_config(const struct bmi2_wake_up_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets orientation configurations like upside/down - * detection, symmetrical modes, blocking mode, theta, hysteresis and output - * configuration. - * - * @param[in] config : Structure instance of bmi2_orient_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *----------------------------------------------------------------------------- - * bmi2_orient_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * upside/down | Enables upside/down detection, if set to 1. - * detection | - * -------------------------|--------------------------------------------------- - * | Sets the mode: - * mode | Values 0 or 3 - symmetrical. - * | Value 1 - high asymmetrical - * | Value 2 - low asymmetrical - * -------------------------|--------------------------------------------------- - * | Enable - no orientation interrupt will be set. - * blocking | Default value: 3, the most restrictive blocking. - * -------------------------|--------------------------------------------------- - * | Threshold angle used in blocking mode. - * theta | Theta = 64 * (tan(angle)^2) - * | Default value: 40, equivalent to 38 degrees angle. - * -------------------------|--------------------------------------------------- - * | Acceleration hysteresis for Orientation detection - * | is expressed in 5.11g format. - * hysteresis | Default value is 128 = 0.0625g. - * | Range is 0 to 1g. - * -------------------------|--------------------------------------------------- - * | Enable bits for enabling output into the - * out_conf | register status bits and, if desired, onto the - * | interrupt pin. - * -------------------------|--------------------------------------------------- - * - * @endverbatim - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_orient_config(const struct bmi2_orient_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets high-g configurations like threshold, - * hysteresis, duration, and output configuration. - * - * @param[in] config : Structure instance of bmi2_high_g_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *----------------------------------------------------------------------------- - * bmi2_high_g_config | - * Structure parameter | Description - *--------------------------|-------------------------------------------------- - * threshold | The acceleration threshold above which the - * | high_g motion is signaled. - * | Holds threshold in 5.11 g format. - * | Default is 3072 = 1.5 g. - * | Range is 0 to 16g. - * -------------------------|--------------------------------------------------- - * | Acceleration hysteresis for high-g detection - * | is expressed in 1.11g format. - * hysteresis | Default value is 256 = 0.125 g. - * | Range is 0 to 2g. - * | Should be smaller than threshold. - * -------------------------|--------------------------------------------------- - * | Holds the duration in 200 Hz samples (5 ms) for - * | which the threshold has to be exceeded. - * duration | Default value 4 = 20 msec. - * | Range is 0 to 20sec. - * -------------------------|--------------------------------------------------- - * x_sel, y_sel, z_sel | Selects the feature on a per-axis basis - * -------------------------|--------------------------------------------------- - * | Enable bits for enabling output into the - * out_conf | register status bits and, if desired, onto the - * | interrupt pin. - * -------------------------|--------------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_high_g_config(const struct bmi2_high_g_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets low-g configurations like threshold, - * hysteresis, duration, and output configuration. - * - * @param[in] config : Structure instance of bmi2_low_g_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *---------------------------------------------------------------------------- - * bmi2_low_g_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * threshold | The acceleration threshold above which the - * | low-g motion is signaled. - * | Holds threshold in 5.11 g format. - * | Default is 3072 = 1.5 g. - * -------------------------|--------------------------------------------------- - * | Acceleration hysteresis for low-g detection - * hysteresis | is expressed in 1.11g format. - * | Default value is 512 = 0.250 g. - * | Should be smaller than threshold. - * -------------------------|--------------------------------------------------- - * | Holds the duration in 50 Hz samples (20 ms) for - * | which the threshold has to be exceeded. - * duration | Default value 0 = 1 validation sample = (0+1)*20 - * | = 20 ms. - * -------------------------|--------------------------------------------------- - * | Enable bits for enabling output into the - * out_conf | register status bits and, if desired, onto the - * | interrupt pin. - * -------------------------|--------------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_low_g_config(const struct bmi2_low_g_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets flat configurations like theta, blocking, - * hold-time, hysteresis, and output configuration. - * - * @param[in] config : Structure instance of bmi2_flat_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *---------------------------------------------------------------------------- - * bmi2_flat_config | - * Structure parameters| Description - *--------------------------|-------------------------------------------------- - * | Sets the theta angle, used for detecting flat - * | position. - * theta | Theta = 64 * (tan(angle)^2); - * | Default value is 8, equivalent to 20 degrees angle - * -------------------------|--------------------------------------------------- - * | Hysteresis for Theta Flat detection. - * hysteresis | Default value is 9 = 2.5 degrees, corresponding - * | to the default Theta angle of 20 degrees. - * -------------------------|--------------------------------------------------- - * | Sets the blocking mode. If blocking is set, no - * | Flat interrupt will be triggered. - * blocking | Default value is 2, the most restrictive blocking - * | mode. - * -------------------------|--------------------------------------------------- - * | Holds the duration in 50Hz samples for which the - * | condition has to be respected. - * hold-time | Default value is 32 = 640 m-sec. - * | Range is 0 to 5.1 sec. - * -------------------------|--------------------------------------------------- - * | Enable bits for enabling output into the - * out_conf | register status bits and, if desired, onto the - * | interrupt pin. - * -------------------------|--------------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_flat_config(const struct bmi2_flat_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets external sensor sync configurations like output - * configuration. - * - * @param[out] config : Structure instance of bmi2_ext_sens_sync_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - *---------------------------------------------------------------------------- - * bmi2_ext_sens_sync_config | - * Structure parameters | Description - * -----------------------------|---------------------------------------------- - * |Enable bits for enabling output into the - * out_conf |register status bits and, if desired, onto the - * |interrupt pin. - * -----------------------------|---------------------------------------------- - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_ext_sens_sync_config(const struct bmi2_ext_sens_sync_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets wrist gesture configurations like wearable-arm, - * and output-configuration. - * - * @param[in] config : Structure instance of bmi2_wrist_gest_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *----------------------------------------------------------------------------- - * bmi2_wrist_gest_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * | Device in left (0) or right (1) arm. By default, - * wear_arm | the wearable device is assumed to be in left arm - * | i.e. default value is 0. - * -------------------------|--------------------------------------------------- - * | Enable bits for enabling output into the - * out_conf | register status bits and, if desired, onto the - * | interrupt pin. - * -------------------------|--------------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_wrist_gest_config(const struct bmi2_wrist_gest_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets wrist wear wake-up configurations like - * output-configuration. - * - * @param[in] config : Structure instance of - * bmi2_wrist_wear_wake_up_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *----------------------------------------------------------------------------- - * bmi2_wrist_wear_wake_up_config | - * Structure parameters | Description - *----------------------------------|------------------------------------------- - * | Enable bits for enabling output into the - * out_conf | register status bits and, if desired, onto - * | the interrupt pin. - * ---------------------------------|------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_wrist_wear_wake_up_config(const struct bmi2_wrist_wear_wake_up_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets wrist gesture configurations like wearable-arm, - * and output-configuration for wearable variant. - * - * @param[in] config : Structure instance of bmi2_wrist_gest_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *----------------------------------------------------------------------------- - * bmi2_wrist_gest_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * | Device in left (0) or right (1) arm. By default, - * wear_arm | the wearable device is assumed to be in left arm - * | i.e. default value is 0. - * -------------------------|--------------------------------------------------- - * | Enable bits for enabling output into the - * out_conf | register status bits and, if desired, onto the - * | interrupt pin. - * -------------------------|--------------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_wrist_gest_w_config(const struct bmi2_wrist_gest_w_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets wrist wear wake-up configurations like - * output-configuration for wearable variant. - * - * @param[in] config : Structure instance of - * bmi2_wrist_wear_wake_up_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *----------------------------------------------------------------------------- - * bmi2_wrist_wear_wake_up_config | - * Structure parameters | Description - *----------------------------------|------------------------------------------- - * | Enable bits for enabling output into the - * out_conf | register status bits and, if desired, onto - * | the interrupt pin. - * ---------------------------------|------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_wrist_wear_wake_up_wh_config(const struct bmi2_wrist_wear_wake_up_wh_config *config, - struct bmi2_dev *dev); +static int8_t validate_aux_config(struct bmi2_aux_config *config, struct bmi2_dev *dev); /*! * @brief This internal API gets accelerometer configurations like ODR, @@ -1862,11 +331,10 @@ static int8_t set_wrist_wear_wake_up_wh_config(const struct bmi2_wrist_wear_wake * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error + * @retval 0 -> Success + * @retval < 0 -> Fail */ -static int8_t get_accel_config(struct bmi2_accel_config *config, const struct bmi2_dev *dev); +static int8_t get_accel_config(struct bmi2_accel_config *config, struct bmi2_dev *dev); /*! * @brief This internal API gets gyroscope configurations like ODR, bandwidth, @@ -1876,12 +344,10 @@ static int8_t get_accel_config(struct bmi2_accel_config *config, const struct bm * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page + * @retval 0 -> Success + * @retval < 0 -> Fail */ -static int8_t get_gyro_config(struct bmi2_gyro_config *config, const struct bmi2_dev *dev); +static int8_t get_gyro_config(struct bmi2_gyro_config *config, struct bmi2_dev *dev); /*! * @brief This internal API: @@ -1895,215 +361,10 @@ static int8_t get_gyro_config(struct bmi2_gyro_config *config, const struct bmi2 * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page + * @retval 0 -> Success + * @retval < 0 -> Fail */ -static int8_t get_aux_config(struct bmi2_aux_config *config, const struct bmi2_dev *dev); - -/*! - * @brief This internal API gets the enable status of auxiliary interface. - * - * @param[out] config : Structure instance of bmi2_aux_config. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t get_aux_interface(struct bmi2_aux_config *config, const struct bmi2_dev *dev); - -/*! - * @brief This internal API gets auxiliary configurations like manual/auto mode - * FCU write command enable and read burst length for both data and manual mode. - * - * @param[out] config : Structure instance of bmi2_aux_config. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t get_aux_interface_config(struct bmi2_aux_config *config, const struct bmi2_dev *dev); - -/*! - * @brief This internal API gets read out offset and ODR of the auxiliary - * sensor. - * - * @param[out] config : Structure instance of bmi2_aux_config. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t get_aux_cfg(struct bmi2_aux_config *config, const struct bmi2_dev *dev); - -/*! - * @brief This internal API gets any-motion configurations like axes select, - * duration, threshold and output-configuration. - * - * @param[out] config : Structure instance of bmi2_any_motion_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *---------------------------------------------------------------------------- - * bmi2_any_motion_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * | Defines the number of consecutive data points for - * | which the threshold condition must be respected, - * | for interrupt assertion. It is expressed in 50 Hz - * duration | samples (20 msec). - * | Range is 0 to 163sec. - * | Default value is 5 = 100ms. - * -------------------------|--------------------------------------------------- - * | Slope threshold value for in 5.11g format. - * threshold | Range is 0 to 1g. - * | Default value is 0xAA = 83mg. - * -------------------------|--------------------------------------------------- - * x_sel, y_sel, z_sel | Selects the feature on a per-axis basis - * -------------------------|--------------------------------------------------- - * | Enable bits for enabling output into the - * out_conf | register status bits and, if desired, onto the - * | interrupt pin. - * @endverbatim - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t get_any_motion_config(struct bmi2_any_motion_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets no-motion configurations like axes select, - * duration, threshold and output-configuration. - * - * @param[out] config : Structure instance of bmi2_no_motion_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *---------------------------------------------------------------------------- - * bmi2_no_motion_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * | Defines the number of consecutive data points for - * | which the threshold condition must be respected, - * | for interrupt assertion. It is expressed in 50 Hz - * duration | samples (20 msec). - * | Range is 0 to 163sec. - * | Default value is 5 = 100ms. - * -------------------------|--------------------------------------------------- - * | Slope threshold value for in 5.11g format. - * threshold | Range is 0 to 1g. - * | Default value is 0xAA = 83mg. - * -------------------------|--------------------------------------------------- - * x_sel, y_sel, z_sel | Selects the feature on a per-axis basis - * -------------------------|--------------------------------------------------- - * | Enable bits for enabling output into the - * out_conf | register status bits and, if desired, onto the - * | interrupt pin. - * @endverbatim - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t get_no_motion_config(struct bmi2_no_motion_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets sig-motion configurations like block-size, - * output-configuration and other parameters. - * - * @param[out] config : Structure instance of bmi2_sig_motion_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - *---------------------------------------------------------------------------- - * bmi2_sig_motion_config | - * Structure parameters | Description - * -------------------------|--------------------------------------------------- - * | Defines the duration after which the significant - * block_size | motion interrupt is triggered. It is expressed in - * | 50 Hz samples (20 ms). Default value is 0xFA=5sec. - *--------------------------|--------------------------------------------------- - * | Enable bits for enabling output into the - * out_conf | register status bits and, if desired, onto the - * | interrupt pin. - *--------------------------|--------------------------------------------------- - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t get_sig_motion_config(struct bmi2_sig_motion_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets step counter parameter configurations. - * - * @param[in] step_count_params : Array that stores parameters 1 to 25. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t get_step_count_params_config(uint16_t *step_count_params, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets step counter/detector/activity configurations. - * - * @param[out] config : Structure instance of bmi2_step_config. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * - * @verbatim - *---------------------------------------------------------------------------- - * bmi2_step_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * | The Step-counter will trigger output every time - * | the number of steps are counted. Holds implicitly - * water-mark level | a 20x factor, so the range is 0 to 10230, - * | with resolution of 20 steps. - * -------------------------|--------------------------------------------------- - * reset counter | Flag to reset the counted steps. - * -------------------------|--------------------------------------------------- - * | Enable bits for enabling output into the - * step_det_out_conf | register status bits and, if desired, onto the - * | interrupt pin for step detector. - * -------------------------|--------------------------------------------------- - * | Enable bits for enabling output into the - * step_act_out_conf | register status bits and, if desired, onto the - * | interrupt pin for step activity. - * -------------------------|--------------------------------------------------- - * @endverbatim - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t get_step_config(struct bmi2_step_config *config, struct bmi2_dev *dev); +static int8_t get_aux_config(struct bmi2_aux_config *config, struct bmi2_dev *dev); /*! * @brief This internal API gets gyroscope user-gain configurations like gain @@ -2122,455 +383,97 @@ static int8_t get_step_config(struct bmi2_step_config *config, struct bmi2_dev * * ratio_y | Gain update value for y-axis * ------------------------|--------------------------------------------------- * ratio_z | Gain update value for z-axis + * * @endverbatim * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t get_gyro_gain_update_config(struct bmi2_gyro_user_gain_config *config, struct bmi2_dev *dev); /*! - * @brief This internal API gets tilt configurations like output-configuration. + * @brief This internal API gets the enable status of auxiliary interface. * - * @param[out] config : Structure instance of bmi2_tilt_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *---------------------------------------------------------------------------- - * bmi2_tilt_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * | Enable bits for enabling output into the - * out_conf | register status bits and, if desired, onto the - * | interrupt pin. - * -------------------------|--------------------------------------------------- - * @endverbatim + * @param[out] config : Structure instance of bmi2_aux_config. + * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page + * @retval 0 -> Success + * @retval < 0 -> Fail */ -static int8_t get_tilt_config(struct bmi2_tilt_config *config, struct bmi2_dev *dev); +static int8_t get_aux_interface(struct bmi2_aux_config *config, struct bmi2_dev *dev); /*! - * @brief This internal API gets uphold to wake configurations like output - * configuration. + * @brief This internal API gets auxiliary configurations like manual/auto mode + * FCU write command enable and read burst length for both data and manual mode. * - * @param[out] config : Structure instance of bmi2_up_hold_to_wake_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *---------------------------------------------------------------------------- - * bmi2_up_hold_to_wake_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * | Enable bits for enabling output into the - * out_conf | register status bits and, if desired, onto the - * | interrupt pin. - * -------------------------|--------------------------------------------------- - * @endverbatim + * @param[out] config : Structure instance of bmi2_aux_config. + * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page + * @retval 0 -> Success + * @retval < 0 -> Fail */ -static int8_t get_up_hold_to_wake_config(struct bmi2_up_hold_to_wake_config *config, struct bmi2_dev *dev); +static int8_t get_aux_interface_config(struct bmi2_aux_config *config, struct bmi2_dev *dev); /*! - * @brief This internal API gets glance detector configurations like output - * configuration. + * @brief This internal API gets read out offset and ODR of the auxiliary + * sensor. * - * @param[out] config : Structure instance of bmi2_glance_det_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *---------------------------------------------------------------------------- - * bmi2_glance_det_config| - * Structure parameters | Description - *-------------------------|-------------------------------------------------- - * | Enable bits for enabling output into the - * out_conf | register status bits and, if desired, onto the - * | interrupt pin. - * ------------------------|---------------------------------------------------- - * @endverbatim + * @param[out] config : Structure instance of bmi2_aux_config. + * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page + * @retval 0 -> Success + * @retval < 0 -> Fail */ -static int8_t get_glance_detect_config(struct bmi2_glance_det_config *config, struct bmi2_dev *dev); +static int8_t get_aux_cfg(struct bmi2_aux_config *config, struct bmi2_dev *dev); /*! - * @brief This internal API gets wake-up configurations like sensitivity, - * single/double tap enable and output-configuration. + * @brief This internal API gets the saturation status for the gyroscope user + * gain update. * - * @param[out] config : Structure instance of bmi2_wake_up_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *----------------------------------------------------------------------------- - * bmi2_wake_up_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * | Configures wake-up sensitivity, the range goes - * sensitivity | from 0 (high sensitive) to 7 (low sensitive). - * -------------------------|--------------------------------------------------- - * | Enable - Single Tap detection - * single_tap_en | Disable- Double Tap detection (Default value) - * -------------------------|--------------------------------------------------- - * | Enable bits for enabling output into the - * out_conf | register status bits and, if desired, onto the - * | interrupt pin. - * -------------------------|--------------------------------------------------- - * @endverbatim + * @param[out] user_gain_stat : Stores the saturation status of the axes. + * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page + * @retval 0 -> Success + * @retval < 0 -> Fail */ -static int8_t get_wake_up_config(struct bmi2_wake_up_config *config, struct bmi2_dev *dev); +static int8_t get_gyro_gain_update_status(struct bmi2_gyr_user_gain_status *user_gain, struct bmi2_dev *dev); /*! - * @brief This internal API gets wrist gesture configurations like wearable-arm, - * and output-configuration. + * @brief This internal API is used to extract the output feature configuration + * details like page and start address from the look-up table. * - * @param[out] config : Structure instance of bmi2_wrist_gest_config. - * @param[in, out] dev : Structure instance of bmi2_dev. + * @param[out] feat_output : Structure that stores output feature + * configurations. + * @param[in] type : Type of feature or sensor. + * @param[in] dev : Structure instance of bmi2_dev. * - * @verbatim - *----------------------------------------------------------------------------- - * bmi2_wrist_gest_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * | Device in left (0) or right (1) arm. By default, - * wear_arm | the wearable device is assumed to be in left arm - * | i.e. default value is 0. - * -------------------------|--------------------------------------------------- - * | Enable bits for enabling output into the - * out_conf | register status bits and, if desired, onto the - * | interrupt pin. - * -------------------------|--------------------------------------------------- - * @endverbatim + * @return Returns the feature found flag. * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page + * @retval BMI2_FALSE : Feature not found + * BMI2_TRUE : Feature found */ -static int8_t get_wrist_gest_config(struct bmi2_wrist_gest_config *config, struct bmi2_dev *dev); +static uint8_t extract_output_feat_config(struct bmi2_feature_config *feat_output, + uint8_t type, + const struct bmi2_dev *dev); /*! - * @brief This internal API gets wrist wear wake-up configurations like - * output-configuration. + * @brief This internal API gets the cross sensitivity coefficient between + * gyroscope's X and Z axes. * - * @param[out] config : Structure instance of - * bmi2_wrist_wear_wake_up_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *----------------------------------------------------------------------------- - * bmi2_wrist_wear_wake_up_config | - * Structure parameters | Description - *----------------------------------|------------------------------------------- - * | Enable bits for enabling output into the - * out_conf | register status bits and, if desired, onto - * | the interrupt pin. - * ---------------------------------|------------------------------------------- - * @endverbatim + * @param[out] cross_sense : Pointer to the stored cross sensitivity + * coefficient. + * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page + * @retval 0 -> Success + * @retval < 0 -> Fail */ -static int8_t get_wrist_wear_wake_up_config(struct bmi2_wrist_wear_wake_up_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets wrist gesture configurations like wearable-arm, - * and output-configuration for wearable variant. - * - * @param[out] config : Structure instance of bmi2_wrist_gest_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *----------------------------------------------------------------------------- - * bmi2_wrist_gest_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * | Device in left (0) or right (1) arm. By default, - * wear_arm | the wearable device is assumed to be in left arm - * | i.e. default value is 0. - * -------------------------|--------------------------------------------------- - * | Enable bits for enabling output into the - * out_conf | register status bits and, if desired, onto the - * | interrupt pin. - * -------------------------|--------------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t get_wrist_gest_w_config(struct bmi2_wrist_gest_w_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets wrist wear wake-up configurations like - * output-configuration for wearable variant. - * - * @param[out] config : Structure instance of - * bmi2_wrist_wear_wake_up_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *----------------------------------------------------------------------------- - * bmi2_wrist_wear_wake_up_config | - * Structure parameters | Description - *----------------------------------|------------------------------------------- - * | Enable bits for enabling output into the - * out_conf | register status bits and, if desired, onto - * | the interrupt pin. - * ---------------------------------|------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t get_wrist_wear_wake_up_wh_config(struct bmi2_wrist_wear_wake_up_wh_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets orientation configurations like upside/down - * detection, symmetrical modes, blocking mode, theta, hysteresis and output - * configuration. - * - * @param[out] config : Structure instance of bmi2_orient_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *----------------------------------------------------------------------------- - * bmi2_orient_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * upside/down | Enables upside/down detection, if set to 1. - * detection | - * -------------------------|--------------------------------------------------- - * | Sets the mode: - * mode | Values 0 or 3 - symmetrical. - * | Value 1 - high asymmetrical - * | Value 2 - low asymmetrical - * -------------------------|--------------------------------------------------- - * | Enable - no orientation interrupt will be set. - * blocking | Default value: 3, the most restrictive blocking. - * -------------------------|--------------------------------------------------- - * | Threshold angle used in blocking mode. - * theta | Theta = 64 * (tan(angle)^2) - * | Default value: 40, equivalent to 38 degrees angle. - * -------------------------|--------------------------------------------------- - * | Acceleration hysteresis for Orientation detection - * | is expressed in 5.11g format. - * hysteresis | Default value is 128 = 0.0625g. - * | Range is 0 to 1g. - * -------------------------|--------------------------------------------------- - * | Enable bits for enabling output into the - * out_conf | register status bits and, if desired, onto the - * | interrupt pin. - * -------------------------|--------------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t get_orient_config(struct bmi2_orient_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets high-g configurations like threshold, - * hysteresis, duration, and output configuration. - * - * @param[out] config : Structure instance of bmi2_high_g_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *----------------------------------------------------------------------------- - * bmi2_high_g_config | - * Structure parameter | Description - *--------------------------|-------------------------------------------------- - * threshold | The acceleration threshold above which the - * | high_g motion is signaled. - * | Holds threshold in 5.11 g format. - * | Default is 3072 = 1.5 g. - * | Range is 0 to 16g. - * -------------------------|--------------------------------------------------- - * | Acceleration hysteresis for high-g detection - * | is expressed in 1.11g format. - * hysteresis | Default value is 256 = 0.125 g. - * | Range is 0 to 2g. - * | Should be smaller than threshold. - * -------------------------|--------------------------------------------------- - * | Holds the duration in 200 Hz samples (5 ms) for - * | which the threshold has to be exceeded. - * duration |Default value 4 = 20 msec. - * | Range is 0 to 20sec. - * -------------------------|--------------------------------------------------- - * x_sel, y_sel, z_sel | Selects the feature on a per-axis basis - * -------------------------|--------------------------------------------------- - * | Enable bits for enabling output into the - * out_conf | register status bits and, if desired, onto the - * | interrupt pin. - * -------------------------|--------------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t get_high_g_config(struct bmi2_high_g_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets low-g configurations like threshold, - * hysteresis, duration, and output configuration. - * - * @param[out] config : Structure instance of bmi2_low_g_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - * bmi2_low_g_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * threshold | The acceleration threshold above which the - * | low-g motion is signaled. - * | Holds threshold in 5.11 g format. - * | Default is 3072 = 1.5 g. - * -------------------------|--------------------------------------------------- - * | Acceleration hysteresis for low-g detection - * hysteresis | is expressed in 1.11g format. - * | Default value is 512 = 0.250 g. - * | Should be smaller than threshold. - * -------------------------|--------------------------------------------------- - * | Holds the duration in 50 Hz samples (20 ms) for - * | which the threshold has to be exceeded. - * duration | Default value 0 = 1 validation sample = (0+1)*20 - * | = 20 ms. - * -------------------------|--------------------------------------------------- - * | Enable bits for enabling output into the - * out_conf | register status bits and, if desired, onto the - * | interrupt pin. - * -------------------------|--------------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t get_low_g_config(struct bmi2_low_g_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets flat configurations like theta, blocking, - * hold-time, hysteresis, and output configuration. - * - * @param[out] config : Structure instance of bmi2_flat_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *---------------------------------------------------------------------------- - * bmi2_flat_config | - * Structure parameters| Description - *--------------------------|-------------------------------------------------- - * | Theta angle, used for detecting flat - * | position. - * theta | Theta = 64 * (tan(angle)^2); - * | Default value is 8, equivalent to 20 degrees angle - * -------------------------|--------------------------------------------------- - * | Hysteresis for Theta Flat detection. - * hysteresis | Default value is 9 = 2.5 degrees, corresponding - * | to the default Theta angle of 20 degrees. - * -------------------------|--------------------------------------------------- - * | Sets the blocking mode. If blocking is set, no - * | Flat interrupt will be triggered. - * blocking | Default value is 2, the most restrictive blocking - * | mode. - * -------------------------|--------------------------------------------------- - * | Holds the duration in 50Hz samples for which the - * | condition has to be respected. - * hold-time | Default value is 32 = 640 m-sec. - * | Range is 0 to 5.1 sec. - * -------------------------|--------------------------------------------------- - * | Enable bits for enabling output into the - * out_conf | register status bits and, if desired, onto the - * | interrupt pin. - * -------------------------|--------------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t get_flat_config(struct bmi2_flat_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets external sensor sync configurations like output - * configuration. - * - * @param[out] config : Structure instance of bmi2_ext_sens_sync_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *---------------------------------------------------------------------------- - * bmi2_ext_sens_sync_config | - * Structure parameters | Description - * -----------------------------|---------------------------------------------- - * |Enable bits for enabling output into the - * out_conf |register status bits and, if desired, onto the - * |interrupt pin. - * -----------------------------|---------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t get_ext_sens_sync_config(struct bmi2_ext_sens_sync_config *config, struct bmi2_dev *dev); +static int8_t get_gyro_cross_sense(int16_t *cross_sense, struct bmi2_dev *dev); /*! * @brief This internal API gets the accelerometer data from the register. @@ -2580,11 +483,10 @@ static int8_t get_ext_sens_sync_config(struct bmi2_ext_sens_sync_config *config, * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ -static int8_t get_accel_sensor_data(struct bmi2_sens_axes_data *data, uint8_t reg_addr, const struct bmi2_dev *dev); +static int8_t get_accel_sensor_data(struct bmi2_sens_axes_data *data, uint8_t reg_addr, struct bmi2_dev *dev); /*! * @brief This internal API gets the gyroscope data from the register. @@ -2594,11 +496,10 @@ static int8_t get_accel_sensor_data(struct bmi2_sens_axes_data *data, uint8_t re * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ -static int8_t get_gyro_sensor_data(struct bmi2_sens_axes_data *data, uint8_t reg_addr, const struct bmi2_dev *dev); +static int8_t get_gyro_sensor_data(struct bmi2_sens_axes_data *data, uint8_t reg_addr, struct bmi2_dev *dev); /*! * @brief This internal API gets the accelerometer/gyroscope data. @@ -2632,12 +533,10 @@ static void get_remapped_data(struct bmi2_sens_axes_data *data, const struct bmi * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_AUX_INVALID_CFG - Error: Invalid auxiliary configuration + * @retval 0 -> Success + * @retval < 0 -> Fail */ -static int8_t read_aux_data_mode(uint8_t *aux_data, const struct bmi2_dev *dev); +static int8_t read_aux_data_mode(uint8_t *aux_data, struct bmi2_dev *dev); /*! * @brief This internal API reads the user-defined bytes of data from the given @@ -2650,10 +549,8 @@ static int8_t read_aux_data_mode(uint8_t *aux_data, const struct bmi2_dev *dev); * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_AUX_BUSY - Error: Auxiliary sensor is busy + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t read_aux_data(uint8_t reg_addr, uint8_t *aux_data, uint16_t len, uint8_t burst_len, struct bmi2_dev *dev); @@ -2668,10 +565,8 @@ static int8_t read_aux_data(uint8_t reg_addr, uint8_t *aux_data, uint16_t len, u * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_AUX_BUSY - Error: Auxiliary sensor is busy + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t set_if_aux_not_busy(uint8_t reg_addr, uint8_t reg_data, struct bmi2_dev *dev); @@ -2683,9 +578,8 @@ static int8_t set_if_aux_not_busy(uint8_t reg_addr, uint8_t reg_data, struct bmi * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_AUX_INVALID_CFG - Error: Invalid auxiliary configuration + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t map_read_len(uint8_t *len, const struct bmi2_dev *dev); @@ -2702,250 +596,22 @@ static int8_t map_read_len(uint8_t *len, const struct bmi2_dev *dev); * @note Change of BMI2_AUX_WR_ADDR is only allowed if AUX is not busy. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_AUX_BUSY - Error: Auxiliary sensor is busy + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t write_aux_data(uint8_t reg_addr, uint8_t reg_data, struct bmi2_dev *dev); -/*! - * @brief This internal API gets the output values of step counter. - * - * @param[out] step_count : Pointer to the stored step counter data. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t get_step_counter_output(uint32_t *step_count, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets the output values of step activity. - * - * @param[out] step_act : Pointer to the stored step activity data. - * @param[in] dev : Structure instance of bmi2_dev. - * - * *step_act | Output - * -----------|------------ - * 0x00 | STILL - * 0x01 | WALKING - * 0x02 | RUNNING - * 0x03 | UNKNOWN - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t get_step_activity_output(uint8_t *step_act, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets the output values of orientation: portrait- - * landscape and face up-down. - * - * @param[out] orient_out : Structure pointer to the orientation data. - * @param[in] dev : Structure instance of bmi2_dev. - * - * - * portrait | - * landscape | Output - * -----------|------------ - * 0x00 | PORTRAIT UPRIGHT - * 0x01 | LANDSCAPE LEFT - * 0x02 | PORTRAIT UPSIDE DOWN - * 0x03 | LANDSCAPE RIGHT - * - * Face | - * up-down | Output - * -----------|------------ - * 0x00 | FACE-UP - * 0x01 | FACE-DOWN - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t get_orient_output(struct bmi2_orientation_output *orient_out, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets the output values of high-g. - * - * @param[out] high_g_out : Pointer to the stored high-g output. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t get_high_g_output(uint8_t *high_g_out, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets the output values of wrist gesture. - * - * @param[out] wrist_gest : Pointer to the stored wrist gesture. - * @param[in] dev : Structure instance of bmi2_dev. - * - * *wrist_gest | Output - * -------------|------------ - * 0x00 | UNKNOWN - * 0x01 | PUSH_ARM_DOWN - * 0x02 | PIVOT_UP - * 0x03 | WRIST_SHAKE_JIGGLE - * 0x04 | FLICK_IN - * 0x05 | FLICK_OUT - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t get_wrist_gest_status(uint8_t *wrist_gest, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets the cross sensitivity coefficient between - * gyroscope's X and Z axes. - * - * @param[out] cross_sense : Pointer to the stored cross sensitivity - * coefficient. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t get_gyro_cross_sense(int16_t *cross_sense, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets the saturation status for the gyroscope user - * gain update. - * - * @param[out] user_gain_stat : Stores the saturation status of the axes. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t get_gyro_gain_update_status(struct bmi2_gyr_user_gain_status *user_gain, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets the error status related to NVM. - * - * @param[out] nvm_err_stat : Stores the NVM error status. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t get_nvm_error_status(struct bmi2_nvm_err_status *nvm_err_stat, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets the error status related to virtual frames. - * - * @param[out] vfrm_err_stat : Stores the VFRM related error status. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t get_vfrm_error_status(struct bmi2_vfrm_err_status *vfrm_err_stat, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to parse and store the activity recognition - * output from the FIFO data. - * - * @param[out] act_recog : Structure to retrieve output of activity - * recognition frame. - * @param[in] data_index : Index of the FIFO data which contains - * activity recognition frame. - * @param[out] fifo : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_W_FIFO_EMPTY - Warning : FIFO is empty - * @retval BMI2_W_PARTIAL_READ - Warning : There are more frames to be read - */ -static int8_t unpack_act_recog_output(struct bmi2_act_recog_output *act_recog, - uint16_t *data_index, - const struct bmi2_fifo_frame *fifo); - -/*! - * @brief This internal API is used to get enable status of gyroscope user gain - * update. - * - * @param[out] status : Stores status of gyroscope user gain update. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t get_user_gain_upd_status(uint8_t *status, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets the accelerometer self-test status. - * - * @param[out] acc_self_test_stat : Stores status of accelerometer self- - * test. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t get_accel_self_test_status(struct bmi2_acc_self_test_status *acc_self_test_stat, struct bmi2_dev *dev); - /*! * @brief This internal API maps/unmaps feature interrupts to that of interrupt * pins. * * @param[in] int_pin : Interrupt pin selected. - * @param[in] feat_int : Type of feature interrupt to be mapped or the value - * of out_conf. + * @param[in] feat_int : Type of feature interrupt to be mapped. * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_FEAT_BIT - Error: Invalid feature Interrupt + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t map_feat_int(uint8_t *reg_data_array, enum bmi2_hw_int_pin int_pin, uint8_t int_mask); @@ -2959,9 +625,8 @@ static int8_t map_feat_int(uint8_t *reg_data_array, enum bmi2_hw_int_pin int_pin * @param[in] fifo : Structure instance of bmi2_fifo_frame. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_W_FIFO_EMPTY - Warning : FIFO is empty + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t parse_fifo_accel_len(uint16_t *start_idx, uint16_t *len, @@ -2979,10 +644,8 @@ static int8_t parse_fifo_accel_len(uint16_t *start_idx, * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_W_FIFO_EMPTY - Warning : FIFO is empty - * @retval BMI2_W_PARTIAL_READ - Warning : There are more frames to be read + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t extract_accel_header_mode(struct bmi2_sens_axes_data *acc, uint16_t *accel_length, @@ -3005,10 +668,8 @@ static int8_t extract_accel_header_mode(struct bmi2_sens_axes_data *acc, * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_W_FIFO_EMPTY - Warning : FIFO is empty - * @retval BMI2_W_PARTIAL_READ - Warning : There are more frames to be read + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t unpack_accel_frame(struct bmi2_sens_axes_data *acc, uint16_t *idx, @@ -3048,9 +709,8 @@ static void unpack_accel_data(struct bmi2_sens_axes_data *acc, * @param[in] fifo : Structure instance of bmi2_fifo_frame. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_W_FIFO_EMPTY - Warning : FIFO is empty + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t parse_fifo_gyro_len(uint16_t *start_idx, uint16_t(*len), @@ -3072,10 +732,8 @@ static int8_t parse_fifo_gyro_len(uint16_t *start_idx, * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_W_FIFO_EMPTY - Warning : FIFO is empty - * @retval BMI2_W_PARTIAL_READ - Warning : There are more frames to be read + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t unpack_gyro_frame(struct bmi2_sens_axes_data *gyr, uint16_t *idx, @@ -3113,10 +771,8 @@ static void unpack_gyro_data(struct bmi2_sens_axes_data *gyr, * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_W_FIFO_EMPTY - Warning : FIFO is empty - * @retval BMI2_W_PARTIAL_READ - Warning : There are more frames to be read + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t extract_gyro_header_mode(struct bmi2_sens_axes_data *gyr, uint16_t *gyro_length, @@ -3132,11 +788,9 @@ static int8_t extract_gyro_header_mode(struct bmi2_sens_axes_data *gyr, * @param[in] aux_count : Number of accelerometer frames to be read. * @param[in] fifo : Structure instance of bmi2_fifo_frame. * - * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_W_FIFO_EMPTY - Warning : FIFO is empty + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t parse_fifo_aux_len(uint16_t *start_idx, uint16_t(*len), @@ -3153,10 +807,8 @@ static int8_t parse_fifo_aux_len(uint16_t *start_idx, * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_W_FIFO_EMPTY - Warning : FIFO is empty - * @retval BMI2_W_PARTIAL_READ - Warning : There are more frames to be read + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t extract_aux_header_mode(struct bmi2_aux_fifo_data *aux, uint16_t *aux_length, @@ -3179,10 +831,8 @@ static int8_t extract_aux_header_mode(struct bmi2_aux_fifo_data *aux, * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_W_FIFO_EMPTY - Warning : FIFO is empty - * @retval BMI2_W_PARTIAL_READ - Warning : There are more frames to be read + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t unpack_aux_frame(struct bmi2_aux_fifo_data *aux, uint16_t *idx, @@ -3246,10 +896,8 @@ static int8_t check_empty_fifo(uint16_t *data_index, const struct bmi2_fifo_fram * @param[in] fifo : Structure instance of bmi2_fifo_frame. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_W_FIFO_EMPTY - Warning : FIFO is empty - * @retval BMI2_W_PARTIAL_READ - Warning : There are more frames to be read + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t move_next_frame(uint16_t *data_index, uint8_t current_frame_length, const struct bmi2_fifo_frame *fifo); @@ -3261,10 +909,8 @@ static int8_t move_next_frame(uint16_t *data_index, uint8_t current_frame_length * @param[in] fifo : Structure instance of bmi2_fifo_frame. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_W_FIFO_EMPTY - Warning : FIFO is empty - * @retval BMI2_W_PARTIAL_READ - Warning : There are more frames to be read + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t unpack_sensortime_frame(uint16_t *data_index, struct bmi2_fifo_frame *fifo); @@ -3277,10 +923,8 @@ static int8_t unpack_sensortime_frame(uint16_t *data_index, struct bmi2_fifo_fra * @param[in] fifo : Structure instance of bmi2_fifo_frame. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_W_FIFO_EMPTY - Warning : FIFO is empty - * @retval BMI2_W_PARTIAL_READ - Warning : There are more frames to be read + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t unpack_skipped_frame(uint16_t *data_index, struct bmi2_fifo_frame *fifo); @@ -3291,11 +935,8 @@ static int8_t unpack_skipped_frame(uint16_t *data_index, struct bmi2_fifo_frame * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t pre_self_test_config(struct bmi2_dev *dev); @@ -3312,9 +953,8 @@ static int8_t pre_self_test_config(struct bmi2_dev *dev); * BMI2_DISABLE | negative excitation * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t self_test_config(uint8_t sign, struct bmi2_dev *dev); @@ -3331,9 +971,8 @@ static int8_t self_test_config(uint8_t sign, struct bmi2_dev *dev); * BMI2_DISABLE | Disables self-test * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t set_accel_self_test_enable(uint8_t enable, struct bmi2_dev *dev); @@ -3350,9 +989,8 @@ static int8_t set_accel_self_test_enable(uint8_t enable, struct bmi2_dev *dev); * BMI2_DISABLE | negative excitation * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t set_acc_self_test_sign(uint8_t sign, struct bmi2_dev *dev); @@ -3369,9 +1007,8 @@ static int8_t set_acc_self_test_sign(uint8_t sign, struct bmi2_dev *dev); * BMI2_DISABLE | self-test amplitude is low * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t set_accel_self_test_amp(uint8_t amp, struct bmi2_dev *dev); @@ -3383,11 +1020,10 @@ static int8_t set_accel_self_test_amp(uint8_t amp, struct bmi2_dev *dev); * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ -static int8_t read_accel_xyz(struct bmi2_sens_axes_data *accel, const struct bmi2_dev *dev); +static int8_t read_accel_xyz(struct bmi2_sens_axes_data *accel, struct bmi2_dev *dev); /*! * @brief This internal API converts LSB value of accelerometer axes to form @@ -3400,8 +1036,8 @@ static int8_t read_accel_xyz(struct bmi2_sens_axes_data *accel, const struct bmi * @return None * @retval None */ -static void convert_lsb_g(const struct selftest_delta_limit *acc_data_diff, - struct selftest_delta_limit *acc_data_diff_mg, +static void convert_lsb_g(const struct bmi2_selftest_delta_limit *acc_data_diff, + struct bmi2_selftest_delta_limit *acc_data_diff_mg, const struct bmi2_dev *dev); /*! @@ -3422,11 +1058,10 @@ static int32_t power(int16_t base, uint8_t resolution); * @param[in] accel_data_diff : Stores the acceleration value difference. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_SELF_TEST_FAIL - Error: Self-test fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ -static int8_t validate_self_test(const struct selftest_delta_limit *accel_data_diff); +static int8_t validate_self_test(const struct bmi2_selftest_delta_limit *accel_data_diff); /*! * @brief This internal API gets the re-mapped x, y and z axes from the sensor. @@ -3435,13 +1070,10 @@ static int8_t validate_self_test(const struct selftest_delta_limit *accel_data_d * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page + * @retval 0 -> Success + * @retval < 0 -> Fail */ -static int8_t get_remap_axes(struct axes_remap *remap, struct bmi2_dev *dev); +static int8_t get_remap_axes(struct bmi2_axes_remap *remap, struct bmi2_dev *dev); /*! * @brief This internal API sets the re-mapped x, y and z axes in the sensor. @@ -3450,13 +1082,10 @@ static int8_t get_remap_axes(struct axes_remap *remap, struct bmi2_dev *dev); * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page + * @retval 0 -> Success + * @retval < 0 -> Fail */ -static int8_t set_remap_axes(const struct axes_remap *remap, struct bmi2_dev *dev); +static int8_t set_remap_axes(const struct bmi2_axes_remap *remap, struct bmi2_dev *dev); /*! * @brief Interface to get max burst length @@ -3465,11 +1094,8 @@ static int8_t set_remap_axes(const struct axes_remap *remap, struct bmi2_dev *de * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t get_maxburst_len(uint8_t *max_burst_len, struct bmi2_dev *dev); @@ -3480,50 +1106,11 @@ static int8_t get_maxburst_len(uint8_t *max_burst_len, struct bmi2_dev *dev); * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t set_maxburst_len(const uint16_t write_len_byte, struct bmi2_dev *dev); -/*! - * @brief This internal API is used to get the feature configuration from the - * selected page. - * - * @param[in] sw_page : Switches to the desired page. - * @param[out] feat_config : Pointer to the feature configuration. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t get_feat_config(uint8_t sw_page, uint8_t *feat_config, struct bmi2_dev *dev); - -/*! - * @brief This internal API enables/disables compensation of the gain defined - * in the GAIN register. - * - * @param[in] enable : Enables/Disables gain compensation - * @param[in] dev : Structure instance of bmi2_dev. - * - * enable | Description - * -------------|--------------- - * BMI2_ENABLE | Enable gain compensation. - * BMI2_DISABLE | Disable gain compensation. - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - */ -static int8_t enable_gyro_gain(uint8_t enable, struct bmi2_dev *dev); - /*! * @brief This internal API parses virtual frame header from the FIFO data. * @@ -3566,24 +1153,6 @@ static void unpack_virt_aux_sensor_time(struct bmi2_aux_fifo_data *aux, uint16_t *idx, const struct bmi2_fifo_frame *fifo); -/*! - * @brief This internal API skips S4S frame in the FIFO data while getting - * activity recognition output. - * - * @param[in, out] frame_header : FIFO frame header. - * @param[in, out] data_index : Index value of the FIFO data bytes - * from which S4S frame header is to be - * skipped. - * @param[in] fifo : Structure instance of bmi2_fifo_frame. - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_W_FIFO_EMPTY - Warning : FIFO is empty - * @retval BMI2_W_PARTIAL_READ - Warning : There are more frames to be read - */ -static int8_t move_if_s4s_frame(const uint8_t *frame_header, uint16_t *data_index, const struct bmi2_fifo_frame *fifo); - /*! * @brief This internal API corrects the gyroscope cross-axis sensitivity * between the z and the x axis. @@ -3598,41 +1167,6 @@ static int8_t move_if_s4s_frame(const uint8_t *frame_header, uint16_t *data_inde */ static void comp_gyro_cross_axis_sensitivity(struct bmi2_sens_axes_data *gyr_data, const struct bmi2_dev *dev); -/*! - * @brief This internal API is used to extract the input feature configuration - * details like page and start address from the look-up table. - * - * @param[out] feat_config : Structure that stores feature configurations. - * @param[in] type : Type of feature or sensor. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Returns the feature found flag. - * - * @retval BMI2_FALSE : Feature not found - * BMI2_TRUE : Feature found - */ -static uint8_t extract_input_feat_config(struct bmi2_feature_config *feat_config, - uint8_t type, - const struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to extract the output feature configuration - * details like page and start address from the look-up table. - * - * @param[out] feat_output : Structure that stores output feature - * configurations. - * @param[in] type : Type of feature or sensor. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Returns the feature found flag. - * - * @retval BMI2_FALSE : Feature not found - * BMI2_TRUE : Feature found - */ -static uint8_t extract_output_feat_config(struct bmi2_feature_config *feat_output, - uint8_t type, - const struct bmi2_dev *dev); - /*! * @brief This internal API saves the configurations before performing FOC. * @@ -3642,9 +1176,8 @@ static uint8_t extract_output_feat_config(struct bmi2_feature_config *feat_outpu * @param[in] dev : Structure instance of bmi2_dev * * @return Result of API execution status - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t save_accel_foc_config(struct bmi2_accel_config *acc_cfg, uint8_t *aps, @@ -3666,12 +1199,10 @@ static int8_t save_accel_foc_config(struct bmi2_accel_config *acc_cfg, * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ -static int8_t perform_accel_foc(const struct accel_foc_g_value *accel_g_value, +static int8_t perform_accel_foc(const struct bmi2_accel_foc_g_value *accel_g_value, const struct bmi2_accel_config *acc_cfg, struct bmi2_dev *dev); @@ -3681,9 +1212,8 @@ static int8_t perform_accel_foc(const struct accel_foc_g_value *accel_g_value, * @param[in] dev : Structure instance of bmi2_dev * * @return Result of API execution status - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t set_accel_foc_config(struct bmi2_dev *dev); @@ -3695,8 +1225,8 @@ static int8_t set_accel_foc_config(struct bmi2_dev *dev); * @param[in] dev : Structure instance of bmi2_dev * * @return Result of API execution status - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t set_accel_offset_comp(uint8_t offset_en, struct bmi2_dev *dev); @@ -3726,9 +1256,9 @@ static void map_accel_range(uint8_t range_in, uint8_t *range_out); * @retval None */ static void comp_for_gravity(uint16_t lsb_per_g, - const struct accel_foc_g_value *g_val, + const struct bmi2_accel_foc_g_value *g_val, const struct bmi2_sens_axes_data *data, - struct offset_delta *comp_data); + struct bmi2_offset_delta *comp_data); /*! * @brief This internal API scales the compensated accelerometer data according @@ -3743,7 +1273,8 @@ static void comp_for_gravity(uint16_t lsb_per_g, * @return None * @retval None */ -static void scale_accel_offset(uint8_t range, const struct offset_delta *comp_data, struct accel_offset *data); +static void scale_accel_offset(uint8_t range, const struct bmi2_offset_delta *comp_data, + struct bmi2_accel_offset *data); /*! * @brief This internal API finds the bit position of 3.9mg according to given @@ -3764,7 +1295,7 @@ static int8_t get_bit_pos_3_9mg(uint8_t range); * @return None * @retval None */ -static void invert_accel_offset(struct accel_offset *offset_data); +static void invert_accel_offset(struct bmi2_accel_offset *offset_data); /*! * @brief This internal API writes the offset data in the offset compensation @@ -3774,10 +1305,10 @@ static void invert_accel_offset(struct accel_offset *offset_data); * @param[in] dev : Structure instance of bmi2_dev * * @return Result of API execution status - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ -static int8_t write_accel_offset(const struct accel_offset *offset, struct bmi2_dev *dev); +static int8_t write_accel_offset(const struct bmi2_accel_offset *offset, struct bmi2_dev *dev); /*! * @brief This internal API restores the configurations saved before performing @@ -3789,10 +1320,8 @@ static int8_t write_accel_offset(const struct accel_offset *offset, struct bmi2_ * @param[in] dev : Structure instance of bmi2_dev * * @return Result of API execution status - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_SET_APS_FAIL - Error: Set Advance Power Save Fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t restore_accel_foc_config(struct bmi2_accel_config *acc_cfg, uint8_t aps, @@ -3809,9 +1338,8 @@ static int8_t restore_accel_foc_config(struct bmi2_accel_config *acc_cfg, * @param[in] dev : Structure instance of bmi2_dev * * @return Result of API execution status - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t save_gyro_config(struct bmi2_gyro_config *gyr_cfg, uint8_t *aps, uint8_t *gyr_en, struct bmi2_dev *dev); @@ -3821,9 +1349,8 @@ static int8_t save_gyro_config(struct bmi2_gyro_config *gyr_cfg, uint8_t *aps, u * @param[in] dev : Structure instance of bmi2_dev * * @return Result of API execution status - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t set_gyro_foc_config(struct bmi2_dev *dev); @@ -3847,9 +1374,8 @@ static void invert_gyro_offset(struct bmi2_sens_axes_data *offset_data); * @param[in] dev : Structure instance of bmi2_dev * * @return Result of API execution status - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t restore_gyro_config(struct bmi2_gyro_config *gyr_cfg, uint8_t aps, uint8_t gyr_en, struct bmi2_dev *dev); @@ -3872,11 +1398,10 @@ static void saturate_gyro_data(struct bmi2_sens_axes_data *gyr_off); * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ -static int8_t read_gyro_xyz(struct bmi2_sens_axes_data *gyro, const struct bmi2_dev *dev); +static int8_t read_gyro_xyz(struct bmi2_sens_axes_data *gyro, struct bmi2_dev *dev); /*! * @brief This internal API is used to check the boundary conditions. @@ -3887,10 +1412,8 @@ static int8_t read_gyro_xyz(struct bmi2_sens_axes_data *gyro, const struct bmi2_ * @param[in] max : maximum value. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t check_boundary_val(uint8_t *val, uint8_t min, uint8_t max, struct bmi2_dev *dev); @@ -3901,9 +1424,8 @@ static int8_t check_boundary_val(uint8_t *val, uint8_t min, uint8_t max, struct * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t null_ptr_check(const struct bmi2_dev *dev); @@ -3913,9 +1435,8 @@ static int8_t null_ptr_check(const struct bmi2_dev *dev); * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * @retval BMI2_OK - Success. - * @retval BMI2_E_CRT_ERROR - Error: CRT Fail - * + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t crt_gyro_st_update_result(struct bmi2_dev *dev); @@ -3926,12 +1447,10 @@ static int8_t crt_gyro_st_update_result(struct bmi2_dev *dev); * @param[in] dev : Structure instance of bmi2_dev * * @return Result of API execution status - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * + * @retval 0 -> Success + * @retval < 0 -> Fail */ -static int8_t get_st_running(uint8_t *st_status, const struct bmi2_dev *dev); +static int8_t get_st_running(uint8_t *st_status, struct bmi2_dev *dev); /*! * @brief This function is to set crt bit to running. @@ -3940,10 +1459,8 @@ static int8_t get_st_running(uint8_t *st_status, const struct bmi2_dev *dev); * @param[in] dev : Structure instance of bmi2_dev * * @return Result of API execution status - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t set_st_running(uint8_t st_status, struct bmi2_dev *dev); @@ -3955,10 +1472,8 @@ static int8_t set_st_running(uint8_t st_status, struct bmi2_dev *dev); * @param[in] dev : Structure instance of bmi2_dev * * @return Result of API execution status - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t crt_prepare_setup(struct bmi2_dev *dev); @@ -3973,12 +1488,10 @@ static int8_t do_gtrigger_test(uint8_t gyro_st_crt, struct bmi2_dev *dev); * @param[in] dev : Structure instance of bmi2_dev * * @return Result of API execution status - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * + * @retval 0 -> Success + * @retval < 0 -> Fail */ -static int8_t get_rdy_for_dl(uint8_t *rdy_for_dl, const struct bmi2_dev *dev); +static int8_t get_rdy_for_dl(uint8_t *rdy_for_dl, struct bmi2_dev *dev); /*! * @brief This function is to write the config file in the given location for crt. @@ -3991,10 +1504,8 @@ static int8_t get_rdy_for_dl(uint8_t *rdy_for_dl, const struct bmi2_dev *dev); * @param[in] dev : Structure instance of bmi2_dev * * @return Result of API execution status - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t write_crt_config_file(uint16_t write_len, uint16_t config_file_size, @@ -4009,12 +1520,10 @@ static int8_t write_crt_config_file(uint16_t write_len, * @param[in] dev : Structure instance of bmi2_dev * * @return Result of API execution status - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * + * @retval 0 -> Success + * @retval < 0 -> Fail */ -static int8_t wait_rdy_for_dl_toggle(uint8_t retry_complete, uint8_t download_ready, const struct bmi2_dev *dev); +static int8_t wait_rdy_for_dl_toggle(uint8_t retry_complete, uint8_t download_ready, struct bmi2_dev *dev); /*! * @brief This function is to wait till the CRT or gyro self-test process is completed @@ -4023,12 +1532,10 @@ static int8_t wait_rdy_for_dl_toggle(uint8_t retry_complete, uint8_t download_re * @param[in] dev : Structure instance of bmi2_dev * * @return Result of API execution status - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * + * @retval 0 -> Success + * @retval < 0 -> Fail */ -static int8_t wait_st_running(uint8_t retry_complete, const struct bmi2_dev *dev); +static int8_t wait_st_running(uint8_t retry_complete, struct bmi2_dev *dev); /*! * @brief This function is to complete the crt process if max burst length is not zero @@ -4038,10 +1545,8 @@ static int8_t wait_st_running(uint8_t retry_complete, const struct bmi2_dev *dev * @param[in] dev : Structure instance of bmi2_dev * * @return Result of API execution status - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t process_crt_download(uint8_t last_byte_flag, struct bmi2_dev *dev); @@ -4051,10 +1556,8 @@ static int8_t process_crt_download(uint8_t last_byte_flag, struct bmi2_dev *dev) * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t select_self_test(uint8_t gyro_st_crt, struct bmi2_dev *dev); @@ -4065,10 +1568,8 @@ static int8_t select_self_test(uint8_t gyro_st_crt, struct bmi2_dev *dev); * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t abort_bmi2(uint8_t abort_enable, struct bmi2_dev *dev); @@ -4079,11 +1580,10 @@ static int8_t abort_bmi2(uint8_t abort_enable, struct bmi2_dev *dev); * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval <0 - Fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ -static int8_t gyro_self_test_completed(struct bmi2_gyro_self_test_status *gyro_st_result, const struct bmi2_dev *dev); +static int8_t gyro_self_test_completed(struct bmi2_gyro_self_test_status *gyro_st_result, struct bmi2_dev *dev); /*! * @brief This api is used to trigger the preparation for system for NVM programming. @@ -4092,10 +1592,8 @@ static int8_t gyro_self_test_completed(struct bmi2_gyro_self_test_status *gyro_s * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t set_nvm_prep_prog(uint8_t nvm_prep, struct bmi2_dev *dev); @@ -4108,12 +1606,11 @@ static int8_t set_nvm_prep_prog(uint8_t nvm_prep, struct bmi2_dev *dev); * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_INVALID_INPUT - Error: Invalid input + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t validate_foc_position(uint8_t sens_list, - const struct accel_foc_g_value *accel_g_axis, + const struct bmi2_accel_foc_g_value *accel_g_axis, struct bmi2_sens_axes_data avg_foc_data, struct bmi2_dev *dev); @@ -4124,9 +1621,8 @@ static int8_t validate_foc_position(uint8_t sens_list, * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_INVALID_INPUT - Error: Invalid input + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t validate_foc_accel_axis(int16_t avg_foc_data, struct bmi2_dev *dev); @@ -4138,12 +1634,11 @@ static int8_t validate_foc_accel_axis(int16_t avg_foc_data, struct bmi2_dev *dev * @param[in] accel_g_axis: Accel Foc axis and sign input * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_INVALID_INPUT - Error: Invalid input + * @retval 0 -> Success + * @retval < 0 -> Fail */ -static int8_t verify_foc_position(uint8_t sens_list, const struct accel_foc_g_value *accel_g_axis, +static int8_t verify_foc_position(uint8_t sens_list, + const struct bmi2_accel_foc_g_value *accel_g_axis, struct bmi2_dev *dev); /*! @@ -4155,87 +1650,12 @@ static int8_t verify_foc_position(uint8_t sens_list, const struct accel_foc_g_va * @param[in] temp_foc_data: to store data samples * * @return Result of API execution status - * - * @retval BMI2_OK + * @retval 0 -> Success + * @retval < 0 -> Fail */ -static int8_t get_average_of_sensor_data(uint8_t sens_list, struct foc_temp_value *temp_foc_data, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets primary OIS configurations, - * low pass filter cut-off frequency. - * - * @param[in] config : Structure instance of bmi2_primary_ois_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *---------------------------------------------------------------------------- - * bmi2_primary_ois_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * lp filter config | Lp filter cut off frequency - * | Value Name Description - * | 00 280_Hz Cut off frequency configured to 280 Hz - * | 01 350_Hz Cut off frequency configured to 350 Hz - * | 10 410_Hz Cut off frequency configured to 410 Hz - * | 11 612_Hz Cut off frequency configured to 612 Hz - * ---------------------------------------------------------------------------- - * - * @endverbatim - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_primary_ois_config(const struct bmi2_primary_ois_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets primary OIS configurations, - * low pass filter cut-off frequency. - * - * @param[out] config : Structure instance of bmi2_primary_ois_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *---------------------------------------------------------------------------- - * bmi2_primary_ois_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * lp filter config | Lp filter cut off frequency - * | Value Name Description - * | 00 280_Hz Cut off frequency configured to 280 Hz - * | 01 350_Hz Cut off frequency configured to 350 Hz - * | 10 410_Hz Cut off frequency configured to 410 Hz - * | 11 612_Hz Cut off frequency configured to 612 Hz - * ---------------------------------------------------------------------------- - * - * @endverbatim - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t get_primary_ois_config(struct bmi2_primary_ois_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets the output data of OIS. - * - * @param[out] ois_output : Structure instance to the stored OIS data. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t get_ois_output(struct bmi2_ois_output *ois_output, struct bmi2_dev *dev); +static int8_t get_average_of_sensor_data(uint8_t sens_list, + struct bmi2_foc_temp_value *temp_foc_data, + struct bmi2_dev *dev); /*! * @brief This internal api gets major and minor version for config file @@ -4245,92 +1665,60 @@ static int8_t get_ois_output(struct bmi2_ois_output *ois_output, struct bmi2_dev * @param[in] dev : Structure instance of bmi2_dev * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page + * @retval 0 -> Success + * @retval < 0 -> Fail */ -static int8_t extract_config_file(uint16_t *config_major, uint8_t *config_minor, struct bmi2_dev *dev); +static int8_t extract_config_file(uint8_t *config_major, uint8_t *config_minor, struct bmi2_dev *dev); /*! - * @brief This internal API gets free-fall configurations like free-fall accel settings, - * and output configuration. - * - * @param[in] config : Structure instance of bmi2_free_fall_det_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *---------------------------------------------------------------------------- - * bmi2_free_fall_det_config | - * Structure parameters | Description - *------------------------------|-------------------------------------------------- - * free-fall accel settings | Free-fall accel settings. - * 1 - 7 | - * -----------------------------|--------------------------------------------------- - * | Enable bits for enabling output into the - * out_conf | register status bits and, if desired, onto the - * | interrupt pin. - * -----------------------------|--------------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t get_free_fall_det_config(struct bmi2_free_fall_det_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets free-fall configurations like free-fall accel settings, - * and output configuration. - * - * @param[in] config : Structure instance of bmi2_free_fall_det_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *---------------------------------------------------------------------------- - * bmi2_free_fall_det_config | - * Structure parameters | Description - *------------------------------|-------------------------------------------------- - * free-fall accel settings | Free-fall accel settings. - * 1 - 7 | - * -----------------------------|--------------------------------------------------- - * | Enable bits for enabling output into the - * out_conf | register status bits and, if desired, onto the - * | interrupt pin. - * -----------------------------|--------------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -static int8_t set_free_fall_det_config(const struct bmi2_free_fall_det_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to enable/disable free-fall detection feature. + * @brief This internal API is used to map the interrupts to the sensor. * + * @param[in] map_int : Structure instance of bmi2_map_int. + * @param[in] type : Type of feature or sensor. * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables free-fall detection. * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables free-fall detection - * BMI2_ENABLE | Enables free-fall detection + * @return None + * @retval None + */ +static void extract_feat_int_map(struct bmi2_map_int *map_int, uint8_t type, const struct bmi2_dev *dev); + +/*! + * @brief This internal API selects the sensors/features to be enabled or + * disabled. + * + * @param[in] sens_list : Pointer to select the sensor. + * @param[in] n_sens : Number of sensors selected. + * @param[out] sensor_sel : Gets the selected sensor. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page + * @retval 0 -> Success + * @retval < 0 -> Fail */ -static int8_t set_free_fall_det(uint8_t enable, struct bmi2_dev *dev); +static int8_t select_sensor(const uint8_t *sens_list, uint8_t n_sens, uint64_t *sensor_sel); + +/*! + * @brief This internal API enables the selected sensor/features. + * + * @param[in] sensor_sel : Selects the desired sensor. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t sensor_enable(uint64_t sensor_sel, struct bmi2_dev *dev); + +/*! + * @brief This internal API disables the selected sensor/features. + * + * @param[in] sensor_sel : Selects the desired sensor. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t sensor_disable(uint64_t sensor_sel, struct bmi2_dev *dev); /******************************************************************************/ /*! @name User Interface Definitions */ @@ -4351,8 +1739,8 @@ int8_t bmi2_sec_init(struct bmi2_dev *dev) /* Structure to define the default values for axes re-mapping */ struct bmi2_axes_remap axes_remap = { - .x_axis = MAP_X_AXIS, .x_axis_sign = POS_SIGN, .y_axis = MAP_Y_AXIS, .y_axis_sign = POS_SIGN, - .z_axis = MAP_Z_AXIS, .z_axis_sign = POS_SIGN + .x_axis = BMI2_MAP_X_AXIS, .x_axis_sign = BMI2_POS_SIGN, .y_axis = BMI2_MAP_Y_AXIS, + .y_axis_sign = BMI2_POS_SIGN, .z_axis = BMI2_MAP_Z_AXIS, .z_axis_sign = BMI2_POS_SIGN }; /* Null-pointer check */ @@ -4363,10 +1751,12 @@ int8_t bmi2_sec_init(struct bmi2_dev *dev) * default values */ rslt = bmi2_soft_reset(dev); + if (rslt == BMI2_OK) { /* Read chip-id of the BMI2 sensor */ rslt = bmi2_get_regs(BMI2_CHIP_ID_ADDR, &chip_id, 1, dev); + if (rslt == BMI2_OK) { /* Validate chip-id */ @@ -4406,17 +1796,11 @@ int8_t bmi2_sec_init(struct bmi2_dev *dev) * exception of a few special registers, which trap the address. For e.g., * Register address - 0x26, 0x5E. */ -int8_t bmi2_get_regs(uint8_t reg_addr, uint8_t *data, uint16_t len, const struct bmi2_dev *dev) +int8_t bmi2_get_regs(uint8_t reg_addr, uint8_t *data, uint16_t len, struct bmi2_dev *dev) { /* Variable to define error */ int8_t rslt; - /* Variable to define temporary length */ - uint16_t temp_len = len + dev->dummy_byte; - - /* Variable to define temporary buffer */ - uint8_t temp_buf[temp_len]; - /* Variable to define loop */ uint16_t index = 0; @@ -4424,23 +1808,30 @@ int8_t bmi2_get_regs(uint8_t reg_addr, uint8_t *data, uint16_t len, const struct rslt = null_ptr_check(dev); if ((rslt == BMI2_OK) && (data != NULL)) { + /* Variable to define temporary length */ + uint16_t temp_len = len + dev->dummy_byte; + + /* Variable to define temporary buffer */ + uint8_t temp_buf[temp_len]; + /* Configuring reg_addr for SPI Interface */ - if (dev->intf == BMI2_SPI_INTERFACE) + if (dev->intf == BMI2_SPI_INTF) { reg_addr = (reg_addr | BMI2_SPI_RD_MASK); } + + dev->intf_rslt = dev->read(reg_addr, temp_buf, temp_len, dev->intf_ptr); + if (dev->aps_status == BMI2_ENABLE) { - rslt = dev->read(dev->dev_id, reg_addr, temp_buf, temp_len); - dev->delay_us(450); + dev->delay_us(450, dev->intf_ptr); } else { - rslt = dev->read(dev->dev_id, reg_addr, temp_buf, temp_len); - dev->delay_us(20); + dev->delay_us(2, dev->intf_ptr); } - if (rslt == BMI2_OK) + if (dev->intf_rslt == BMI2_INTF_RET_SUCCESS) { /* Read the data from the position next to dummy byte */ while (index < len) @@ -4470,37 +1861,27 @@ int8_t bmi2_set_regs(uint8_t reg_addr, const uint8_t *data, uint16_t len, struct /* Variable to define error */ int8_t rslt; - /* Variable to define loop */ - uint16_t loop = 0; - /* Null-pointer check */ rslt = null_ptr_check(dev); if ((rslt == BMI2_OK) && (data != NULL)) { /* Configuring reg_addr for SPI Interface */ - if (dev->intf == BMI2_SPI_INTERFACE) + if (dev->intf == BMI2_SPI_INTF) { reg_addr = (reg_addr & BMI2_SPI_WR_MASK); } + dev->intf_rslt = dev->write(reg_addr, data, len, dev->intf_ptr); + + /* Delay for Low power mode of the sensor is 450 us */ if (dev->aps_status == BMI2_ENABLE) { - for (loop = 0; loop < len; loop++) - { - /* Byte write if APS is enabled */ - rslt = dev->write(dev->dev_id, (uint8_t)((uint16_t) reg_addr + loop), &data[loop], 1); - dev->delay_us(450); - if (rslt != BMI2_OK) - { - break; - } - } + dev->delay_us(450, dev->intf_ptr); } + /* Delay for Normal mode of the sensor is 2 us */ else { - /* Burst write if APS is disabled */ - rslt = dev->write(dev->dev_id, reg_addr, data, len); - dev->delay_us(20); + dev->delay_us(2, dev->intf_ptr); } /* updating the advance power saver flag */ @@ -4515,7 +1896,8 @@ int8_t bmi2_set_regs(uint8_t reg_addr, const uint8_t *data, uint16_t len, struct dev->aps_status = BMI2_DISABLE; } } - if (rslt != BMI2_OK) + + if (dev->intf_rslt != BMI2_INTF_RET_SUCCESS) { rslt = BMI2_E_COM_FAIL; } @@ -4549,7 +1931,7 @@ int8_t bmi2_soft_reset(struct bmi2_dev *dev) { /* Reset bmi2 device */ rslt = bmi2_set_regs(BMI2_CMD_REG_ADDR, &data, 1, dev); - dev->delay_us(2000); + dev->delay_us(2000, dev->intf_ptr); /* set APS flag as after soft reset the sensor is on advance power save mode */ dev->aps_status = BMI2_ENABLE; @@ -4557,7 +1939,7 @@ int8_t bmi2_soft_reset(struct bmi2_dev *dev) /* Performing a dummy read to bring interface back to SPI from * I2C after a soft-reset */ - if ((rslt == BMI2_OK) && (dev->intf == BMI2_SPI_INTERFACE)) + if ((rslt == BMI2_OK) && (dev->intf == BMI2_SPI_INTF)) { rslt = bmi2_get_regs(BMI2_CHIP_ID_ADDR, &dummy_read, 1, dev); } @@ -4581,7 +1963,7 @@ int8_t bmi2_soft_reset(struct bmi2_dev *dev) /*! * @brief This API is used to get the config file major and minor version information. */ -int8_t bmi2_get_config_file_version(uint16_t *config_major, uint8_t *config_minor, struct bmi2_dev *dev) +int8_t bmi2_get_config_file_version(uint8_t *config_major, uint8_t *config_minor, struct bmi2_dev *dev) { /* Variable to define error */ int8_t rslt; @@ -4601,592 +1983,6 @@ int8_t bmi2_get_config_file_version(uint16_t *config_major, uint8_t *config_mino return rslt; } -/*! - * @brief This API selects the sensors/features to be enabled. - */ -int8_t bmi2_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to select sensor */ - uint32_t sensor_sel = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (sens_list != NULL)) - { - /* Get the selected sensors */ - rslt = select_sensor(sens_list, n_sens, &sensor_sel); - if (rslt == BMI2_OK) - { - /* Enable the selected sensors */ - rslt = sensor_enable(sensor_sel, dev); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API selects the sensors/features to be disabled. - */ -int8_t bmi2_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to select sensor */ - uint32_t sensor_sel = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (sens_list != NULL)) - { - /* Get the selected sensors */ - rslt = select_sensor(sens_list, n_sens, &sensor_sel); - if (rslt == BMI2_OK) - { - /* Disable the selected sensors */ - rslt = sensor_disable(sensor_sel, dev); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API sets the sensor/feature configuration. - */ -int8_t bmi2_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define loop */ - uint8_t loop; - - /* Variable to get the status of advance power save */ - uint8_t aps_stat = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (sens_cfg != NULL)) - { - /* Get status of advance power save mode */ - aps_stat = dev->aps_status; - for (loop = 0; loop < n_sens; loop++) - { - /* Disable Advance power save if enabled for auxiliary - * and feature configurations - */ - if ((sens_cfg[loop].type != BMI2_ACCEL) || (sens_cfg[loop].type != BMI2_GYRO) || - (sens_cfg[loop].type != BMI2_TEMP) || (sens_cfg[loop].type != BMI2_AUX)) - { - - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if - * enabled - */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - } - if (rslt == BMI2_OK) - { - switch (sens_cfg[loop].type) - { - /* Set accelerometer configuration */ - case BMI2_ACCEL: - rslt = set_accel_config(&sens_cfg[loop].cfg.acc, dev); - break; - - /* Set gyroscope configuration */ - case BMI2_GYRO: - rslt = set_gyro_config(&sens_cfg[loop].cfg.gyr, dev); - break; - - /* Set auxiliary configuration */ - case BMI2_AUX: - rslt = set_aux_config(&sens_cfg[loop].cfg.aux, dev); - break; - - /* Set any motion configuration */ - case BMI2_ANY_MOTION: - rslt = set_any_motion_config(&sens_cfg[loop].cfg.any_motion, dev); - break; - - /* Set no motion configuration */ - case BMI2_NO_MOTION: - rslt = set_no_motion_config(&sens_cfg[loop].cfg.no_motion, dev); - break; - - /* Set sig-motion configuration */ - case BMI2_SIG_MOTION: - rslt = set_sig_motion_config(&sens_cfg[loop].cfg.sig_motion, dev); - break; - - /* Set the step counter parameters */ - case BMI2_STEP_COUNTER_PARAMS: - rslt = set_step_count_params_config(sens_cfg[loop].cfg.step_counter_params, dev); - break; - - /* Set step counter/detector/activity configuration */ - case BMI2_STEP_DETECTOR: - case BMI2_STEP_COUNTER: - case BMI2_STEP_ACTIVITY: - rslt = set_step_config(&sens_cfg[loop].cfg.step_counter, dev); - break; - - /* Set gyroscope user gain configuration */ - case BMI2_GYRO_GAIN_UPDATE: - rslt = set_gyro_user_gain_config(&sens_cfg[loop].cfg.gyro_gain_update, dev); - break; - - /* Set tilt configuration */ - case BMI2_TILT: - rslt = set_tilt_config(&sens_cfg[loop].cfg.tilt, dev); - break; - - /* Set uphold to wake configuration */ - case BMI2_UP_HOLD_TO_WAKE: - rslt = set_up_hold_to_wake_config(&sens_cfg[loop].cfg.up_hold_to_wake, dev); - break; - - /* Set glance detector configuration */ - case BMI2_GLANCE_DETECTOR: - rslt = set_glance_detect_config(&sens_cfg[loop].cfg.glance_det, dev); - break; - - /* Set wake-up configuration */ - case BMI2_WAKE_UP: - rslt = set_wake_up_config(&sens_cfg[loop].cfg.tap, dev); - break; - - /* Set orientation configuration */ - case BMI2_ORIENTATION: - rslt = set_orient_config(&sens_cfg[loop].cfg.orientation, dev); - break; - - /* Set high-g configuration */ - case BMI2_HIGH_G: - rslt = set_high_g_config(&sens_cfg[loop].cfg.high_g, dev); - break; - - /* Set low-g configuration */ - case BMI2_LOW_G: - rslt = set_low_g_config(&sens_cfg[loop].cfg.low_g, dev); - break; - - /* Set flat configuration */ - case BMI2_FLAT: - rslt = set_flat_config(&sens_cfg[loop].cfg.flat, dev); - break; - - /* Set external sensor sync configuration */ - case BMI2_EXT_SENS_SYNC: - rslt = set_ext_sens_sync_config(&sens_cfg[loop].cfg.ext_sens_sync, dev); - break; - - /* Set the wrist gesture configuration */ - case BMI2_WRIST_GESTURE: - rslt = set_wrist_gest_config(&sens_cfg[loop].cfg.wrist_gest, dev); - break; - - /* Set the wrist wear wake-up configuration */ - case BMI2_WRIST_WEAR_WAKE_UP: - rslt = set_wrist_wear_wake_up_config(&sens_cfg[loop].cfg.wrist_wear_wake_up, dev); - break; - - /* Set the wrist gesture configuration */ - case BMI2_WRIST_GESTURE_WH: - rslt = set_wrist_gest_w_config(&sens_cfg[loop].cfg.wrist_gest_w, dev); - break; - - /* Set the wrist wear wake-up configuration for wearable variant */ - case BMI2_WRIST_WEAR_WAKE_UP_WH: - rslt = set_wrist_wear_wake_up_wh_config(&sens_cfg[loop].cfg.wrist_wear_wake_up_wh, dev); - break; - - /* Set primary OIS configuration */ - case BMI2_PRIMARY_OIS: - rslt = set_primary_ois_config(&sens_cfg[loop].cfg.primary_ois, dev); - break; - - /* Set the free-fall detection configurations */ - case BMI2_FREE_FALL_DET: - rslt = set_free_fall_det_config(&sens_cfg[loop].cfg.free_fall_det, dev); - break; - - default: - rslt = BMI2_E_INVALID_SENSOR; - break; - } - } - - /* Return error if any of the set configurations fail */ - if (rslt != BMI2_OK) - { - break; - } - } - - /* Enable Advance power save if disabled while configuring and - * not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API gets the sensor/feature configuration. - */ -int8_t bmi2_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define loop */ - uint8_t loop; - - /* Variable to get the status of advance power save */ - uint8_t aps_stat = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (sens_cfg != NULL)) - { - /* Get status of advance power save mode */ - aps_stat = dev->aps_status; - for (loop = 0; loop < n_sens; loop++) - { - /* Disable Advance power save if enabled for auxiliary - * and feature configurations - */ - if ((sens_cfg[loop].type >= BMI2_MAIN_SENS_MAX_NUM) || (sens_cfg[loop].type == BMI2_AUX)) - { - - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if - * enabled - */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - } - if (rslt == BMI2_OK) - { - switch (sens_cfg[loop].type) - { - /* Get accelerometer configuration */ - case BMI2_ACCEL: - rslt = get_accel_config(&sens_cfg[loop].cfg.acc, dev); - break; - - /* Get gyroscope configuration */ - case BMI2_GYRO: - rslt = get_gyro_config(&sens_cfg[loop].cfg.gyr, dev); - break; - - /* Get auxiliary configuration */ - case BMI2_AUX: - rslt = get_aux_config(&sens_cfg[loop].cfg.aux, dev); - break; - - /* Get sig-motion configuration */ - case BMI2_SIG_MOTION: - rslt = get_sig_motion_config(&sens_cfg[loop].cfg.sig_motion, dev); - break; - - /* Get any motion configuration */ - case BMI2_ANY_MOTION: - rslt = get_any_motion_config(&sens_cfg[loop].cfg.any_motion, dev); - break; - - /* Get no motion configuration */ - case BMI2_NO_MOTION: - rslt = get_no_motion_config(&sens_cfg[loop].cfg.no_motion, dev); - break; - - /* Set the step counter parameters */ - case BMI2_STEP_COUNTER_PARAMS: - rslt = get_step_count_params_config(sens_cfg[loop].cfg.step_counter_params, dev); - break; - - /* Get step counter/detector/activity configuration */ - case BMI2_STEP_DETECTOR: - case BMI2_STEP_COUNTER: - case BMI2_STEP_ACTIVITY: - rslt = get_step_config(&sens_cfg[loop].cfg.step_counter, dev); - break; - - /* Get gyroscope user gain configuration */ - case BMI2_GYRO_GAIN_UPDATE: - rslt = get_gyro_gain_update_config(&sens_cfg[loop].cfg.gyro_gain_update, dev); - break; - - /* Get tilt configuration */ - case BMI2_TILT: - rslt = get_tilt_config(&sens_cfg[loop].cfg.tilt, dev); - break; - - /* Get up hold to wake configuration */ - case BMI2_UP_HOLD_TO_WAKE: - rslt = get_up_hold_to_wake_config(&sens_cfg[loop].cfg.up_hold_to_wake, dev); - break; - - /* Get glance detector configuration */ - case BMI2_GLANCE_DETECTOR: - rslt = get_glance_detect_config(&sens_cfg[loop].cfg.glance_det, dev); - break; - - /* Get wake-up configuration */ - case BMI2_WAKE_UP: - rslt = get_wake_up_config(&sens_cfg[loop].cfg.tap, dev); - break; - - /* Get orientation configuration */ - case BMI2_ORIENTATION: - rslt = get_orient_config(&sens_cfg[loop].cfg.orientation, dev); - break; - - /* Get high-g configuration */ - case BMI2_HIGH_G: - rslt = get_high_g_config(&sens_cfg[loop].cfg.high_g, dev); - break; - - /* Get low-g configuration */ - case BMI2_LOW_G: - rslt = get_low_g_config(&sens_cfg[loop].cfg.low_g, dev); - break; - - /* Get flat configuration */ - case BMI2_FLAT: - rslt = get_flat_config(&sens_cfg[loop].cfg.flat, dev); - break; - - /* Get external sensor sync configuration */ - case BMI2_EXT_SENS_SYNC: - rslt = get_ext_sens_sync_config(&sens_cfg[loop].cfg.ext_sens_sync, dev); - break; - - /* Get the wrist gesture configuration */ - case BMI2_WRIST_GESTURE: - rslt = get_wrist_gest_config(&sens_cfg[loop].cfg.wrist_gest, dev); - break; - - /* Get the wrist wear wake-up configuration */ - case BMI2_WRIST_WEAR_WAKE_UP: - rslt = get_wrist_wear_wake_up_config(&sens_cfg[loop].cfg.wrist_wear_wake_up, dev); - break; - - /* Get the wrist gesture configuration for wearable variant */ - case BMI2_WRIST_GESTURE_WH: - rslt = get_wrist_gest_w_config(&sens_cfg[loop].cfg.wrist_gest_w, dev); - break; - - /* Get the wrist wear wake-up configuration for wearable variant */ - case BMI2_WRIST_WEAR_WAKE_UP_WH: - rslt = get_wrist_wear_wake_up_wh_config(&sens_cfg[loop].cfg.wrist_wear_wake_up_wh, dev); - break; - - /* Get the primary OIS filter configuration */ - case BMI2_PRIMARY_OIS: - rslt = get_primary_ois_config(&sens_cfg[loop].cfg.primary_ois, dev); - break; - - /* Get the free-fall detection configurations */ - case BMI2_FREE_FALL_DET: - rslt = get_free_fall_det_config(&sens_cfg[loop].cfg.free_fall_det, dev); - break; - - default: - rslt = BMI2_E_INVALID_SENSOR; - break; - } - } - - /* Return error if any of the get configurations fail */ - if (rslt != BMI2_OK) - { - break; - } - } - - /* Enable Advance power save if disabled while configuring and - * not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API gets the sensor/feature data for accelerometer, gyroscope, - * auxiliary sensor, step counter, high-g, gyroscope user-gain update, - * orientation, gyroscope cross sensitivity and error status for NVM and VFRM. - */ -int8_t bmi2_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define loop */ - uint8_t loop; - - /* Variable to get the status of advance power save */ - uint8_t aps_stat = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (sensor_data != NULL)) - { - /* Get status of advance power save mode */ - aps_stat = dev->aps_status; - for (loop = 0; loop < n_sens; loop++) - { - /* Disable Advance power save if enabled for feature - * configurations - */ - if (sensor_data[loop].type >= BMI2_MAIN_SENS_MAX_NUM) - { - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if - * enabled - */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - } - if (rslt == BMI2_OK) - { - switch (sensor_data[loop].type) - { - case BMI2_ACCEL: - - /* Get accelerometer data */ - rslt = get_accel_sensor_data(&sensor_data[loop].sens_data.acc, BMI2_ACC_X_LSB_ADDR, dev); - break; - case BMI2_GYRO: - - /* Get gyroscope data */ - rslt = get_gyro_sensor_data(&sensor_data[loop].sens_data.gyr, BMI2_GYR_X_LSB_ADDR, dev); - break; - case BMI2_AUX: - - /* Get auxiliary sensor data in data mode */ - rslt = read_aux_data_mode(sensor_data[loop].sens_data.aux_data, dev); - break; - case BMI2_STEP_COUNTER: - - /* Get step counter output */ - rslt = get_step_counter_output(&sensor_data[loop].sens_data.step_counter_output, dev); - break; - case BMI2_STEP_ACTIVITY: - - /* Get step activity output */ - rslt = get_step_activity_output(&sensor_data[loop].sens_data.activity_output, dev); - break; - case BMI2_ORIENTATION: - - /* Get orientation output */ - rslt = get_orient_output(&sensor_data[loop].sens_data.orient_output, dev); - break; - case BMI2_HIGH_G: - - /* Get high-g output */ - rslt = get_high_g_output(&sensor_data[loop].sens_data.high_g_output, dev); - break; - case BMI2_GYRO_GAIN_UPDATE: - - /* Get saturation status of gyroscope user gain update */ - rslt = get_gyro_gain_update_status(&sensor_data[loop].sens_data.gyro_user_gain_status, dev); - break; - case BMI2_NVM_STATUS: - - /* Get NVM error status */ - rslt = get_nvm_error_status(&sensor_data[loop].sens_data.nvm_status, dev); - break; - case BMI2_VFRM_STATUS: - - /* Get VFRM error status */ - rslt = get_vfrm_error_status(&sensor_data[loop].sens_data.vfrm_status, dev); - break; - case BMI2_WRIST_GESTURE: - case BMI2_WRIST_GESTURE_WH: - - /* Get wrist gesture status */ - rslt = get_wrist_gest_status(&sensor_data[loop].sens_data.wrist_gesture_output, dev); - break; - case BMI2_GYRO_CROSS_SENSE: - - /* Get Gyroscope cross sense value of z axis */ - rslt = get_gyro_cross_sense(&sensor_data[loop].sens_data.correction_factor_zx, dev); - break; - case BMI2_ACCEL_SELF_TEST: - - /* Get accelerometer self-test */ - rslt = get_accel_self_test_status(&sensor_data[loop].sens_data.accel_self_test_output, dev); - break; - case BMI2_OIS_OUTPUT: - - /* Get OIS accel and gyro x,y,z data */ - rslt = get_ois_output(&sensor_data[loop].sens_data.ois_output, dev); - break; - default: - rslt = BMI2_E_INVALID_SENSOR; - break; - } - - /* Return error if any of the get sensor data fails */ - if (rslt != BMI2_OK) - { - break; - } - } - - /* Enable Advance power save if disabled while - * configuring and not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - /*! * @brief This API enables/disables the advance power save mode in the sensor. */ @@ -5207,12 +2003,13 @@ int8_t bmi2_set_adv_power_save(uint8_t enable, struct bmi2_dev *dev) { reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_ADV_POW_EN, enable); rslt = bmi2_set_regs(BMI2_PWR_CONF_ADDR, ®_data, 1, dev); - dev->delay_us(500); + if (rslt != BMI2_OK) { /* Return error if enable/disable APS fails */ rslt = BMI2_E_SET_APS_FAIL; } + if (rslt == BMI2_OK) { dev->aps_status = BMI2_GET_BIT_POS0(reg_data, BMI2_ADV_POW_EN); @@ -5273,6 +2070,7 @@ int8_t bmi2_write_config_file(struct bmi2_dev *dev) { dev->read_write_len = dev->read_write_len - 1; } + if (dev->read_write_len < 2) { dev->read_write_len = 2; @@ -5399,7 +2197,7 @@ int8_t bmi2_set_int_pin_config(const struct bmi2_int_pin_config *int_cfg, struct * INT1 or INT2. * 2) The interrupt mode: permanently latched or non-latched. */ -int8_t bmi2_get_int_pin_config(struct bmi2_int_pin_config *int_cfg, const struct bmi2_dev *dev) +int8_t bmi2_get_int_pin_config(struct bmi2_int_pin_config *int_cfg, struct bmi2_dev *dev) { /* Variable to define error */ int8_t rslt; @@ -5469,7 +2267,7 @@ int8_t bmi2_get_int_pin_config(struct bmi2_int_pin_config *int_cfg, const struct * @brief This API gets the interrupt status of both feature and data * interrupts */ -int8_t bmi2_get_int_status(uint16_t *int_status, const struct bmi2_dev *dev) +int8_t bmi2_get_int_status(uint16_t *int_status, struct bmi2_dev *dev) { /* Variable to define error */ int8_t rslt; @@ -5496,6 +2294,343 @@ int8_t bmi2_get_int_status(uint16_t *int_status, const struct bmi2_dev *dev) return rslt; } +/*! + * @brief This API selects the sensors/features to be enabled. + */ +int8_t bmi2_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to select sensor */ + uint64_t sensor_sel = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_list != NULL)) + { + /* Get the selected sensors */ + rslt = select_sensor(sens_list, n_sens, &sensor_sel); + if (rslt == BMI2_OK) + { + /* Enable the selected sensors */ + rslt = sensor_enable(sensor_sel, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API selects the sensors/features to be disabled. + */ +int8_t bmi2_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to select sensor */ + uint64_t sensor_sel = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_list != NULL)) + { + /* Get the selected sensors */ + rslt = select_sensor(sens_list, n_sens, &sensor_sel); + if (rslt == BMI2_OK) + { + /* Disable the selected sensors */ + rslt = sensor_disable(sensor_sel, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API sets the sensor/feature configuration. + */ +int8_t bmi2_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_cfg != NULL)) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + + for (loop = 0; loop < n_sens; loop++) + { + /* Disable Advance power save if enabled for auxiliary + * and feature configurations + */ + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if + * enabled + */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + switch (sens_cfg[loop].type) + { + /* Set accelerometer configuration */ + case BMI2_ACCEL: + rslt = set_accel_config(&sens_cfg[loop].cfg.acc, dev); + break; + + /* Set gyroscope configuration */ + case BMI2_GYRO: + rslt = set_gyro_config(&sens_cfg[loop].cfg.gyr, dev); + break; + + /* Set auxiliary configuration */ + case BMI2_AUX: + rslt = set_aux_config(&sens_cfg[loop].cfg.aux, dev); + break; + + /* Set gyroscope user gain configuration */ + case BMI2_GYRO_GAIN_UPDATE: + rslt = set_gyro_user_gain_config(&sens_cfg[loop].cfg.gyro_gain_update, dev); + break; + + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + } + + /* Return error if any of the set configurations fail */ + if (rslt != BMI2_OK) + { + break; + } + } + + /* Enable Advance power save if disabled while configuring and + * not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API gets the sensor/feature configuration. + */ +int8_t bmi2_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_cfg != NULL)) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + for (loop = 0; loop < n_sens; loop++) + { + /* Disable Advance power save if enabled for auxiliary + * and feature configurations + */ + if ((sens_cfg[loop].type >= BMI2_MAIN_SENS_MAX_NUM) || (sens_cfg[loop].type == BMI2_AUX)) + { + + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if + * enabled + */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + } + + if (rslt == BMI2_OK) + { + switch (sens_cfg[loop].type) + { + /* Get accelerometer configuration */ + case BMI2_ACCEL: + rslt = get_accel_config(&sens_cfg[loop].cfg.acc, dev); + break; + + /* Get gyroscope configuration */ + case BMI2_GYRO: + rslt = get_gyro_config(&sens_cfg[loop].cfg.gyr, dev); + break; + + /* Get auxiliary configuration */ + case BMI2_AUX: + rslt = get_aux_config(&sens_cfg[loop].cfg.aux, dev); + break; + + /* Get gyroscope user gain configuration */ + case BMI2_GYRO_GAIN_UPDATE: + rslt = get_gyro_gain_update_config(&sens_cfg[loop].cfg.gyro_gain_update, dev); + break; + + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + } + + /* Return error if any of the get configurations fail */ + if (rslt != BMI2_OK) + { + break; + } + } + + /* Enable Advance power save if disabled while configuring and + * not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API gets the sensor/feature data for accelerometer, gyroscope, + * auxiliary sensor, step counter, high-g, gyroscope user-gain update, + * orientation, gyroscope cross sensitivity and error status for NVM and VFRM. + */ +int8_t bmi2_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sensor_data != NULL)) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + for (loop = 0; loop < n_sens; loop++) + { + /* Disable Advance power save if enabled for feature + * configurations + */ + if (sensor_data[loop].type >= BMI2_MAIN_SENS_MAX_NUM) + { + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if + * enabled + */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + } + + if (rslt == BMI2_OK) + { + switch (sensor_data[loop].type) + { + case BMI2_ACCEL: + + /* Get accelerometer data */ + rslt = get_accel_sensor_data(&sensor_data[loop].sens_data.acc, BMI2_ACC_X_LSB_ADDR, dev); + break; + case BMI2_GYRO: + + /* Get gyroscope data */ + rslt = get_gyro_sensor_data(&sensor_data[loop].sens_data.gyr, BMI2_GYR_X_LSB_ADDR, dev); + break; + case BMI2_AUX: + + /* Get auxiliary sensor data in data mode */ + rslt = read_aux_data_mode(sensor_data[loop].sens_data.aux_data, dev); + break; + + case BMI2_GYRO_CROSS_SENSE: + + /* Get Gyroscope cross sense value of z axis */ + rslt = get_gyro_cross_sense(&sensor_data[loop].sens_data.correction_factor_zx, dev); + break; + + case BMI2_GYRO_GAIN_UPDATE: + + /* Get saturation status of gyroscope user gain update */ + rslt = get_gyro_gain_update_status(&sensor_data[loop].sens_data.gyro_user_gain_status, dev); + break; + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + + /* Return error if any of the get sensor data fails */ + if (rslt != BMI2_OK) + { + break; + } + } + + /* Enable Advance power save if disabled while + * configuring and not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + /*! * @brief This API sets the FIFO configuration in the sensor. */ @@ -5544,7 +2679,7 @@ int8_t bmi2_set_fifo_config(uint16_t config, uint8_t enable, struct bmi2_dev *de rslt = get_maxburst_len(&max_burst_len, dev); if (rslt == BMI2_OK && max_burst_len == 0) { - rslt = set_maxburst_len(CRT_MIN_BURST_WORD_LENGTH, dev); + rslt = set_maxburst_len(BMI2_CRT_MIN_BURST_WORD_LENGTH, dev); } } } @@ -5567,7 +2702,7 @@ int8_t bmi2_set_fifo_config(uint16_t config, uint8_t enable, struct bmi2_dev *de /*! * @brief This API reads the FIFO configuration from the sensor. */ -int8_t bmi2_get_fifo_config(uint16_t *fifo_config, const struct bmi2_dev *dev) +int8_t bmi2_get_fifo_config(uint16_t *fifo_config, struct bmi2_dev *dev) { /* Variable to define error */ int8_t rslt; @@ -5598,7 +2733,7 @@ int8_t bmi2_get_fifo_config(uint16_t *fifo_config, const struct bmi2_dev *dev) /*! * @brief This API reads the FIFO data. */ -int8_t bmi2_read_fifo_data(struct bmi2_fifo_frame *fifo, const struct bmi2_dev *dev) +int8_t bmi2_read_fifo_data(struct bmi2_fifo_frame *fifo, struct bmi2_dev *dev) { /* Variable to define error */ int8_t rslt; @@ -5618,6 +2753,7 @@ int8_t bmi2_read_fifo_data(struct bmi2_fifo_frame *fifo, const struct bmi2_dev * /* Read FIFO data */ rslt = bmi2_get_regs(addr, fifo->data, fifo->length, dev); + if (rslt == BMI2_OK) { @@ -5688,6 +2824,7 @@ int8_t bmi2_extract_accel(struct bmi2_sens_axes_data *accel_data, { /* Unpack frame to get the accelerometer data */ rslt = unpack_accel_frame(accel_data, &data_index, &accel_index, data_enable, fifo, dev); + if (rslt != BMI2_W_FIFO_EMPTY) { /* Check for the availability of next two bytes of FIFO data */ @@ -5903,7 +3040,7 @@ int8_t bmi2_set_fifo_self_wake_up(uint8_t fifo_self_wake_up, struct bmi2_dev *de * @brief This API gets the status of FIFO self wake up functionality from * the sensor. */ -int8_t bmi2_get_fifo_self_wake_up(uint8_t *fifo_self_wake_up, const struct bmi2_dev *dev) +int8_t bmi2_get_fifo_self_wake_up(uint8_t *fifo_self_wake_up, struct bmi2_dev *dev) { /* Variable to define error */ int8_t rslt; @@ -5961,7 +3098,7 @@ int8_t bmi2_set_fifo_wm(uint16_t fifo_wm, struct bmi2_dev *dev) /*! * @brief This API reads the FIFO water mark level set in the sensor. */ -int8_t bmi2_get_fifo_wm(uint16_t *fifo_wm, const struct bmi2_dev *dev) +int8_t bmi2_get_fifo_wm(uint16_t *fifo_wm, struct bmi2_dev *dev) { /* Variable to define error */ int8_t rslt; @@ -6023,6 +3160,7 @@ int8_t bmi2_set_fifo_filter_data(uint8_t sens_sel, uint8_t fifo_filter_data, str { rslt = BMI2_E_OUT_OF_RANGE; } + break; case BMI2_GYRO: @@ -6041,6 +3179,7 @@ int8_t bmi2_set_fifo_filter_data(uint8_t sens_sel, uint8_t fifo_filter_data, str { rslt = BMI2_E_OUT_OF_RANGE; } + break; default: rslt = BMI2_E_INVALID_SENSOR; @@ -6054,7 +3193,7 @@ int8_t bmi2_set_fifo_filter_data(uint8_t sens_sel, uint8_t fifo_filter_data, str /*! * @brief This API gets the FIFO accelerometer or gyroscope filter data. */ -int8_t bmi2_get_fifo_filter_data(uint8_t sens_sel, uint8_t *fifo_filter_data, const struct bmi2_dev *dev) +int8_t bmi2_get_fifo_filter_data(uint8_t sens_sel, uint8_t *fifo_filter_data, struct bmi2_dev *dev) { /* Variable to define error */ int8_t rslt; @@ -6076,6 +3215,7 @@ int8_t bmi2_get_fifo_filter_data(uint8_t sens_sel, uint8_t *fifo_filter_data, co { (*fifo_filter_data) = BMI2_GET_BITS(data, BMI2_ACC_FIFO_FILT_DATA); } + break; case BMI2_GYRO: @@ -6085,6 +3225,7 @@ int8_t bmi2_get_fifo_filter_data(uint8_t sens_sel, uint8_t *fifo_filter_data, co { (*fifo_filter_data) = BMI2_GET_BITS(data, BMI2_GYR_FIFO_FILT_DATA); } + break; default: rslt = BMI2_E_INVALID_SENSOR; @@ -6126,6 +3267,7 @@ int8_t bmi2_set_fifo_down_sample(uint8_t sens_sel, uint8_t fifo_down_samp, struc data = BMI2_SET_BITS(data, BMI2_ACC_FIFO_DOWNS, fifo_down_samp); rslt = bmi2_set_regs(BMI2_FIFO_DOWNS_ADDR, &data, 1, dev); } + break; case BMI2_GYRO: @@ -6136,6 +3278,7 @@ int8_t bmi2_set_fifo_down_sample(uint8_t sens_sel, uint8_t fifo_down_samp, struc data = BMI2_SET_BIT_POS0(data, BMI2_GYR_FIFO_DOWNS, fifo_down_samp); rslt = bmi2_set_regs(BMI2_FIFO_DOWNS_ADDR, &data, 1, dev); } + break; default: rslt = BMI2_E_INVALID_SENSOR; @@ -6150,7 +3293,7 @@ int8_t bmi2_set_fifo_down_sample(uint8_t sens_sel, uint8_t fifo_down_samp, struc * @brief This API reads the down sampling rates which is configured for * accelerometer or gyroscope FIFO data. */ -int8_t bmi2_get_fifo_down_sample(uint8_t sens_sel, uint8_t *fifo_down_samp, const struct bmi2_dev *dev) +int8_t bmi2_get_fifo_down_sample(uint8_t sens_sel, uint8_t *fifo_down_samp, struct bmi2_dev *dev) { /* Variable to define error */ int8_t rslt; @@ -6172,6 +3315,7 @@ int8_t bmi2_get_fifo_down_sample(uint8_t sens_sel, uint8_t *fifo_down_samp, cons { (*fifo_down_samp) = BMI2_GET_BITS(data, BMI2_ACC_FIFO_DOWNS); } + break; case BMI2_GYRO: @@ -6181,6 +3325,7 @@ int8_t bmi2_get_fifo_down_sample(uint8_t sens_sel, uint8_t *fifo_down_samp, cons { (*fifo_down_samp) = BMI2_GET_BIT_POS0(data, BMI2_GYR_FIFO_DOWNS); } + break; default: rslt = BMI2_E_INVALID_SENSOR; @@ -6199,7 +3344,7 @@ int8_t bmi2_get_fifo_down_sample(uint8_t sens_sel, uint8_t *fifo_down_samp, cons * @brief This API gets the length of FIFO data available in the sensor in * bytes. */ -int8_t bmi2_get_fifo_length(uint16_t *fifo_length, const struct bmi2_dev *dev) +int8_t bmi2_get_fifo_length(uint16_t *fifo_length, struct bmi2_dev *dev) { /* Variable to define error */ int8_t rslt; @@ -6267,6 +3412,7 @@ int8_t bmi2_read_aux_man_mode(uint8_t reg_addr, uint8_t *aux_data, uint16_t len, /* Disable APS if enabled */ rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); } + if (rslt == BMI2_OK) { /* Map the register value set to that of burst @@ -6339,7 +3485,7 @@ int8_t bmi2_write_aux_man_mode(uint8_t reg_addr, const uint8_t *aux_data, uint16 for (; ((loop < len) && (rslt == BMI2_OK)); loop++) { rslt = write_aux_data((reg_addr + loop), aux_data[loop], dev); - dev->delay_us(1000); + dev->delay_us(1000, dev->intf_ptr); } } @@ -6396,6 +3542,7 @@ int8_t bmi2_write_aux_interleaved(uint8_t reg_addr, const uint8_t *aux_data, uin /* Disable APS if enabled */ rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); } + if (rslt == BMI2_OK) { /* Write the start register address extracted @@ -6410,7 +3557,7 @@ int8_t bmi2_write_aux_interleaved(uint8_t reg_addr, const uint8_t *aux_data, uin for (; ((loop < len) && (rslt == BMI2_OK)); loop += 2) { rslt = write_aux_data(aux_data[loop], aux_data[loop + 1], dev); - dev->delay_us(1000); + dev->delay_us(1000, dev->intf_ptr); } /* Enable Advance power save if disabled for @@ -6439,7 +3586,7 @@ int8_t bmi2_write_aux_interleaved(uint8_t reg_addr, const uint8_t *aux_data, uin * @brief This API gets the data ready status of accelerometer, gyroscope, * auxiliary, command decoder and busy status of auxiliary. */ -int8_t bmi2_get_status(uint8_t *status, const struct bmi2_dev *dev) +int8_t bmi2_get_status(uint8_t *status, struct bmi2_dev *dev) { /* Variable to define error */ int8_t rslt; @@ -6534,10 +3681,10 @@ int8_t bmi2_perform_accel_self_test(struct bmi2_dev *dev) struct bmi2_sens_axes_data negative = { 0, 0, 0, 0 }; /* Structure for difference of accelerometer values in g */ - struct selftest_delta_limit accel_data_diff = { 0, 0, 0 }; + struct bmi2_selftest_delta_limit accel_data_diff = { 0, 0, 0 }; /* Structure for difference of accelerometer values in mg */ - struct selftest_delta_limit accel_data_diff_mg = { 0, 0, 0 }; + struct bmi2_selftest_delta_limit accel_data_diff_mg = { 0, 0, 0 }; /* Initialize the polarity of self-test as positive */ int8_t sign = BMI2_ENABLE; @@ -6550,7 +3697,7 @@ int8_t bmi2_perform_accel_self_test(struct bmi2_dev *dev) rslt = pre_self_test_config(dev); /* Wait for greater than 2 milliseconds */ - dev->delay_us(3000); + dev->delay_us(3000, dev->intf_ptr); if (rslt == BMI2_OK) { do @@ -6562,7 +3709,7 @@ int8_t bmi2_perform_accel_self_test(struct bmi2_dev *dev) if (rslt == BMI2_OK) { /* Wait for greater than 50 milli-sec */ - dev->delay_us(51000); + dev->delay_us(51000, dev->intf_ptr); /* If polarity is positive */ if (sign == BMI2_ENABLE) @@ -6627,164 +3774,45 @@ int8_t bmi2_perform_accel_self_test(struct bmi2_dev *dev) /*! * @brief This API maps/unmaps feature interrupts to that of interrupt pins. */ -int8_t bmi2_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev) +int8_t bmi2_map_feat_int(uint8_t type, enum bmi2_hw_int_pin hw_int_pin, struct bmi2_dev *dev) { /* Variable to define error */ int8_t rslt; - /* Variable to define loop */ - uint8_t loop; - /* Variable to define the value of feature interrupts */ uint8_t feat_int = 0; - /* Variable to define the mask bits of feature interrupts */ - uint8_t feat_int_mask = 0; - /* Array to store the interrupt mask bits */ uint8_t data_array[2] = { 0 }; + /* Structure to define map the interrupts */ + struct bmi2_map_int map_int = { 0 }; + /* Null-pointer check */ rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (sens_int != NULL)) + if (rslt == BMI2_OK) { /* Read interrupt map1 and map2 and register */ rslt = bmi2_get_regs(BMI2_INT1_MAP_FEAT_ADDR, data_array, 2, dev); + if (rslt == BMI2_OK) { - for (loop = 0; loop < n_sens; loop++) - { - switch (sens_int[loop].type) - { - case BMI2_SIG_MOTION: + /* Get the value of the feature interrupt to be mapped */ + extract_feat_int_map(&map_int, type, dev); - /* Get the value of the feature interrupt to be mapped */ - feat_int = dev->int_map.sig_mot_out_conf; - break; - case BMI2_ANY_MOTION: + feat_int = map_int.sens_map_int; - /* Get the value of the feature interrupt to be mapped */ - feat_int = dev->int_map.any_mot_out_conf; - break; - case BMI2_NO_MOTION: - - /* Get the value of the feature interrupt to be mapped */ - feat_int = dev->int_map.no_mot_out_conf; - break; - case BMI2_STEP_DETECTOR: - case BMI2_STEP_COUNTER: - - /* Get the value of the feature interrupt to be mapped */ - feat_int = dev->int_map.step_det_out_conf; - break; - case BMI2_STEP_ACTIVITY: - - /* Get the value of the feature interrupt to be mapped */ - feat_int = dev->int_map.step_act_out_conf; - break; - case BMI2_TILT: - - /* Get the value of the feature interrupt to be mapped */ - feat_int = dev->int_map.tilt_out_conf; - break; - case BMI2_UP_HOLD_TO_WAKE: - - /* Get the value of the feature interrupt to be mapped */ - feat_int = dev->int_map.up_hold_to_wake_out_conf; - break; - case BMI2_GLANCE_DETECTOR: - - /* Get the value of the feature interrupt to be mapped */ - feat_int = dev->int_map.glance_out_conf; - break; - case BMI2_WAKE_UP: - - /* Get the value of the feature interrupt to be mapped */ - feat_int = dev->int_map.wake_up_out_conf; - break; - case BMI2_ORIENTATION: - - /* Get the value of the feature interrupt to be mapped */ - feat_int = dev->int_map.orient_out_conf; - break; - case BMI2_HIGH_G: - - /* Get the value of the feature interrupt to be mapped */ - feat_int = dev->int_map.high_g_out_conf; - break; - case BMI2_LOW_G: - - /* Get the value of the feature interrupt to be mapped */ - feat_int = dev->int_map.low_g_out_conf; - break; - case BMI2_FLAT: - - /* Get the value of the feature interrupt to be mapped */ - feat_int = dev->int_map.flat_out_conf; - break; - case BMI2_EXT_SENS_SYNC: - - /* Get the value of the feature interrupt to be mapped */ - feat_int = dev->int_map.ext_sync_out_conf; - break; - case BMI2_WRIST_GESTURE: - - /* Get the value of the feature interrupt to be mapped */ - feat_int = dev->int_map.wrist_gest_out_conf; - break; - case BMI2_WRIST_WEAR_WAKE_UP: - - /* Get the value of the feature interrupt to be mapped */ - feat_int = dev->int_map.wrist_wear_wake_up_out_conf; - break; - case BMI2_WRIST_GESTURE_WH: - - /* Get the value of the feature interrupt to be mapped for wearable variant */ - feat_int = dev->int_map.wrist_gest_out_conf; - break; - case BMI2_WRIST_WEAR_WAKE_UP_WH: - - /* Get the value of the feature interrupt to be mapped for wearable variant */ - feat_int = dev->int_map.wrist_wear_wake_up_out_conf; - break; - case BMI2_FREE_FALL_DET: - - /* Get the value of the feature interrupt to be mapped for free-fall detection */ - feat_int = dev->int_map.freefall_out_conf; - break; - default: - rslt = BMI2_E_INVALID_SENSOR; - break; - } - - /* Map the features to the required interrupt */ - if (rslt != BMI2_E_INVALID_SENSOR) - { - if ((feat_int > BMI2_FEAT_BIT_DISABLE) && (feat_int < BMI2_FEAT_BIT_MAX)) - { - /* Get the interrupt mask */ - feat_int_mask = (uint8_t)((uint32_t) 1 << (feat_int - 1)); - - /* Map the interrupts */ - rslt = map_feat_int(data_array, sens_int[loop].hw_int_pin, feat_int_mask); - } - else - { - rslt = BMI2_E_INVALID_FEAT_BIT; - } - } - - /* Return error if interrupt mapping fails */ - if (rslt != BMI2_OK) - { - break; - } - } + /* Map the interrupts */ + rslt = map_feat_int(data_array, hw_int_pin, feat_int); /* Map the interrupts to INT1 and INT2 map register */ if (rslt == BMI2_OK) { - rslt = bmi2_set_regs(BMI2_INT1_MAP_FEAT_ADDR, data_array, 2, dev); + rslt = bmi2_set_regs(BMI2_INT1_MAP_FEAT_ADDR, &data_array[0], 1, dev); + if (rslt == BMI2_OK) + { + rslt = bmi2_set_regs(BMI2_INT2_MAP_FEAT_ADDR, &data_array[1], 1, dev); + } } } } @@ -6876,7 +3904,7 @@ int8_t bmi2_get_remap_axes(struct bmi2_remap *remapped_axis, struct bmi2_dev *de int8_t rslt; /* Initialize the local structure for axis re-mapping */ - struct axes_remap remap = { 0, 0, 0, 0, 0, 0 }; + struct bmi2_axes_remap remap = { 0, 0, 0, 0, 0, 0 }; /* Null-pointer check */ rslt = null_ptr_check(dev); @@ -6891,22 +3919,22 @@ int8_t bmi2_get_remap_axes(struct bmi2_remap *remapped_axis, struct bmi2_dev *de */ switch (remap.x_axis) { - case MAP_X_AXIS: + case BMI2_MAP_X_AXIS: /* If mapped to x-axis */ - dev->remap.x_axis = MAP_X_AXIS; + dev->remap.x_axis = BMI2_MAP_X_AXIS; remapped_axis->x = BMI2_X; break; - case MAP_Y_AXIS: + case BMI2_MAP_Y_AXIS: /* If mapped to y-axis */ - dev->remap.x_axis = MAP_Y_AXIS; + dev->remap.x_axis = BMI2_MAP_Y_AXIS; remapped_axis->x = BMI2_Y; break; - case MAP_Z_AXIS: + case BMI2_MAP_Z_AXIS: /* If mapped to z-axis */ - dev->remap.x_axis = MAP_Z_AXIS; + dev->remap.x_axis = BMI2_MAP_Z_AXIS; remapped_axis->x = BMI2_Z; break; default: @@ -6919,12 +3947,12 @@ int8_t bmi2_get_remap_axes(struct bmi2_remap *remapped_axis, struct bmi2_dev *de if (remap.x_axis_sign) { /* If x-axis is mapped to -ve sign */ - dev->remap.x_axis_sign = NEG_SIGN; + dev->remap.x_axis_sign = BMI2_NEG_SIGN; remapped_axis->x |= BMI2_AXIS_SIGN; } else { - dev->remap.x_axis_sign = POS_SIGN; + dev->remap.x_axis_sign = BMI2_POS_SIGN; } /* Store the re-mapped y-axis value in device structure @@ -6932,22 +3960,22 @@ int8_t bmi2_get_remap_axes(struct bmi2_remap *remapped_axis, struct bmi2_dev *de */ switch (remap.y_axis) { - case MAP_X_AXIS: + case BMI2_MAP_X_AXIS: /* If mapped to x-axis */ - dev->remap.y_axis = MAP_X_AXIS; + dev->remap.y_axis = BMI2_MAP_X_AXIS; remapped_axis->y = BMI2_X; break; - case MAP_Y_AXIS: + case BMI2_MAP_Y_AXIS: /* If mapped to y-axis */ - dev->remap.y_axis = MAP_Y_AXIS; + dev->remap.y_axis = BMI2_MAP_Y_AXIS; remapped_axis->y = BMI2_Y; break; - case MAP_Z_AXIS: + case BMI2_MAP_Z_AXIS: /* If mapped to z-axis */ - dev->remap.y_axis = MAP_Z_AXIS; + dev->remap.y_axis = BMI2_MAP_Z_AXIS; remapped_axis->y = BMI2_Z; break; default: @@ -6960,12 +3988,12 @@ int8_t bmi2_get_remap_axes(struct bmi2_remap *remapped_axis, struct bmi2_dev *de if (remap.y_axis_sign) { /* If y-axis is mapped to -ve sign */ - dev->remap.y_axis_sign = NEG_SIGN; + dev->remap.y_axis_sign = BMI2_NEG_SIGN; remapped_axis->y |= BMI2_AXIS_SIGN; } else { - dev->remap.y_axis_sign = POS_SIGN; + dev->remap.y_axis_sign = BMI2_POS_SIGN; } /* Store the re-mapped z-axis value in device structure @@ -6973,22 +4001,22 @@ int8_t bmi2_get_remap_axes(struct bmi2_remap *remapped_axis, struct bmi2_dev *de */ switch (remap.z_axis) { - case MAP_X_AXIS: + case BMI2_MAP_X_AXIS: /* If mapped to x-axis */ - dev->remap.z_axis = MAP_X_AXIS; + dev->remap.z_axis = BMI2_MAP_X_AXIS; remapped_axis->z = BMI2_X; break; - case MAP_Y_AXIS: + case BMI2_MAP_Y_AXIS: /* If mapped to y-axis */ - dev->remap.z_axis = MAP_Y_AXIS; + dev->remap.z_axis = BMI2_MAP_Y_AXIS; remapped_axis->z = BMI2_Y; break; - case MAP_Z_AXIS: + case BMI2_MAP_Z_AXIS: /* If mapped to z-axis */ - dev->remap.z_axis = MAP_Z_AXIS; + dev->remap.z_axis = BMI2_MAP_Z_AXIS; remapped_axis->z = BMI2_Z; break; default: @@ -7001,12 +4029,12 @@ int8_t bmi2_get_remap_axes(struct bmi2_remap *remapped_axis, struct bmi2_dev *de if (remap.z_axis_sign) { /* If z-axis is mapped to -ve sign */ - dev->remap.z_axis_sign = NEG_SIGN; + dev->remap.z_axis_sign = BMI2_NEG_SIGN; remapped_axis->z |= BMI2_AXIS_SIGN; } else { - dev->remap.z_axis_sign = POS_SIGN; + dev->remap.z_axis_sign = BMI2_POS_SIGN; } } } @@ -7040,7 +4068,7 @@ int8_t bmi2_set_remap_axes(const struct bmi2_remap *remapped_axis, struct bmi2_d uint8_t remap_z = 0; /* Initialize the local structure for axis re-mapping */ - struct axes_remap remap = { 0, 0, 0, 0, 0, 0 }; + struct bmi2_axes_remap remap = { 0, 0, 0, 0, 0, 0 }; /* Null-pointer check */ rslt = null_ptr_check(dev); @@ -7065,20 +4093,20 @@ int8_t bmi2_set_remap_axes(const struct bmi2_remap *remapped_axis, struct bmi2_d case BMI2_X: /* If mapped to x-axis */ - dev->remap.x_axis = MAP_X_AXIS; - remap.x_axis = MAP_X_AXIS; + dev->remap.x_axis = BMI2_MAP_X_AXIS; + remap.x_axis = BMI2_MAP_X_AXIS; break; case BMI2_Y: /* If mapped to y-axis */ - dev->remap.x_axis = MAP_Y_AXIS; - remap.x_axis = MAP_Y_AXIS; + dev->remap.x_axis = BMI2_MAP_Y_AXIS; + remap.x_axis = BMI2_MAP_Y_AXIS; break; case BMI2_Z: /* If mapped to z-axis */ - dev->remap.x_axis = MAP_Z_AXIS; - remap.x_axis = MAP_Z_AXIS; + dev->remap.x_axis = BMI2_MAP_Z_AXIS; + remap.x_axis = BMI2_MAP_Z_AXIS; break; default: break; @@ -7090,13 +4118,13 @@ int8_t bmi2_set_remap_axes(const struct bmi2_remap *remapped_axis, struct bmi2_d if (remapped_axis->x & BMI2_AXIS_SIGN) { /* If x-axis is mapped to -ve sign */ - dev->remap.x_axis_sign = NEG_SIGN; - remap.x_axis_sign = MAP_NEGATIVE; + dev->remap.x_axis_sign = BMI2_NEG_SIGN; + remap.x_axis_sign = BMI2_MAP_NEGATIVE; } else { - dev->remap.x_axis_sign = POS_SIGN; - remap.x_axis_sign = MAP_POSITIVE; + dev->remap.x_axis_sign = BMI2_POS_SIGN; + remap.x_axis_sign = BMI2_MAP_POSITIVE; } /* Store the value of re-mapped y-axis in both @@ -7107,20 +4135,20 @@ int8_t bmi2_set_remap_axes(const struct bmi2_remap *remapped_axis, struct bmi2_d case BMI2_X: /* If mapped to x-axis */ - dev->remap.y_axis = MAP_X_AXIS; - remap.y_axis = MAP_X_AXIS; + dev->remap.y_axis = BMI2_MAP_X_AXIS; + remap.y_axis = BMI2_MAP_X_AXIS; break; case BMI2_Y: /* If mapped to y-axis */ - dev->remap.y_axis = MAP_Y_AXIS; - remap.y_axis = MAP_Y_AXIS; + dev->remap.y_axis = BMI2_MAP_Y_AXIS; + remap.y_axis = BMI2_MAP_Y_AXIS; break; case BMI2_Z: /* If mapped to z-axis */ - dev->remap.y_axis = MAP_Z_AXIS; - remap.y_axis = MAP_Z_AXIS; + dev->remap.y_axis = BMI2_MAP_Z_AXIS; + remap.y_axis = BMI2_MAP_Z_AXIS; break; default: break; @@ -7132,13 +4160,13 @@ int8_t bmi2_set_remap_axes(const struct bmi2_remap *remapped_axis, struct bmi2_d if (remapped_axis->y & BMI2_AXIS_SIGN) { /* If y-axis is mapped to -ve sign */ - dev->remap.y_axis_sign = NEG_SIGN; - remap.y_axis_sign = MAP_NEGATIVE; + dev->remap.y_axis_sign = BMI2_NEG_SIGN; + remap.y_axis_sign = BMI2_MAP_NEGATIVE; } else { - dev->remap.y_axis_sign = POS_SIGN; - remap.y_axis_sign = MAP_POSITIVE; + dev->remap.y_axis_sign = BMI2_POS_SIGN; + remap.y_axis_sign = BMI2_MAP_POSITIVE; } /* Store the value of re-mapped z-axis in both @@ -7149,20 +4177,20 @@ int8_t bmi2_set_remap_axes(const struct bmi2_remap *remapped_axis, struct bmi2_d case BMI2_X: /* If mapped to x-axis */ - dev->remap.z_axis = MAP_X_AXIS; - remap.z_axis = MAP_X_AXIS; + dev->remap.z_axis = BMI2_MAP_X_AXIS; + remap.z_axis = BMI2_MAP_X_AXIS; break; case BMI2_Y: /* If mapped to y-axis */ - dev->remap.z_axis = MAP_Y_AXIS; - remap.z_axis = MAP_Y_AXIS; + dev->remap.z_axis = BMI2_MAP_Y_AXIS; + remap.z_axis = BMI2_MAP_Y_AXIS; break; case BMI2_Z: /* If mapped to z-axis */ - dev->remap.z_axis = MAP_Z_AXIS; - remap.z_axis = MAP_Z_AXIS; + dev->remap.z_axis = BMI2_MAP_Z_AXIS; + remap.z_axis = BMI2_MAP_Z_AXIS; break; default: break; @@ -7174,13 +4202,13 @@ int8_t bmi2_set_remap_axes(const struct bmi2_remap *remapped_axis, struct bmi2_d if (remapped_axis->z & BMI2_AXIS_SIGN) { /* If z-axis is mapped to -ve sign */ - dev->remap.z_axis_sign = NEG_SIGN; - remap.z_axis_sign = MAP_NEGATIVE; + dev->remap.z_axis_sign = BMI2_NEG_SIGN; + remap.z_axis_sign = BMI2_MAP_NEGATIVE; } else { - dev->remap.z_axis_sign = POS_SIGN; - remap.z_axis_sign = MAP_POSITIVE; + dev->remap.z_axis_sign = BMI2_POS_SIGN; + remap.z_axis_sign = BMI2_MAP_POSITIVE; } /* Set the re-mapped axes in the sensor */ @@ -7199,136 +4227,6 @@ int8_t bmi2_set_remap_axes(const struct bmi2_remap *remapped_axis, struct bmi2_d return rslt; } -/*! - * @brief This API updates the gyroscope user-gain. - */ -int8_t bmi2_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to select sensor */ - uint8_t sens_sel[2] = { BMI2_GYRO, BMI2_GYRO_GAIN_UPDATE }; - - /* Structure to define sensor configurations */ - struct bmi2_sens_config sens_cfg; - - /* Variable to store status of user-gain update module */ - uint8_t status = 0; - - /* Variable to define count */ - uint8_t count = 100; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (user_gain != NULL)) - { - /* Select type of feature */ - sens_cfg.type = BMI2_GYRO_GAIN_UPDATE; - - /* Get the user gain configurations */ - rslt = bmi2_get_sensor_config(&sens_cfg, 1, dev); - if (rslt == BMI2_OK) - { - /* Get the user-defined ratio */ - sens_cfg.cfg.gyro_gain_update = *user_gain; - - /* Set rate ratio for all axes */ - rslt = bmi2_set_sensor_config(&sens_cfg, 1, dev); - } - - /* Disable gyroscope */ - if (rslt == BMI2_OK) - { - rslt = bmi2_sensor_disable(&sens_sel[0], 1, dev); - } - - /* Enable gyroscope user-gain update module */ - if (rslt == BMI2_OK) - { - rslt = bmi2_sensor_enable(&sens_sel[1], 1, dev); - } - - /* Set the command to trigger the computation */ - if (rslt == BMI2_OK) - { - rslt = bmi2_set_command_register(BMI2_USR_GAIN_CMD, dev); - } - if (rslt == BMI2_OK) - { - /* Poll until enable bit of user-gain update is 0 */ - while (count--) - { - rslt = get_user_gain_upd_status(&status, dev); - if ((rslt == BMI2_OK) && (status == 0)) - { - /* Enable compensation of gain defined - * in the GAIN register - */ - rslt = enable_gyro_gain(BMI2_ENABLE, dev); - - /* Enable gyroscope */ - if (rslt == BMI2_OK) - { - rslt = bmi2_sensor_enable(&sens_sel[0], 1, dev); - } - break; - } - dev->delay_us(10000); - } - - /* Return error if user-gain update is failed */ - if ((rslt == BMI2_OK) && (status != 0)) - { - rslt = BMI2_E_GYR_USER_GAIN_UPD_FAIL; - } - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API reads the compensated gyroscope user-gain values. - */ -int8_t bmi2_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, const struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define register data */ - uint8_t reg_data[3] = { 0 }; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (gyr_usr_gain != NULL)) - { - /* Get the gyroscope compensated gain values */ - rslt = bmi2_get_regs(BMI2_GYR_USR_GAIN_0_ADDR, reg_data, 3, dev); - if (rslt == BMI2_OK) - { - /* Gyroscope user gain correction X-axis */ - gyr_usr_gain->x = (int8_t)BMI2_GET_BIT_POS0(reg_data[0], BMI2_GYR_USR_GAIN_X); - - /* Gyroscope user gain correction Y-axis */ - gyr_usr_gain->y = (int8_t)BMI2_GET_BIT_POS0(reg_data[1], BMI2_GYR_USR_GAIN_Y); - - /* Gyroscope user gain correction z-axis */ - gyr_usr_gain->z = (int8_t)BMI2_GET_BIT_POS0(reg_data[2], BMI2_GYR_USR_GAIN_Z); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - /*! * @brief This API enables/disables gyroscope offset compensation. It adds the * offsets defined in the offset register with gyroscope data. @@ -7367,7 +4265,7 @@ int8_t bmi2_set_gyro_offset_comp(uint8_t enable, struct bmi2_dev *dev) * @brief This API reads the gyroscope bias values for each axis which is used * for gyroscope offset compensation. */ -int8_t bmi2_read_gyro_offset_comp_axes(struct bmi2_sens_axes_data *gyr_off_comp_axes, const struct bmi2_dev *dev) +int8_t bmi2_read_gyro_offset_comp_axes(struct bmi2_sens_axes_data *gyr_off_comp_axes, struct bmi2_dev *dev) { /* Variable to define error */ int8_t rslt; @@ -7407,9 +4305,9 @@ int8_t bmi2_read_gyro_offset_comp_axes(struct bmi2_sens_axes_data *gyr_off_comp_ gyr_off_lsb_x = reg_data[0]; gyr_off_lsb_y = reg_data[1]; gyr_off_lsb_z = reg_data[2]; - gyr_off_msb_x = reg_data[3] & GYR_OFF_COMP_MSB_X_MASK; - gyr_off_msb_y = reg_data[3] & GYR_OFF_COMP_MSB_Y_MASK; - gyr_off_msb_z = reg_data[3] & GYR_OFF_COMP_MSB_Z_MASK; + gyr_off_msb_x = reg_data[3] & BMI2_GYR_OFF_COMP_MSB_X_MASK; + gyr_off_msb_y = reg_data[3] & BMI2_GYR_OFF_COMP_MSB_Y_MASK; + gyr_off_msb_z = reg_data[3] & BMI2_GYR_OFF_COMP_MSB_Z_MASK; /* Gyroscope offset compensation value for x-axis */ gyr_off_comp_axes->x = (int16_t)(((uint16_t) gyr_off_msb_x << 8) | gyr_off_lsb_x); @@ -7459,31 +4357,31 @@ int8_t bmi2_write_gyro_offset_comp_axes(const struct bmi2_sens_axes_data *gyr_of if (rslt == BMI2_OK) { /* Get MSB value of x-axis from user-input */ - gyr_off_msb_x = (uint8_t)((gyr_off_comp_axes->x & GYR_OFF_COMP_MSB_MASK) >> 8); + gyr_off_msb_x = (uint8_t)((gyr_off_comp_axes->x & BMI2_GYR_OFF_COMP_MSB_MASK) >> 8); /* Get MSB value of y-axis from user-input */ - gyr_off_msb_y = (uint8_t)((gyr_off_comp_axes->y & GYR_OFF_COMP_MSB_MASK) >> 8); + gyr_off_msb_y = (uint8_t)((gyr_off_comp_axes->y & BMI2_GYR_OFF_COMP_MSB_MASK) >> 8); /* Get MSB value of z-axis from user-input */ - gyr_off_msb_z = (uint8_t)((gyr_off_comp_axes->z & GYR_OFF_COMP_MSB_MASK) >> 8); + gyr_off_msb_z = (uint8_t)((gyr_off_comp_axes->z & BMI2_GYR_OFF_COMP_MSB_MASK) >> 8); /* Get LSB value of x-axis from user-input */ - reg_data[0] = (uint8_t)(gyr_off_comp_axes->x & GYR_OFF_COMP_LSB_MASK); + reg_data[0] = (uint8_t)(gyr_off_comp_axes->x & BMI2_GYR_OFF_COMP_LSB_MASK); /* Get LSB value of y-axis from user-input */ - reg_data[1] = (uint8_t)(gyr_off_comp_axes->y & GYR_OFF_COMP_LSB_MASK); + reg_data[1] = (uint8_t)(gyr_off_comp_axes->y & BMI2_GYR_OFF_COMP_LSB_MASK); /* Get LSB value of z-axis from user-input */ - reg_data[2] = (uint8_t)(gyr_off_comp_axes->z & GYR_OFF_COMP_LSB_MASK); + reg_data[2] = (uint8_t)(gyr_off_comp_axes->z & BMI2_GYR_OFF_COMP_LSB_MASK); /* Get MSB value of x-axis to be set */ - reg_data[3] = BMI2_SET_BIT_POS0(reg_data[3], GYR_OFF_COMP_MSB_X, gyr_off_msb_x); + reg_data[3] = BMI2_SET_BIT_POS0(reg_data[3], BMI2_GYR_OFF_COMP_MSB_X, gyr_off_msb_x); /* Get MSB value of y-axis to be set */ - reg_data[3] = BMI2_SET_BITS(reg_data[3], GYR_OFF_COMP_MSB_Y, gyr_off_msb_y); + reg_data[3] = BMI2_SET_BITS(reg_data[3], BMI2_GYR_OFF_COMP_MSB_Y, gyr_off_msb_y); /* Get MSB value of z-axis to be set */ - reg_data[3] = BMI2_SET_BITS(reg_data[3], GYR_OFF_COMP_MSB_Z, gyr_off_msb_z); + reg_data[3] = BMI2_SET_BITS(reg_data[3], BMI2_GYR_OFF_COMP_MSB_Z, gyr_off_msb_z); /* Set the offset compensation values of axes */ rslt = bmi2_set_regs(BMI2_GYR_OFF_COMP_3_ADDR, reg_data, 4, dev); @@ -7497,155 +4395,6 @@ int8_t bmi2_write_gyro_offset_comp_axes(const struct bmi2_sens_axes_data *gyr_of return rslt; } -/*! - * @brief This internal API is used to parse the activity output from the - * FIFO in header mode. - */ -int8_t bmi2_get_act_recog_output(struct bmi2_act_recog_output *act_recog, - uint16_t *act_frm_len, - struct bmi2_fifo_frame *fifo, - const struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define header frame */ - uint8_t frame_header = 0; - - /* Variable to index the data bytes */ - uint16_t data_index; - - /* Variable to index activity frames */ - uint16_t act_idx = 0; - - /* Variable to indicate activity frames read */ - uint16_t frame_to_read = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (act_recog != NULL) && (act_frm_len != NULL) && (fifo != NULL)) - { - - /* Store the number of frames to be read */ - frame_to_read = *act_frm_len; - for (data_index = fifo->act_recog_byte_start_idx; data_index < fifo->length;) - { - /* Get frame header byte */ - frame_header = fifo->data[data_index] & BMI2_FIFO_TAG_INTR_MASK; - - /* Skip S4S frames if S4S is enabled */ - rslt = move_if_s4s_frame(&frame_header, &data_index, fifo); - - /* Break if FIFO is empty */ - if (rslt == BMI2_W_FIFO_EMPTY) - { - break; - } - - /* Index shifted to next byte where data starts */ - data_index++; - switch (frame_header) - { - /* If header defines accelerometer frame */ - case BMI2_FIFO_HEADER_ACC_FRM: - rslt = move_next_frame(&data_index, fifo->acc_frm_len, fifo); - break; - - /* If header defines accelerometer and auxiliary frames */ - case BMI2_FIFO_HEADER_AUX_ACC_FRM: - rslt = move_next_frame(&data_index, fifo->acc_aux_frm_len, fifo); - break; - - /* If header defines accelerometer and gyroscope frames */ - case BMI2_FIFO_HEADER_GYR_ACC_FRM: - rslt = move_next_frame(&data_index, fifo->acc_gyr_frm_len, fifo); - break; - - /* If header defines accelerometer, auxiliary and gyroscope frames */ - case BMI2_FIFO_HEADER_ALL_FRM: - rslt = move_next_frame(&data_index, fifo->all_frm_len, fifo); - break; - - /* If header defines only gyroscope frame */ - case BMI2_FIFO_HEADER_GYR_FRM: - rslt = move_next_frame(&data_index, fifo->gyr_frm_len, fifo); - break; - - /* If header defines only auxiliary frame */ - case BMI2_FIFO_HEADER_AUX_FRM: - rslt = move_next_frame(&data_index, fifo->aux_frm_len, fifo); - break; - - /* If header defines auxiliary and gyroscope frame */ - case BMI2_FIFO_HEADER_AUX_GYR_FRM: - rslt = move_next_frame(&data_index, fifo->aux_gyr_frm_len, fifo); - break; - - /* If header defines sensor time frame */ - case BMI2_FIFO_HEADER_SENS_TIME_FRM: - rslt = move_next_frame(&data_index, BMI2_SENSOR_TIME_LENGTH, fifo); - break; - - /* If header defines skip frame */ - case BMI2_FIFO_HEADER_SKIP_FRM: - rslt = move_next_frame(&data_index, BMI2_FIFO_SKIP_FRM_LENGTH, fifo); - break; - - /* If header defines Input configuration frame */ - case BMI2_FIFO_HEADER_INPUT_CFG_FRM: - rslt = move_next_frame(&data_index, BMI2_FIFO_INPUT_CFG_LENGTH, fifo); - break; - - /* If header defines invalid frame or end of valid data */ - case BMI2_FIFO_HEAD_OVER_READ_MSB: - - /* Move the data index to the last byte to mark completion */ - data_index = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - break; - - /* If header defines activity recognition frame */ - case BMI2_FIFO_VIRT_ACT_RECOG_FRM: - - /* Get the activity output */ - rslt = unpack_act_recog_output(&act_recog[(act_idx)], &data_index, fifo); - - /* Update activity frame index */ - (act_idx)++; - break; - default: - - /* Move the data index to the last byte in case of invalid values */ - data_index = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - break; - } - - /* Number of frames to be read is complete or FIFO is empty */ - if ((frame_to_read == act_idx) || (rslt == BMI2_W_FIFO_EMPTY)) - { - break; - } - } - - /* Update the activity frame index */ - (*act_frm_len) = act_idx; - - /* Update the activity byte index */ - fifo->act_recog_byte_start_idx = data_index; - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - /*! * @brief This API updates the cross sensitivity coefficient between gyroscope's * X and Z axes. @@ -7687,7 +4436,7 @@ int8_t bmi2_get_gyro_cross_sense(struct bmi2_dev *dev) /*! * @brief This API gets Error bits and message indicating internal status. */ -int8_t bmi2_get_internal_status(uint8_t *int_stat, const struct bmi2_dev *dev) +int8_t bmi2_get_internal_status(uint8_t *int_stat, struct bmi2_dev *dev) { /* Variable to define error */ int8_t rslt; @@ -7697,7 +4446,7 @@ int8_t bmi2_get_internal_status(uint8_t *int_stat, const struct bmi2_dev *dev) if ((rslt == BMI2_OK) && (int_stat != NULL)) { /* Delay to read the internal status */ - dev->delay_us(20000); + dev->delay_us(20000, dev->intf_ptr); /* Get the error bits and message */ rslt = bmi2_get_regs(BMI2_INTERNAL_STATUS_ADDR, int_stat, 1, dev); @@ -7710,16 +4459,23 @@ int8_t bmi2_get_internal_status(uint8_t *int_stat, const struct bmi2_dev *dev) return rslt; } +/*! @cond DOXYGEN_SUPRESS */ + +/* Suppressing doxygen warnings triggered for same static function names present across various sensor variant + * directories */ + /*! * @brief This API verifies and allows only the correct position to do Fast Offset Compensation for * accelerometer & gyro. */ -static int8_t verify_foc_position(uint8_t sens_list, const struct accel_foc_g_value *accel_g_axis, struct bmi2_dev *dev) +static int8_t verify_foc_position(uint8_t sens_list, + const struct bmi2_accel_foc_g_value *accel_g_axis, + struct bmi2_dev *dev) { int8_t rslt; struct bmi2_sens_axes_data avg_foc_data = { 0 }; - struct foc_temp_value temp_foc_data = { 0 }; + struct bmi2_foc_temp_value temp_foc_data = { 0 }; rslt = null_ptr_check(dev); if (rslt == BMI2_OK) @@ -7727,6 +4483,7 @@ static int8_t verify_foc_position(uint8_t sens_list, const struct accel_foc_g_va /* Enable sensor */ rslt = bmi2_sensor_enable(&sens_list, 1, dev); } + if (rslt == BMI2_OK) { @@ -7763,10 +4520,12 @@ static int8_t verify_foc_position(uint8_t sens_list, const struct accel_foc_g_va return rslt; } +/*! @endcond */ + /*! * @brief This API performs Fast Offset Compensation for accelerometer. */ -int8_t bmi2_perform_accel_foc(const struct accel_foc_g_value *accel_g_value, struct bmi2_dev *dev) +int8_t bmi2_perform_accel_foc(const struct bmi2_accel_foc_g_value *accel_g_value, struct bmi2_dev *dev) { /* Variable to define error */ int8_t rslt; @@ -7850,7 +4609,7 @@ int8_t bmi2_perform_gyro_foc(struct bmi2_dev *dev) struct bmi2_sens_axes_data gyr_value[128]; /* Structure to store gyroscope data temporarily */ - struct foc_temp_value temp = { 0, 0, 0 }; + struct bmi2_foc_temp_value temp = { 0, 0, 0 }; /* Variable to store status read from the status register */ uint8_t reg_status = 0; @@ -7886,7 +4645,7 @@ int8_t bmi2_perform_gyro_foc(struct bmi2_dev *dev) for (loop = 0; loop < 128; loop++) { /* Giving a delay of more than 40ms since ODR is configured as 25Hz */ - dev->delay_us(50000); + dev->delay_us(50000, dev->intf_ptr); /* Get gyroscope data ready interrupt status */ rslt = bmi2_get_status(®_status, dev); @@ -7903,6 +4662,7 @@ int8_t bmi2_perform_gyro_foc(struct bmi2_dev *dev) temp.z = temp.z + (int32_t)gyr_value[loop].z; } } + if (rslt != BMI2_OK) { break; @@ -7913,6 +4673,7 @@ int8_t bmi2_perform_gyro_foc(struct bmi2_dev *dev) break; } } + if (rslt == BMI2_OK) { /* Take average of x, y and z data for lesser @@ -7957,187 +4718,124 @@ int8_t bmi2_perform_gyro_foc(struct bmi2_dev *dev) } /*! - * @brief This api is used for retrieving the activity recognition settings currently set. + * @brief This API is used to get the feature configuration from the + * selected page. */ -int8_t bmi2_get_act_recg_sett(struct act_recg_sett *sett, struct bmi2_dev *dev) +int8_t bmi2_get_feat_config(uint8_t sw_page, uint8_t *feat_config, struct bmi2_dev *dev) { /* Variable to define error */ - int8_t rslt = BMI2_OK; + int8_t rslt; - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + /* Variable to define bytes remaining to read */ + uint8_t bytes_remain = BMI2_FEAT_SIZE_IN_BYTES; - /* Variable to define the array offset */ - uint8_t idx = 0; + /* Variable to define the read-write length */ + uint8_t read_write_len = 0; - /* Variable to get the status of advance power save */ - uint8_t aps_stat; + /* Variable to define the feature configuration address */ + uint8_t addr = BMI2_FEATURES_REG_ADDR; - /* Variable to set flag */ - uint8_t feat_found; - uint16_t msb_lsb; - uint8_t lsb; - uint8_t msb; + /* Variable to define index */ + uint8_t index = 0; - /* Initialize feature configuration for activity recognition */ - struct bmi2_feature_config act_recg_sett = { 0, 0, 0 }; - - /* Search for bmi2 Abort feature and extract its configuration details */ - feat_found = extract_input_feat_config(&act_recg_sett, BMI2_ACTIVITY_RECOGNITION_SETTINGS, dev); - if (feat_found) + if ((feat_config == NULL) || (dev == NULL)) { - aps_stat = dev->aps_status; - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if enabled */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - - /* Get the configuration from the page where activity recognition setting feature resides */ - if (rslt == BMI2_OK) - { - rslt = get_feat_config(act_recg_sett.page, feat_config, dev); - } - if (rslt == BMI2_OK) - { - /* Define the offset in bytes */ - idx = act_recg_sett.start_addr; - - /* get the status of enable/disable post processing */ - sett->act_rec_1 = BMI2_GET_BIT_POS0(feat_config[idx], BMI2_ACT_RECG_POST_PROS_EN_DIS); - - /* increment idx by 2 to point min gdi thres addres */ - idx = idx + 2; - lsb = feat_config[idx]; - idx++; - msb = feat_config[idx]; - msb_lsb = (uint16_t)(lsb | msb << 8); - sett->act_rec_2 = msb_lsb; - - /* increment idx by 1 to point max gdi thres addres */ - idx++; - lsb = feat_config[idx]; - idx++; - msb = feat_config[idx]; - msb_lsb = (uint16_t)(lsb | msb << 8); - sett->act_rec_3 = msb_lsb; - - /* increment idx by 1 to point buffer size */ - idx++; - sett->act_rec_4 = BMI2_GET_BIT_POS0(feat_config[idx], BMI2_ACT_RECG_BUFF_SIZE); - - /* increment idx by 2 to to point to min segment confidence */ - idx = idx + 2; - sett->act_rec_5 = BMI2_GET_BIT_POS0(feat_config[idx], BMI2_ACT_RECG_MIN_SEG_CONF); - - } - - /* Enable Advance power save if disabled while - * configuring and not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } + rslt = BMI2_E_NULL_PTR; } else { - rslt = BMI2_E_INVALID_SENSOR; + /* Check whether the page is valid */ + if (sw_page < dev->page_max) + { + /* Switch page */ + rslt = bmi2_set_regs(BMI2_FEAT_PAGE_ADDR, &sw_page, 1, dev); + + /* If user length is less than feature length */ + if ((rslt == BMI2_OK) && (dev->read_write_len < BMI2_FEAT_SIZE_IN_BYTES)) + { + /* Read-write should be even */ + if ((dev->read_write_len % 2) != 0) + { + dev->read_write_len--; + } + + while (bytes_remain > 0) + { + if (bytes_remain >= dev->read_write_len) + { + /* Read from the page */ + rslt = bmi2_get_regs(addr, &feat_config[index], dev->read_write_len, dev); + + /* Update index */ + index += (uint8_t) dev->read_write_len; + + /* Update address */ + addr += (uint8_t) dev->read_write_len; + + /* Update read-write length */ + read_write_len += (uint8_t) dev->read_write_len; + } + else + { + /* Read from the page */ + rslt = bmi2_get_regs(addr, (uint8_t *) (feat_config + index), (uint16_t) bytes_remain, dev); + + /* Update read-write length */ + read_write_len += bytes_remain; + } + + /* Remaining bytes */ + bytes_remain = BMI2_FEAT_SIZE_IN_BYTES - read_write_len; + + if (rslt != BMI2_OK) + { + break; + } + } + } + else if (rslt == BMI2_OK) + { + /* Get configuration from the page */ + rslt = bmi2_get_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_PAGE; + } } return rslt; } /*! - * @brief This api is used for setting the activity recognition settings. + * @brief This API is used to extract the input feature configuration + * details from the look-up table. */ -int8_t bmi2_set_act_recg_sett(const struct act_recg_sett *sett, struct bmi2_dev *dev) +uint8_t bmi2_extract_input_feat_config(struct bmi2_feature_config *feat_config, uint8_t type, + const struct bmi2_dev *dev) { - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to get the status of advance power save */ - uint8_t aps_stat; + /* Variable to define loop */ + uint8_t loop = 0; /* Variable to set flag */ - uint8_t feat_found; + uint8_t feat_found = BMI2_FALSE; - /* Initialize feature configuration for activity recognition */ - struct bmi2_feature_config act_recg_sett = { 0, 0, 0 }; - - /* Search for bmi2 Abort feature and extract its configuration details */ - feat_found = extract_input_feat_config(&act_recg_sett, BMI2_ACTIVITY_RECOGNITION_SETTINGS, dev); - if (feat_found) + /* Search for the input feature from the input configuration array */ + while (loop < dev->input_sens) { - aps_stat = dev->aps_status; - if (aps_stat == BMI2_ENABLE) + if (dev->feat_config[loop].type == type) { - /* Disable advance power save if enabled */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + *feat_config = dev->feat_config[loop]; + feat_found = BMI2_TRUE; + break; } - /* Get the configuration from the page where activity recognition setting feature resides */ - if (rslt == BMI2_OK) - { - rslt = get_feat_config(act_recg_sett.page, feat_config, dev); - } - if (rslt == BMI2_OK) - { - /* Define the offset in bytes */ - idx = act_recg_sett.start_addr; - if ((sett->act_rec_4 > 10) || (sett->act_rec_5 > 10)) - { - rslt = BMI2_E_INVALID_INPUT; - } - if (rslt == BMI2_OK) - { - feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], BMI2_ACT_RECG_POST_PROS_EN_DIS, sett->act_rec_1); - - /* increment idx by 2 to point min gdi thres addres */ - idx = idx + 2; - feat_config[idx] = BMI2_GET_LSB(sett->act_rec_2); - idx++; - feat_config[idx] = BMI2_GET_MSB(sett->act_rec_2); - - /* increment idx by 1 to point max gdi thres addres */ - idx++; - feat_config[idx] = BMI2_GET_LSB(sett->act_rec_3); - idx++; - feat_config[idx] = BMI2_GET_MSB(sett->act_rec_3); - - /* increment idx by 1 to point buffer size */ - idx++; - feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], BMI2_ACT_RECG_BUFF_SIZE, sett->act_rec_4); - - /* increment idx by 2 to to point to min segment confidence */ - idx = idx + 2; - feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], BMI2_ACT_RECG_MIN_SEG_CONF, sett->act_rec_5); - - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - - } - - /* Enable Advance power save if disabled while - * configuring and not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; + loop++; } - return rslt; + /* Return flag */ + return feat_found; } /***************************************************************************/ @@ -8145,6 +4843,11 @@ int8_t bmi2_set_act_recg_sett(const struct act_recg_sett *sett, struct bmi2_dev /*! Local Function Definitions ****************************************************************************/ +/*! @cond DOXYGEN_SUPRESS */ + +/* Suppressing doxygen warnings triggered for same static function names present across various sensor variant + * directories */ + /*! * @brief This internal API writes the configuration file. */ @@ -8194,6 +4897,7 @@ static int8_t write_config_file(struct bmi2_dev *dev) { rslt = upload_file((dev->config_file_ptr + index), index, dev->read_write_len, dev); } + if (rslt == BMI2_OK) { /* Update length in a temporary variable */ @@ -8214,18 +4918,18 @@ static int8_t write_config_file(struct bmi2_dev *dev) dev->read_write_len = read_write_len; } } + if (rslt == BMI2_OK) { /* Enable loading of the configuration */ rslt = set_config_load(BMI2_ENABLE, dev); /* Wait till ASIC is initialized */ - dev->delay_us(150000); + dev->delay_us(150000, dev->intf_ptr); if (rslt == BMI2_OK) { /* Enable advanced power save mode */ rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - dev->delay_us(1000); } } } @@ -8291,2125 +4995,6 @@ static int8_t upload_file(const uint8_t *config_data, uint16_t index, uint16_t w return rslt; } -/*! - * @brief This internal API enables the selected sensor/features. - */ -static int8_t sensor_enable(uint32_t sensor_sel, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store register values */ - uint8_t reg_data = 0; - - /* Variable to define loop */ - uint8_t loop = 1; - - /* Variable to get the status of advance power save */ - uint8_t aps_stat = 0; - - rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); - if (rslt == BMI2_OK) - { - /* Enable accelerometer */ - if (sensor_sel & BMI2_ACCEL_SENS_SEL) - { - reg_data = BMI2_SET_BITS(reg_data, BMI2_ACC_EN, BMI2_ENABLE); - } - - /* Enable gyroscope */ - if (sensor_sel & BMI2_GYRO_SENS_SEL) - { - reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_EN, BMI2_ENABLE); - } - - /* Enable auxiliary sensor */ - if (sensor_sel & BMI2_AUX_SENS_SEL) - { - reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_AUX_EN, BMI2_ENABLE); - } - - /* Enable temperature sensor */ - if (sensor_sel & BMI2_TEMP_SENS_SEL) - { - reg_data = BMI2_SET_BITS(reg_data, BMI2_TEMP_EN, BMI2_ENABLE); - } - - /* Enable the sensors that are set in the power control register */ - if (sensor_sel & BMI2_MAIN_SENSORS) - { - rslt = bmi2_set_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); - } - } - if ((rslt == BMI2_OK) && (sensor_sel & ~(BMI2_MAIN_SENSORS))) - { - /* Get status of advance power save mode */ - aps_stat = dev->aps_status; - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if enabled */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - if (rslt == BMI2_OK) - { - while (loop--) - { - /* Enable sig-motion feature */ - if (sensor_sel & BMI2_SIG_MOTION_SEL) - { - rslt = set_sig_motion(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_SIG_MOTION_SEL; - } - else - { - break; - } - } - - /* Enable any motion feature */ - if (sensor_sel & BMI2_ANY_MOT_SEL) - { - rslt = set_any_motion(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_ANY_MOT_SEL; - } - else - { - break; - } - } - - /* Enable no motion feature */ - if (sensor_sel & BMI2_NO_MOT_SEL) - { - rslt = set_no_motion(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_NO_MOT_SEL; - } - else - { - break; - } - } - - /* Enable step detector feature */ - if (sensor_sel & BMI2_STEP_DETECT_SEL) - { - rslt = set_step_detector(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_STEP_DETECT_SEL; - } - else - { - break; - } - } - - /* Enable step counter feature */ - if (sensor_sel & BMI2_STEP_COUNT_SEL) - { - rslt = set_step_counter(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_STEP_COUNT_SEL; - } - else - { - break; - } - } - - /* Enable step activity feature */ - if (sensor_sel & BMI2_STEP_ACT_SEL) - { - rslt = set_step_activity(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_STEP_ACT_SEL; - } - else - { - break; - } - } - - /* Enable gyroscope user gain */ - if (sensor_sel & BMI2_GYRO_GAIN_UPDATE_SEL) - { - rslt = set_gyro_user_gain(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_GYRO_GAIN_UPDATE_SEL; - } - else - { - break; - } - } - - /* Enable tilt feature */ - if (sensor_sel & BMI2_TILT_SEL) - { - rslt = set_tilt(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_TILT_SEL; - } - else - { - break; - } - } - - /* Enable uphold to wake feature */ - if (sensor_sel & BMI2_UP_HOLD_TO_WAKE_SEL) - { - rslt = set_up_hold_to_wake(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_UP_HOLD_TO_WAKE_SEL; - } - else - { - break; - } - } - - /* Enable glance feature */ - if (sensor_sel & BMI2_GLANCE_DET_SEL) - { - rslt = set_glance_detector(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_GLANCE_DET_SEL; - } - else - { - break; - } - } - - /* Enable wake-up feature */ - if (sensor_sel & BMI2_WAKE_UP_SEL) - { - rslt = set_wake_up(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_WAKE_UP_SEL; - } - else - { - break; - } - } - - /* Enable orientation feature */ - if (sensor_sel & BMI2_ORIENT_SEL) - { - rslt = set_orientation(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_ORIENT_SEL; - } - else - { - break; - } - } - - /* Enable high-g feature */ - if (sensor_sel & BMI2_HIGH_G_SEL) - { - rslt = set_high_g(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_HIGH_G_SEL; - } - else - { - break; - } - } - - /* Enable low-g feature */ - if (sensor_sel & BMI2_LOW_G_SEL) - { - rslt = set_low_g(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_LOW_G_SEL; - } - else - { - break; - } - } - - /* Enable flat feature */ - if (sensor_sel & BMI2_FLAT_SEL) - { - rslt = set_flat(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_FLAT_SEL; - } - else - { - break; - } - } - - /* Enable external sensor feature */ - if (sensor_sel & BMI2_EXT_SENS_SEL) - { - rslt = set_ext_sens_sync(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_EXT_SENS_SEL; - } - else - { - break; - } - } - - /* Enable gyroscope self-offset correction feature */ - if (sensor_sel & BMI2_GYRO_SELF_OFF_SEL) - { - rslt = set_gyro_self_offset_corr(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_GYRO_SELF_OFF_SEL; - } - else - { - break; - } - } - - /* Enable wrist gesture feature */ - if (sensor_sel & BMI2_WRIST_GEST_SEL) - { - rslt = set_wrist_gesture(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_WRIST_GEST_SEL; - } - else - { - break; - } - } - - /* Enable wrist wear wake-up feature */ - if (sensor_sel & BMI2_WRIST_WEAR_WAKE_UP_SEL) - { - rslt = set_wrist_wear_wake_up(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_WRIST_WEAR_WAKE_UP_SEL; - } - else - { - break; - } - } - - /* Enable wrist gesture feature for wearable variant */ - if (sensor_sel & BMI2_WRIST_GEST_W_SEL) - { - rslt = set_wrist_gesture_wh(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_WRIST_GEST_SEL; - } - else - { - break; - } - } - - /* Enable wrist wear wake-up feature for wearable variant */ - if (sensor_sel & BMI2_WRIST_WEAR_WAKE_UP_WH_SEL) - { - rslt = set_wrist_wear_wake_up_wh(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_WRIST_WEAR_WAKE_UP_SEL; - } - else - { - break; - } - } - - /* Enable activity recognition feature */ - if (sensor_sel & BMI2_ACTIVITY_RECOGNITION_SEL) - { - rslt = set_act_recog(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_ACTIVITY_RECOGNITION_SEL; - } - else - { - break; - } - } - - /* Enable accelerometer self-test feature */ - if (sensor_sel & BMI2_ACCEL_SELF_TEST_SEL) - { - rslt = set_feat_accel_self_test(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_ACCEL_SELF_TEST_SEL; - } - else - { - break; - } - } - - /* Enable low pass filter feature for wearable variant */ - if (sensor_sel & BMI2_PRIMARY_OIS_SEL) - { - rslt = set_primary_ois_low_pass_filter(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_PRIMARY_OIS_SEL; - } - else - { - break; - } - } - - if (sensor_sel & BMI2_FREE_FALL_DET_SEL) - { - rslt = set_free_fall_det(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_FREE_FALL_DET_SEL; - } - else - { - break; - } - } - } - - /* Enable Advance power save if disabled while - * configuring and not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - } - - return rslt; -} - -/*! - * @brief This internal API disables the selected sensors/features. - */ -static int8_t sensor_disable(uint32_t sensor_sel, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store register values */ - uint8_t reg_data = 0; - - /* Variable to define loop */ - uint8_t loop = 1; - - /* Variable to get the status of advance power save */ - uint8_t aps_stat = 0; - - rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); - if (rslt == BMI2_OK) - { - /* Disable accelerometer */ - if (sensor_sel & BMI2_ACCEL_SENS_SEL) - { - reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_ACC_EN); - } - - /* Disable gyroscope */ - if (sensor_sel & BMI2_GYRO_SENS_SEL) - { - reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_GYR_EN); - } - - /* Disable auxiliary sensor */ - if (sensor_sel & BMI2_AUX_SENS_SEL) - { - reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_AUX_EN); - } - - /* Disable temperature sensor */ - if (sensor_sel & BMI2_TEMP_SENS_SEL) - { - reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_TEMP_EN); - } - - /* Disable the sensors that are set in the power control register */ - if (sensor_sel & BMI2_MAIN_SENSORS) - { - rslt = bmi2_set_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); - } - } - if ((rslt == BMI2_OK) && (sensor_sel & ~(BMI2_MAIN_SENSORS))) - { - /* Get status of advance power save mode */ - aps_stat = dev->aps_status; - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if enabled */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - if (rslt == BMI2_OK) - { - while (loop--) - { - /* Disable sig-motion feature */ - if (sensor_sel & BMI2_SIG_MOTION_SEL) - { - rslt = set_sig_motion(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_SIG_MOTION_SEL; - } - else - { - break; - } - } - - /* Disable any-motion feature */ - if (sensor_sel & BMI2_ANY_MOT_SEL) - { - rslt = set_any_motion(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_ANY_MOT_SEL; - } - else - { - break; - } - } - - /* Disable no-motion feature */ - if (sensor_sel & BMI2_NO_MOT_SEL) - { - rslt = set_no_motion(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_NO_MOT_SEL; - } - else - { - break; - } - } - - /* Disable step detector feature */ - if (sensor_sel & BMI2_STEP_DETECT_SEL) - { - rslt = set_step_detector(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_STEP_DETECT_SEL; - } - else - { - break; - } - } - - /* Disable step counter feature */ - if (sensor_sel & BMI2_STEP_COUNT_SEL) - { - rslt = set_step_counter(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_STEP_COUNT_SEL; - } - else - { - break; - } - } - - /* Disable step activity feature */ - if (sensor_sel & BMI2_STEP_ACT_SEL) - { - rslt = set_step_activity(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_STEP_ACT_SEL; - } - else - { - break; - } - } - - /* Disable gyroscope user gain */ - if (sensor_sel & BMI2_GYRO_GAIN_UPDATE_SEL) - { - rslt = set_gyro_user_gain(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_GYRO_GAIN_UPDATE_SEL; - } - else - { - break; - } - } - - /* Disable tilt feature */ - if (sensor_sel & BMI2_TILT_SEL) - { - rslt = set_tilt(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_TILT_SEL; - } - else - { - break; - } - } - - /* Disable uphold to wake feature */ - if (sensor_sel & BMI2_UP_HOLD_TO_WAKE_SEL) - { - rslt = set_up_hold_to_wake(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_UP_HOLD_TO_WAKE_SEL; - } - else - { - break; - } - } - - /* Disable glance feature */ - if (sensor_sel & BMI2_GLANCE_DET_SEL) - { - rslt = set_glance_detector(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_GLANCE_DET_SEL; - } - else - { - break; - } - } - - /* Disable wake-up feature */ - if (sensor_sel & BMI2_WAKE_UP_SEL) - { - rslt = set_wake_up(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_WAKE_UP_SEL; - } - else - { - break; - } - } - - /* Disable orientation feature */ - if (sensor_sel & BMI2_ORIENT_SEL) - { - rslt = set_orientation(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_ORIENT_SEL; - } - else - { - break; - } - } - - /* Disable high-g feature */ - if (sensor_sel & BMI2_HIGH_G_SEL) - { - rslt = set_high_g(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_HIGH_G_SEL; - } - else - { - break; - } - } - - /* Disable low-g feature */ - if (sensor_sel & BMI2_LOW_G_SEL) - { - rslt = set_low_g(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_LOW_G_SEL; - } - else - { - break; - } - } - - /* Disable flat feature */ - if (sensor_sel & BMI2_FLAT_SEL) - { - rslt = set_flat(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_FLAT_SEL; - } - else - { - break; - } - } - - /* Disable external sensor feature */ - if (sensor_sel & BMI2_EXT_SENS_SEL) - { - rslt = set_ext_sens_sync(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_EXT_SENS_SEL; - } - else - { - break; - } - } - - /* Disable gyroscope self-offset correction feature */ - if (sensor_sel & BMI2_GYRO_SELF_OFF_SEL) - { - rslt = set_gyro_self_offset_corr(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_GYRO_SELF_OFF_SEL; - } - else - { - break; - } - } - - /* Disable wrist gesture feature */ - if (sensor_sel & BMI2_WRIST_GEST_SEL) - { - rslt = set_wrist_gesture(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_WRIST_GEST_SEL; - } - else - { - break; - } - } - - /* Disable wrist wear wake-up feature */ - if (sensor_sel & BMI2_WRIST_WEAR_WAKE_UP_SEL) - { - rslt = set_wrist_wear_wake_up(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_WRIST_WEAR_WAKE_UP_SEL; - } - else - { - break; - } - } - - /* Disable wrist gesture feature for wearable variant*/ - if (sensor_sel & BMI2_WRIST_GEST_W_SEL) - { - rslt = set_wrist_gesture_wh(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_WRIST_GEST_W_SEL; - } - else - { - break; - } - } - - /* Disable wrist wear wake-up feature for wearable variant*/ - if (sensor_sel & BMI2_WRIST_WEAR_WAKE_UP_WH_SEL) - { - rslt = set_wrist_wear_wake_up_wh(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_WRIST_WEAR_WAKE_UP_WH_SEL; - } - else - { - break; - } - } - - /* Disable activity recognition feature */ - if (sensor_sel & BMI2_ACTIVITY_RECOGNITION_SEL) - { - rslt = set_act_recog(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_ACTIVITY_RECOGNITION_SEL; - } - else - { - break; - } - } - - /* Disable accelerometer self-test feature */ - if (sensor_sel & BMI2_ACCEL_SELF_TEST_SEL) - { - rslt = set_feat_accel_self_test(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_ACCEL_SELF_TEST_SEL; - } - else - { - break; - } - } - - /* Disable accelerometer free-fall detection feature */ - if (sensor_sel & BMI2_FREE_FALL_DET_SEL) - { - rslt = set_free_fall_det(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_FREE_FALL_DET_SEL; - } - else - { - break; - } - } - - /* Enable Advance power save if disabled while - * configuring and not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - } - } - - return rslt; -} - -/*! - * @brief This internal API selects the sensor/features to be enabled or - * disabled. - */ -static int8_t select_sensor(const uint8_t *sens_list, uint8_t n_sens, uint32_t *sensor_sel) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Variable to define loop */ - uint8_t count; - - for (count = 0; count < n_sens; count++) - { - switch (sens_list[count]) - { - case BMI2_ACCEL: - *sensor_sel |= BMI2_ACCEL_SENS_SEL; - break; - case BMI2_GYRO: - *sensor_sel |= BMI2_GYRO_SENS_SEL; - break; - case BMI2_AUX: - *sensor_sel |= BMI2_AUX_SENS_SEL; - break; - case BMI2_TEMP: - *sensor_sel |= BMI2_TEMP_SENS_SEL; - break; - case BMI2_SIG_MOTION: - *sensor_sel |= BMI2_SIG_MOTION_SEL; - break; - case BMI2_ANY_MOTION: - *sensor_sel |= BMI2_ANY_MOT_SEL; - break; - case BMI2_NO_MOTION: - *sensor_sel |= BMI2_NO_MOT_SEL; - break; - case BMI2_STEP_DETECTOR: - *sensor_sel |= BMI2_STEP_DETECT_SEL; - break; - case BMI2_STEP_COUNTER: - *sensor_sel |= BMI2_STEP_COUNT_SEL; - break; - case BMI2_STEP_ACTIVITY: - *sensor_sel |= BMI2_STEP_ACT_SEL; - break; - case BMI2_GYRO_GAIN_UPDATE: - *sensor_sel |= BMI2_GYRO_GAIN_UPDATE_SEL; - break; - case BMI2_TILT: - *sensor_sel |= BMI2_TILT_SEL; - break; - case BMI2_UP_HOLD_TO_WAKE: - *sensor_sel |= BMI2_UP_HOLD_TO_WAKE_SEL; - break; - case BMI2_GLANCE_DETECTOR: - *sensor_sel |= BMI2_GLANCE_DET_SEL; - break; - case BMI2_WAKE_UP: - *sensor_sel |= BMI2_WAKE_UP_SEL; - break; - case BMI2_ORIENTATION: - *sensor_sel |= BMI2_ORIENT_SEL; - break; - case BMI2_HIGH_G: - *sensor_sel |= BMI2_HIGH_G_SEL; - break; - case BMI2_LOW_G: - *sensor_sel |= BMI2_LOW_G_SEL; - break; - case BMI2_FLAT: - *sensor_sel |= BMI2_FLAT_SEL; - break; - case BMI2_EXT_SENS_SYNC: - *sensor_sel |= BMI2_EXT_SENS_SEL; - break; - case BMI2_GYRO_SELF_OFF: - *sensor_sel |= BMI2_GYRO_SELF_OFF_SEL; - break; - case BMI2_WRIST_GESTURE: - *sensor_sel |= BMI2_WRIST_GEST_SEL; - break; - - case BMI2_WRIST_GESTURE_WH: - *sensor_sel |= BMI2_WRIST_GEST_W_SEL; - break; - case BMI2_WRIST_WEAR_WAKE_UP: - *sensor_sel |= BMI2_WRIST_WEAR_WAKE_UP_SEL; - break; - case BMI2_WRIST_WEAR_WAKE_UP_WH: - *sensor_sel |= BMI2_WRIST_WEAR_WAKE_UP_WH_SEL; - break; - case BMI2_ACTIVITY_RECOGNITION: - *sensor_sel |= BMI2_ACTIVITY_RECOGNITION_SEL; - break; - case BMI2_ACCEL_SELF_TEST: - *sensor_sel |= BMI2_ACCEL_SELF_TEST_SEL; - break; - case BMI2_PRIMARY_OIS: - *sensor_sel |= BMI2_PRIMARY_OIS_SEL; - break; - case BMI2_FREE_FALL_DET: - *sensor_sel |= BMI2_FREE_FALL_DET_SEL; - break; - default: - rslt = BMI2_E_INVALID_SENSOR; - break; - } - } - - return rslt; -} - -/*! - * @brief This internal API is used to enable/disable any motion feature. - */ -static int8_t set_any_motion(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for any-motion */ - struct bmi2_feature_config any_mot_config = { 0, 0, 0 }; - - /* Search for any-motion feature and extract its configurations details */ - feat_found = extract_input_feat_config(&any_mot_config, BMI2_ANY_MOTION, dev); - if (feat_found) - { - /* Get the configuration from the page where any-motion feature resides */ - rslt = get_feat_config(any_mot_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of any-motion axes */ - idx = any_mot_config.start_addr + ANY_MOT_FEAT_EN_OFFSET; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], ANY_NO_MOT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API is used to enable/disable no-motion feature. - */ -static int8_t set_no_motion(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for no-motion */ - struct bmi2_feature_config no_mot_config = { 0, 0, 0 }; - - /* Search for no-motion feature and extract its configurations details */ - feat_found = extract_input_feat_config(&no_mot_config, BMI2_NO_MOTION, dev); - if (feat_found) - { - /* Get the configuration from the page where any/no-motion feature resides */ - rslt = get_feat_config(no_mot_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of no-motion axes */ - idx = no_mot_config.start_addr + NO_MOT_FEAT_EN_OFFSET; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], ANY_NO_MOT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API is used to enable/disable sig-motion feature. - */ -static int8_t set_sig_motion(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for sig-motion */ - struct bmi2_feature_config sig_mot_config = { 0, 0, 0 }; - - /* Search for sig-motion feature and extract its configuration details */ - feat_found = extract_input_feat_config(&sig_mot_config, BMI2_SIG_MOTION, dev); - if (feat_found) - { - /* Get the configuration from the page where sig-motion feature resides */ - rslt = get_feat_config(sig_mot_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of sig-motion */ - idx = sig_mot_config.start_addr + SIG_MOT_FEAT_EN_OFFSET; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], SIG_MOT_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API is used to enable/disable step detector feature. - */ -static int8_t set_step_detector(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for step detector */ - struct bmi2_feature_config step_det_config = { 0, 0, 0 }; - - /* Search for step detector feature and extract its configuration details */ - feat_found = extract_input_feat_config(&step_det_config, BMI2_STEP_DETECTOR, dev); - if (feat_found) - { - /* Get the configuration from the page where step detector feature resides */ - rslt = get_feat_config(step_det_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of step detector */ - idx = step_det_config.start_addr + STEP_COUNT_FEAT_EN_OFFSET; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], STEP_DET_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API is used to enable/disable step counter feature. - */ -static int8_t set_step_counter(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for step counter */ - struct bmi2_feature_config step_count_config = { 0, 0, 0 }; - - /* Search for step counter feature and extract its configuration details */ - feat_found = extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); - if (feat_found) - { - /* Get the configuration from the page where step-counter feature resides */ - rslt = get_feat_config(step_count_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of step counter */ - idx = step_count_config.start_addr + STEP_COUNT_FEAT_EN_OFFSET; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], STEP_COUNT_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API is used to enable/disable step activity detection. - */ -static int8_t set_step_activity(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for step activity */ - struct bmi2_feature_config step_act_config = { 0, 0, 0 }; - - /* Search for step activity feature and extract its configuration details */ - feat_found = extract_input_feat_config(&step_act_config, BMI2_STEP_ACTIVITY, dev); - if (feat_found) - { - /* Get the configuration from the page where step-activity - * feature resides - */ - rslt = get_feat_config(step_act_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of step activity */ - idx = step_act_config.start_addr + STEP_COUNT_FEAT_EN_OFFSET; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], STEP_ACT_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API is used to enable/disable tilt feature. - */ -static int8_t set_tilt(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for tilt */ - struct bmi2_feature_config tilt_config = { 0, 0, 0 }; - - /* Search for tilt feature and extract its configuration details */ - feat_found = extract_input_feat_config(&tilt_config, BMI2_TILT, dev); - if (feat_found) - { - /* Get the configuration from the page where tilt feature resides */ - rslt = get_feat_config(tilt_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of tilt */ - idx = tilt_config.start_addr; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], TILT_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API is used to enable/disable uphold to wake feature. - */ -static int8_t set_up_hold_to_wake(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for uphold to wake */ - struct bmi2_feature_config up_hold_to_wake_config = { 0, 0, 0 }; - - /* Search for uphold to wake feature and extract its configuration details */ - feat_found = extract_input_feat_config(&up_hold_to_wake_config, BMI2_UP_HOLD_TO_WAKE, dev); - if (feat_found) - { - /* Get the configuration from the page where uphold to wake feature resides */ - rslt = get_feat_config(up_hold_to_wake_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of uphold to wake */ - idx = up_hold_to_wake_config.start_addr; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], UP_HOLD_TO_WAKE_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API is used to enable/disable glance detector feature. - */ -static int8_t set_glance_detector(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for glance detector */ - struct bmi2_feature_config glance_det_config = { 0, 0, 0 }; - - /* Search for glance detector feature and extract its configuration details */ - feat_found = extract_input_feat_config(&glance_det_config, BMI2_GLANCE_DETECTOR, dev); - if (feat_found) - { - /* Get the configuration from the page where glance detector - * feature resides - */ - rslt = get_feat_config(glance_det_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of glance */ - idx = glance_det_config.start_addr; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], GLANCE_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API is used to enable/disable wake-up feature through - * single or double tap. - */ -static int8_t set_wake_up(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for wake-up */ - struct bmi2_feature_config wake_up_config = { 0, 0, 0 }; - - /* Search for wake-up feature and extract its configuration details */ - feat_found = extract_input_feat_config(&wake_up_config, BMI2_WAKE_UP, dev); - if (feat_found) - { - /* Get the configuration from the page where wake-up feature resides */ - rslt = get_feat_config(wake_up_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of wake-up */ - idx = wake_up_config.start_addr; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], WAKE_UP_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API is used to enable/disable orientation feature. - */ -static int8_t set_orientation(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for orientation */ - struct bmi2_feature_config orient_config = { 0, 0, 0 }; - - /* Search for orientation feature and extract its configuration details */ - feat_found = extract_input_feat_config(&orient_config, BMI2_ORIENTATION, dev); - if (feat_found) - { - /* Get the configuration from the page where orientation feature resides */ - rslt = get_feat_config(orient_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of orientation */ - idx = orient_config.start_addr; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], ORIENT_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API is used to enable/disable high-g feature. - */ -static int8_t set_high_g(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for high-g */ - struct bmi2_feature_config high_g_config = { 0, 0, 0 }; - - /* Search for high-g feature and extract its configuration details */ - feat_found = extract_input_feat_config(&high_g_config, BMI2_HIGH_G, dev); - if (feat_found) - { - /* Get the configuration from the page where high-g feature resides */ - rslt = get_feat_config(high_g_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of high-g */ - idx = high_g_config.start_addr + HIGH_G_FEAT_EN_OFFSET; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], HIGH_G_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API is used to enable/disable low-g feature. - */ -static int8_t set_low_g(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for low-g */ - struct bmi2_feature_config low_g_config = { 0, 0, 0 }; - - /* Search for low-g feature and extract its configuration details */ - feat_found = extract_input_feat_config(&low_g_config, BMI2_LOW_G, dev); - if (feat_found) - { - /* Get the configuration from the page where low-g feature resides */ - rslt = get_feat_config(low_g_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of low-g */ - idx = low_g_config.start_addr + LOW_G_FEAT_EN_OFFSET; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], LOW_G_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API is used to enable/disable flat feature. - */ -static int8_t set_flat(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for flat */ - struct bmi2_feature_config flat_config = { 0, 0, 0 }; - - /* Search for flat feature and extract its configuration details */ - feat_found = extract_input_feat_config(&flat_config, BMI2_FLAT, dev); - if (feat_found) - { - /* Get the configuration from the page where flat feature resides */ - rslt = get_feat_config(flat_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of flat */ - idx = flat_config.start_addr; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], FLAT_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API is used to enable/disable external sensor sync - * feature. - */ -static int8_t set_ext_sens_sync(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for external sensor sync */ - struct bmi2_feature_config ext_sens_sync_cfg = { 0, 0, 0 }; - - /* Search for sync feature and extract its configuration details */ - feat_found = extract_input_feat_config(&ext_sens_sync_cfg, BMI2_EXT_SENS_SYNC, dev); - if (feat_found) - { - /* Get the configuration from the page where sync feature resides */ - rslt = get_feat_config(ext_sens_sync_cfg.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of sync */ - idx = ext_sens_sync_cfg.start_addr; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], EXT_SENS_SYNC_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gives an option to enable self-offset correction - * feature of gyroscope, either internally or by the host. - */ -static int8_t set_gyro_self_offset_corr(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for self-offset correction */ - struct bmi2_feature_config self_off_corr_cfg = { 0, 0, 0 }; - - /* Search for self-offset correction and extract its configuration details */ - feat_found = extract_input_feat_config(&self_off_corr_cfg, BMI2_GYRO_SELF_OFF, dev); - if (feat_found) - { - /* Get the configuration from the page where self-offset - * correction feature resides - */ - rslt = get_feat_config(self_off_corr_cfg.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of self-offset correction */ - idx = self_off_corr_cfg.start_addr; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], GYR_SELF_OFF_CORR_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API enables the wrist gesture feature. - */ -static int8_t set_wrist_gesture(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for wrist gesture */ - struct bmi2_feature_config wrist_gest_cfg = { 0, 0, 0 }; - - /* Search for wrist gesture and extract its configuration details */ - feat_found = extract_input_feat_config(&wrist_gest_cfg, BMI2_WRIST_GESTURE, dev); - if (feat_found) - { - /* Get the configuration from the page where wrist gesture feature resides */ - rslt = get_feat_config(wrist_gest_cfg.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of wrist gesture */ - idx = wrist_gest_cfg.start_addr; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], WRIST_GEST_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API enables the wrist wear wake up feature. - */ -static int8_t set_wrist_wear_wake_up(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for wrist wear wake up */ - struct bmi2_feature_config wrist_wake_up_cfg = { 0, 0, 0 }; - - /* Search for wrist wear wake up and extract its configuration details */ - feat_found = extract_input_feat_config(&wrist_wake_up_cfg, BMI2_WRIST_WEAR_WAKE_UP, dev); - if (feat_found) - { - /* Get the configuration from the page where wrist wear wake up - * feature resides - */ - rslt = get_feat_config(wrist_wake_up_cfg.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of wrist wear wake up */ - idx = wrist_wake_up_cfg.start_addr; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], WRIST_WEAR_WAKE_UP_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API enables the wrist gesture feature. - */ -static int8_t set_wrist_gesture_wh(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for wrist gesture */ - struct bmi2_feature_config wrist_gest_cfg = { 0, 0, 0 }; - - /* Search for wrist gesture and extract its configuration details */ - feat_found = extract_input_feat_config(&wrist_gest_cfg, BMI2_WRIST_GESTURE_WH, dev); - if (feat_found) - { - /* Get the configuration from the page where wrist gesture feature resides */ - rslt = get_feat_config(wrist_gest_cfg.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of wrist gesture */ - idx = wrist_gest_cfg.start_addr; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], WRIST_GEST_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API enables the wrist wear wake up feature. - */ -static int8_t set_wrist_wear_wake_up_wh(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for wrist wear wake up */ - struct bmi2_feature_config wrist_wake_up_cfg = { 0, 0, 0 }; - - /* Search for wrist wear wake up and extract its configuration details */ - feat_found = extract_input_feat_config(&wrist_wake_up_cfg, BMI2_WRIST_WEAR_WAKE_UP_WH, dev); - if (feat_found) - { - /* Get the configuration from the page where wrist wear wake up - * feature resides - */ - rslt = get_feat_config(wrist_wake_up_cfg.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of wrist wear wake up */ - idx = wrist_wake_up_cfg.start_addr; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], WRIST_WEAR_WAKE_UP_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API enables/disables the activity recognition feature. - */ -static int8_t set_act_recog(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for activity recognition */ - struct bmi2_feature_config act_recog_cfg = { 0, 0, 0 }; - - /* Search for activity recognition and extract its configuration details */ - feat_found = extract_input_feat_config(&act_recog_cfg, BMI2_ACTIVITY_RECOGNITION, dev); - if (feat_found) - { - /* Get the configuration from the page where activity - * recognition feature resides - */ - rslt = get_feat_config(act_recog_cfg.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of activity recognition */ - idx = act_recog_cfg.start_addr; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], ACTIVITY_RECOG_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API is used to enable/disable gyroscope user gain - * feature. - */ -static int8_t set_gyro_user_gain(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for gyroscope user gain */ - struct bmi2_feature_config gyr_user_gain_cfg = { 0, 0, 0 }; - - /* Search for user gain feature and extract its configuration details */ - feat_found = extract_input_feat_config(&gyr_user_gain_cfg, BMI2_GYRO_GAIN_UPDATE, dev); - if (feat_found) - { - /* Get the configuration from the page where user gain feature resides */ - rslt = get_feat_config(gyr_user_gain_cfg.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of user gain */ - idx = gyr_user_gain_cfg.start_addr + GYR_USER_GAIN_FEAT_EN_OFFSET; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], GYR_USER_GAIN_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gives an option to enable accelerometer self-test - * in the feature register. - */ -static int8_t set_feat_accel_self_test(uint8_t enable, struct bmi2_dev *dev) -{ - int8_t rslt = BMI2_OK; - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - uint8_t idx = 0; - uint8_t feat_found; - struct bmi2_feature_config acc_feat_self_test_cfg = { 0, 0, 0 }; - - /* Search for accelerometer self-test and extract its configuration details */ - feat_found = extract_input_feat_config(&acc_feat_self_test_cfg, BMI2_ACCEL_SELF_TEST, dev); - if (feat_found) - { - /* Get the configuration from the page where accelerometer self-test feature resides */ - rslt = get_feat_config(acc_feat_self_test_cfg.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of accelerometer self-test */ - idx = acc_feat_self_test_cfg.start_addr; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], ACC_SELF_TEST_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - - /* Delay added as per the firmware spec */ - dev->delay_us(160000); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gives an option to enable low pass filter - * in the feature register. - */ -static int8_t set_primary_ois_low_pass_filter(uint8_t enable, struct bmi2_dev *dev) -{ - int8_t rslt = BMI2_OK; - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - uint8_t idx = 0; - uint8_t feat_found; - struct bmi2_feature_config primary_ois_lp_filter_cfg = { 0, 0, 0 }; - - /* Search for low pass filter and extract its configuration details */ - feat_found = extract_input_feat_config(&primary_ois_lp_filter_cfg, BMI2_PRIMARY_OIS, dev); - if (feat_found) - { - /* Get the configuration from the page where low pass filter feature resides */ - rslt = get_feat_config(primary_ois_lp_filter_cfg.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of low pass filter */ - idx = primary_ois_lp_filter_cfg.start_addr; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], LP_FILTER_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API sets accelerometer configurations like ODR, - * bandwidth, performance mode and g-range. - */ -static int8_t set_accel_config(struct bmi2_accel_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store data */ - uint8_t reg_data; - - /* Array to store the default value of accelerometer configuration - * reserved registers - */ - uint8_t data_array[2] = { 0 }; - - /* Validate bandwidth and performance mode */ - rslt = validate_bw_perf_mode(&config->bwp, &config->filter_perf, dev); - if (rslt == BMI2_OK) - { - /* Validate ODR and range */ - rslt = validate_odr_range(&config->odr, &config->range, dev); - if (rslt == BMI2_OK) - { - /* Set accelerometer performance mode */ - reg_data = BMI2_SET_BITS(data_array[0], BMI2_ACC_FILTER_PERF_MODE, config->filter_perf); - - /* Set accelerometer bandwidth */ - reg_data = BMI2_SET_BITS(reg_data, BMI2_ACC_BW_PARAM, config->bwp); - - /* Set accelerometer ODR */ - reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_ACC_ODR, config->odr); - - /* Copy the register data to the array */ - data_array[0] = reg_data; - - /* Set accelerometer range */ - reg_data = BMI2_SET_BIT_POS0(data_array[1], BMI2_ACC_RANGE, config->range); - - /* Copy the register data to the array */ - data_array[1] = reg_data; - - /* Write accelerometer configuration to ACC_CONFand - * ACC_RANGE registers simultaneously as they lie in consecutive places - */ - rslt = bmi2_set_regs(BMI2_ACC_CONF_ADDR, data_array, 2, dev); - - /* Get error status to check for invalid configurations */ - if (rslt == BMI2_OK) - { - rslt = cfg_error_status(dev); - } - } - } - - return rslt; -} - /*! * @brief This internal API validates bandwidth and performance mode of the * accelerometer set by the user. @@ -10459,65 +5044,6 @@ static int8_t validate_odr_range(uint8_t *odr, uint8_t *range, struct bmi2_dev * return rslt; } -/*! - * @brief This internal API sets gyroscope configurations like ODR, bandwidth, - * low power/high performance mode, performance mode and range. It also - * maps/un-maps data interrupts to that of hardware interrupt line. - */ -static int8_t set_gyro_config(struct bmi2_gyro_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store data */ - uint8_t reg_data; - - /* Array to store the default value of gyroscope configuration reserved registers */ - uint8_t data_array[2] = { 0 }; - - /* Validate gyroscope configurations */ - rslt = validate_gyro_config(config, dev); - if (rslt == BMI2_OK) - { - /* Set gyroscope performance mode */ - reg_data = BMI2_SET_BITS(data_array[0], BMI2_GYR_FILTER_PERF_MODE, config->filter_perf); - - /* Set gyroscope noise performance mode */ - reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_NOISE_PERF_MODE, config->noise_perf); - - /* Set gyroscope bandwidth */ - reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_BW_PARAM, config->bwp); - - /* Set gyroscope ODR */ - reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_GYR_ODR, config->odr); - - /* Copy the register data to the array */ - data_array[0] = reg_data; - - /* Set gyroscope OIS range */ - reg_data = BMI2_SET_BITS(data_array[1], BMI2_GYR_OIS_RANGE, config->ois_range); - - /* Set gyroscope range */ - reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_GYR_RANGE, config->range); - - /* Copy the register data to the array */ - data_array[1] = reg_data; - - /* Write accelerometer configuration to GYR_CONF and GYR_RANGE - * registers simultaneously as they lie in consecutive places - */ - rslt = bmi2_set_regs(BMI2_GYR_CONF_ADDR, data_array, 2, dev); - - /* Get error status to check for invalid configurations */ - if (rslt == BMI2_OK) - { - rslt = cfg_error_status(dev); - } - } - - return rslt; -} - /*! * @brief This internal API validates bandwidth, performance mode, low power/ * high performance mode, ODR, and range set by the user. @@ -10562,7 +5088,7 @@ static int8_t validate_gyro_config(struct bmi2_gyro_config *config, struct bmi2_ * @brief This internal API shows the error status when illegal sensor * configuration is set. */ -static int8_t cfg_error_status(const struct bmi2_dev *dev) +static int8_t cfg_error_status(struct bmi2_dev *dev) { /* Variable to define error */ int8_t rslt; @@ -10631,6 +5157,87 @@ static int8_t set_aux_config(struct bmi2_aux_config *config, struct bmi2_dev *de return rslt; } +/*! + * @brief This internal API sets gyroscope user-gain configurations like gain + * update value for x, y and z-axis. + */ +static int8_t set_gyro_user_gain_config(const struct bmi2_gyro_user_gain_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define index */ + uint8_t index = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for user-gain */ + struct bmi2_feature_config user_gain_config = { 0, 0, 0 }; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for user-gain feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&user_gain_config, BMI2_GYRO_GAIN_UPDATE, dev); + if (feat_found) + { + /* Get the configuration from the page where user-gain feature resides */ + rslt = bmi2_get_feat_config(user_gain_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for user-gain select */ + idx = user_gain_config.start_addr; + + /* Get offset in words since all the features are set in words length */ + idx = idx / 2; + + /* Set ratio_x */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_GYR_USER_GAIN_RATIO_X, config->ratio_x); + + /* Increment offset by 1 word to set ratio_y */ + idx++; + + /* Set ratio_y */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_GYR_USER_GAIN_RATIO_Y, config->ratio_y); + + /* Increment offset by 1 word to set ratio_z */ + idx++; + + /* Set ratio_z */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_GYR_USER_GAIN_RATIO_Z, config->ratio_z); + + /* Increment offset by 1 more word to get the total length in words */ + idx++; + + /* Get total length in bytes to copy from local pointer to the array */ + idx = (uint8_t)(idx * 2) - user_gain_config.start_addr; + + /* Copy the bytes to be set back to the array */ + for (index = 0; index < idx; index++) + { + feat_config[user_gain_config.start_addr + + index] = *((uint8_t *) data_p + user_gain_config.start_addr + index); + } + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + /*! * @brief This internal API enables/disables auxiliary interface. */ @@ -10700,7 +5307,7 @@ static int8_t config_aux_interface(const struct bmi2_aux_config *config, struct { /* Set the configurations if AUX is not busy */ rslt = bmi2_set_regs(BMI2_AUX_DEV_ID_ADDR, reg_data, 2, dev); - dev->delay_us(1000); + dev->delay_us(1000, dev->intf_ptr); if (rslt == BMI2_OK) { /* If data mode */ @@ -10711,7 +5318,7 @@ static int8_t config_aux_interface(const struct bmi2_aux_config *config, struct /* Set the read address of the AUX sensor */ rslt = bmi2_set_regs(BMI2_AUX_RD_ADDR, (uint8_t *) &config->read_addr, 1, dev); - dev->delay_us(1000); + dev->delay_us(1000, dev->intf_ptr); } else { @@ -10728,7 +5335,7 @@ static int8_t config_aux_interface(const struct bmi2_aux_config *config, struct } /* Increment count after every 10 seconds */ - dev->delay_us(10000); + dev->delay_us(10000, dev->intf_ptr); count++; /* Break after 2 seconds if AUX still busy - since slowest ODR is 0.78Hz*/ @@ -10766,7 +5373,7 @@ static int8_t config_aux(const struct bmi2_aux_config *config, struct bmi2_dev * /* Set auxiliary configuration register */ rslt = bmi2_set_regs(BMI2_AUX_CONF_ADDR, ®_data, 1, dev); - dev->delay_us(1000); + dev->delay_us(1000, dev->intf_ptr); } return rslt; @@ -10796,14 +5403,14 @@ static int8_t set_if_aux_not_busy(uint8_t reg_addr, uint8_t reg_data, struct bmi if ((rslt == BMI2_OK) && (!(status & BMI2_AUX_BUSY))) { rslt = bmi2_set_regs(reg_addr, ®_data, 1, dev); - dev->delay_us(1000); + dev->delay_us(1000, dev->intf_ptr); /* Break after setting the register */ break; } /* Increment count after every 10 seconds */ - dev->delay_us(10000); + dev->delay_us(10000, dev->intf_ptr); count++; /* Break after 2 seconds if AUX still busy - since slowest ODR is 0.78Hz*/ @@ -10831,1711 +5438,11 @@ static int8_t validate_aux_config(struct bmi2_aux_config *config, struct bmi2_de return rslt; } -/*! - * @brief This internal API sets any-motion configurations like axes select, - * duration, threshold and output-configuration. - */ -static int8_t set_any_motion_config(const struct bmi2_any_motion_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define count */ - uint8_t i = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for any motion */ - struct bmi2_feature_config any_mot_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for any-motion feature and extract its configuration details */ - feat_found = extract_input_feat_config(&any_mot_config, BMI2_ANY_MOTION, dev); - if (feat_found) - { - /* Get the configuration from the page where any-motion feature resides */ - rslt = get_feat_config(any_mot_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for any-motion select */ - idx = any_mot_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - /* Set duration */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), ANY_NO_MOT_DUR, config->duration); - - /* Set x-select */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), ANY_NO_MOT_X_SEL, config->select_x); - - /* Set y-select */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), ANY_NO_MOT_Y_SEL, config->select_y); - - /* Set z-select */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), ANY_NO_MOT_Z_SEL, config->select_z); - - /* Increment offset by 1 word to set threshold and output configuration */ - idx++; - - /* Set threshold */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), ANY_NO_MOT_THRES, config->threshold); - - /* Set output configuration */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), ANY_NO_MOT_OUT_CONF, config->out_conf); - - /* Increment offset by 1 more word to get the total length in words */ - idx++; - - /* Get total length in bytes to copy from local pointer to the array */ - idx = (uint8_t)(idx * 2) - any_mot_config.start_addr; - - /* Copy the bytes to be set back to the array */ - for (i = 0; i < idx; i++) - { - feat_config[any_mot_config.start_addr + i] = *((uint8_t *) data_p + any_mot_config.start_addr + i); - } - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - if (rslt == BMI2_OK) - { - /* Copy out_conf value to a local copy in device structure */ - dev->int_map.any_mot_out_conf = (uint8_t) config->out_conf; - } - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API sets no-motion configurations like axes select, - * duration, threshold and output-configuration. - */ -static int8_t set_no_motion_config(const struct bmi2_no_motion_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define count */ - uint8_t i = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for no-motion */ - struct bmi2_feature_config no_mot_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for no-motion feature and extract its configuration details */ - feat_found = extract_input_feat_config(&no_mot_config, BMI2_NO_MOTION, dev); - if (feat_found) - { - /* Get the configuration from the page where no-motion feature resides */ - rslt = get_feat_config(no_mot_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for no-motion select */ - idx = no_mot_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - /* Set duration */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), ANY_NO_MOT_DUR, config->duration); - - /* Set x-select */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), ANY_NO_MOT_X_SEL, config->select_x); - - /* Set y-select */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), ANY_NO_MOT_Y_SEL, config->select_y); - - /* Set z-select */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), ANY_NO_MOT_Z_SEL, config->select_z); - - /* Increment offset by 1 word to set threshold and output configuration */ - idx++; - - /* Set threshold */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), ANY_NO_MOT_THRES, config->threshold); - - /* Set output configuration */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), ANY_NO_MOT_OUT_CONF, config->out_conf); - - /* Increment offset by 1 more word to get the total length in words */ - idx++; - - /* Get total length in bytes to copy from local pointer to the array */ - idx = (uint8_t)(idx * 2) - no_mot_config.start_addr; - - /* Copy the bytes to be set back to the array */ - for (i = 0; i < idx; i++) - { - feat_config[no_mot_config.start_addr + i] = *((uint8_t *) data_p + no_mot_config.start_addr + i); - } - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - if (rslt == BMI2_OK) - { - /* Copy out_conf value to a local copy in device structure */ - dev->int_map.no_mot_out_conf = (uint8_t) config->out_conf; - } - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API sets sig-motion configurations like block-size, - * output-configuration and other parameters. - */ -static int8_t set_sig_motion_config(const struct bmi2_sig_motion_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define index */ - uint8_t i = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for sig-motion */ - struct bmi2_feature_config sig_mot_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for sig-motion feature and extract its configuration details */ - feat_found = extract_input_feat_config(&sig_mot_config, BMI2_SIG_MOTION, dev); - if (feat_found) - { - /* Get the configuration from the page where sig-motion feature resides */ - rslt = get_feat_config(sig_mot_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for sig-motion select */ - idx = sig_mot_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - /* Set parameter 1 */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), SIG_MOT_PARAM_1, config->block_size); - - /* Increment offset by 1 word to set parameter 2 */ - idx++; - - /* Set parameter 2 */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), SIG_MOT_PARAM_2, config->param_2); - - /* Increment offset by 1 word to set parameter 3 */ - idx++; - - /* Set parameter 3 */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), SIG_MOT_PARAM_3, config->param_3); - - /* Increment offset by 1 word to set parameter 4 */ - idx++; - - /* Set parameter 4 */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), SIG_MOT_PARAM_4, config->param_4); - - /* Increment offset by 1 word to set parameter 5 */ - idx++; - - /* Set parameter 5 */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), SIG_MOT_PARAM_5, config->param_5); - - /* Increment offset by 1 word to set output- configuration */ - idx++; - - /* Set output configuration */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), SIG_MOT_OUT_CONF, config->out_conf); - - /* Increment offset by 1 more word to get the total length in words */ - idx++; - - /* Get total length in bytes to copy from local pointer to the array */ - idx = (uint8_t)(idx * 2) - sig_mot_config.start_addr; - - /* Copy the bytes to be set back to the array */ - for (i = 0; i < idx; i++) - { - feat_config[sig_mot_config.start_addr + i] = *((uint8_t *) data_p + sig_mot_config.start_addr + i); - } - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - if (rslt == BMI2_OK) - { - /* Copy out_conf value to a local copy in device structure for mapping */ - dev->int_map.sig_mot_out_conf = (uint8_t) config->out_conf; - } - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API sets step counter parameter configurations. - */ -static int8_t set_step_count_params_config(const uint16_t *step_count_params, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define index */ - uint8_t i = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for step counter parameters */ - struct bmi2_feature_config step_params_config = { 0, 0, 0 }; - - /* Variable to index the page number */ - uint8_t page_idx; - - /* Variable to define the start page */ - uint8_t start_page; - - /* Variable to define start address of the parameters */ - uint8_t start_addr; - - /* Variable to define number of bytes */ - uint8_t n_bytes = (BMI2_STEP_CNT_N_PARAMS * 2); - - /* Variable to store number of pages */ - uint8_t n_pages = (n_bytes / 16); - - /* Variable to define the end page */ - uint8_t end_page; - - /* Variable to define the remaining bytes to be read */ - uint8_t remain_len; - - /* Variable to define the maximum words(16 bytes or 8 words) to be read in a page */ - uint8_t max_len = 8; - - /* Variable index bytes in a page */ - uint8_t page_byte_idx; - - /* Variable to index the parameters */ - uint8_t param_idx = 0; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for step counter parameter feature and extract its configuration details */ - feat_found = extract_input_feat_config(&step_params_config, BMI2_STEP_COUNTER_PARAMS, dev); - if (feat_found) - { - /* Get the start page for the step counter parameters */ - start_page = step_params_config.page; - - /* Get the end page for the step counter parameters */ - end_page = start_page + n_pages; - - /* Get the start address for the step counter parameters */ - start_addr = step_params_config.start_addr; - - /* Get the remaining length of bytes to be read */ - remain_len = (uint8_t)((n_bytes - (n_pages * 16)) + start_addr); - for (page_idx = start_page; page_idx <= end_page; page_idx++) - { - /* Get the configuration from the respective page */ - rslt = get_feat_config(page_idx, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Start from address 0x00 when switched to next page */ - if (page_idx > start_page) - { - start_addr = 0; - } - - /* Remaining number of words to be read in the page */ - if (page_idx == end_page) - { - max_len = (remain_len / 2); - } - - /* Get offset in words since all the features are set in words length */ - page_byte_idx = start_addr / 2; - for (; page_byte_idx < max_len;) - { - /* Set parameters 1 to 25 */ - *(data_p + page_byte_idx) = BMI2_SET_BIT_POS0(*(data_p + page_byte_idx), - STEP_COUNT_PARAMS, - step_count_params[param_idx]); - - /* Increment offset by 1 word to set to the next parameter */ - page_byte_idx++; - - /* Increment to next parameter */ - param_idx++; - } - - /* Get total length in bytes to copy from local pointer to the array */ - page_byte_idx = (uint8_t)(page_byte_idx * 2) - step_params_config.start_addr; - - /* Copy the bytes to be set back to the array */ - for (i = 0; i < page_byte_idx; i++) - { - feat_config[step_params_config.start_addr + - i] = *((uint8_t *) data_p + step_params_config.start_addr + i); - } - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API sets step counter configurations like water-mark - * level, reset-counter and output-configuration step detector and activity. - */ -static int8_t set_step_config(const struct bmi2_step_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define index */ - uint8_t i = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for step counter 4 */ - struct bmi2_feature_config step_count_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for step counter feature and extract its configuration details */ - feat_found = extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); - if (feat_found) - { - /* Get the configuration from the page where step counter resides */ - rslt = get_feat_config(step_count_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes */ - idx = step_count_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - /* Set water-mark level */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), STEP_COUNT_WM_LEVEL, config->watermark_level); - - /* Set reset-counter */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), STEP_COUNT_RST_CNT, config->reset_counter); - - /* Increment offset by 1 word to set output - * configuration of step detector and step activity - */ - idx++; - - /* Set output configuration of step-detector */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), STEP_DET_OUT_CONF, config->out_conf_step_detector); - - /* Set output configuration of step-activity */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), STEP_ACT_OUT_CONF, config->out_conf_activity); - - /* Set step buffer size */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), STEP_BUFFER_SIZE, config->step_buffer_size); - - /* Increment offset by 1 more word to get the total length in words */ - idx++; - - /* Get total length in bytes to copy from local pointer to the array */ - idx = (uint8_t)(idx * 2) - step_count_config.start_addr; - - /* Copy the bytes to be set back to the array */ - for (i = 0; i < idx; i++) - { - feat_config[step_count_config.start_addr + - i] = *((uint8_t *) data_p + step_count_config.start_addr + i); - } - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - if (rslt == BMI2_OK) - { - /* Copy out_conf value to a local copy in device structure for step-detector */ - dev->int_map.step_det_out_conf = (uint8_t) config->out_conf_step_detector; - - /* Copy out_conf value to a local copy in device structure for step-activity */ - dev->int_map.step_act_out_conf = (uint8_t) config->out_conf_activity; - } - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API sets gyroscope user-gain configurations like gain - * update value for x, y and z-axis. - */ -static int8_t set_gyro_user_gain_config(const struct bmi2_gyro_user_gain_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define index */ - uint8_t i = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for user-gain */ - struct bmi2_feature_config user_gain_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for user-gain feature and extract its configuration details */ - feat_found = extract_input_feat_config(&user_gain_config, BMI2_GYRO_GAIN_UPDATE, dev); - if (feat_found) - { - /* Get the configuration from the page where user-gain feature resides */ - rslt = get_feat_config(user_gain_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for user-gain select */ - idx = user_gain_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - /* Set ratio_x */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), GYR_USER_GAIN_RATIO_X, config->ratio_x); - - /* Increment offset by 1 word to set ratio_y */ - idx++; - - /* Set ratio_y */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), GYR_USER_GAIN_RATIO_Y, config->ratio_y); - - /* Increment offset by 1 word to set ratio_z */ - idx++; - - /* Set ratio_z */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), GYR_USER_GAIN_RATIO_Z, config->ratio_z); - - /* Increment offset by 1 more word to get the total length in words */ - idx++; - - /* Get total length in bytes to copy from local pointer to the array */ - idx = (uint8_t)(idx * 2) - user_gain_config.start_addr; - - /* Copy the bytes to be set back to the array */ - for (i = 0; i < idx; i++) - { - feat_config[user_gain_config.start_addr + i] = *((uint8_t *) data_p + user_gain_config.start_addr + i); - } - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API sets tilt configurations like output-configuration. - */ -static int8_t set_tilt_config(const struct bmi2_tilt_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define index */ - uint8_t i = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for tilt */ - struct bmi2_feature_config tilt_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for tilt feature and extract its configuration details */ - feat_found = extract_input_feat_config(&tilt_config, BMI2_TILT, dev); - if (feat_found) - { - /* Get the configuration from the page where tilt feature resides */ - rslt = get_feat_config(tilt_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for tilt select */ - idx = tilt_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - /* Set output configuration */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), TILT_OUT_CONF, config->out_conf); - - /* Increment offset by 1 more word to get the total length in words */ - idx++; - - /* Get total length in bytes to copy from local pointer to the array */ - idx = (uint8_t)(idx * 2) - tilt_config.start_addr; - - /* Copy the bytes to be set back to the array */ - for (i = 0; i < idx; i++) - { - feat_config[tilt_config.start_addr + i] = *((uint8_t *) data_p + tilt_config.start_addr + i); - } - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - if (rslt == BMI2_OK) - { - /* Copy out_conf value to a local copy in device structure */ - dev->int_map.tilt_out_conf = (uint8_t) config->out_conf; - } - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API sets uphold to wake config configurations like output - * configuration. - */ -static int8_t set_up_hold_to_wake_config(const struct bmi2_up_hold_to_wake_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define index */ - uint8_t i = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for uphold to wake */ - struct bmi2_feature_config up_hold_to_wake_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for uphold to wake feature and extract its configuration details */ - feat_found = extract_input_feat_config(&up_hold_to_wake_config, BMI2_UP_HOLD_TO_WAKE, dev); - if (feat_found) - { - /* Get the configuration from the page where uphold to wake feature resides */ - rslt = get_feat_config(up_hold_to_wake_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for uphold to wake select */ - idx = up_hold_to_wake_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - /* Set output configuration */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), UP_HOLD_TO_WAKE_OUT_CONF, config->out_conf); - - /* Increment offset by 1 more word to get the total length in words */ - idx++; - - /* Get total length in bytes to copy from local pointer to the array */ - idx = (uint8_t)(idx * 2) - up_hold_to_wake_config.start_addr; - - /* Copy the bytes to be set back to the array */ - for (i = 0; i < idx; i++) - { - feat_config[up_hold_to_wake_config.start_addr + - i] = *((uint8_t *) data_p + up_hold_to_wake_config.start_addr + i); - } - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - if (rslt == BMI2_OK) - { - /* Copy out_conf value to a local copy in device structure */ - dev->int_map.up_hold_to_wake_out_conf = (uint8_t) config->out_conf; - } - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API sets glance detector configurations like output - * configuration. - */ -static int8_t set_glance_detect_config(const struct bmi2_glance_det_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define index */ - uint8_t i = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for glance detector */ - struct bmi2_feature_config glance_det_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for glance detector feature and extract its configuration details */ - feat_found = extract_input_feat_config(&glance_det_config, BMI2_GLANCE_DETECTOR, dev); - if (feat_found) - { - /* Get the configuration from the page where glance detector feature resides */ - rslt = get_feat_config(glance_det_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for glance detector select */ - idx = glance_det_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - /* Set output configuration */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), GLANCE_DET_OUT_CONF, config->out_conf); - - /* Increment offset by 1 more word to get the total length in words */ - idx++; - - /* Get total length in bytes to copy from local pointer to the array */ - idx = (uint8_t)(idx * 2) - glance_det_config.start_addr; - - /* Copy the bytes to be set back to the array */ - for (i = 0; i < idx; i++) - { - feat_config[glance_det_config.start_addr + - i] = *((uint8_t *) data_p + glance_det_config.start_addr + i); - } - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - if (rslt == BMI2_OK) - { - /* Copy out_conf value to a local copy in device structure */ - dev->int_map.glance_out_conf = (uint8_t) config->out_conf; - } - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API sets wake-up configurations like sensitivity, - * single/double tap enable and output-configuration. - */ -static int8_t set_wake_up_config(const struct bmi2_wake_up_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define index */ - uint8_t i = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for wake-up */ - struct bmi2_feature_config wake_up_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for wake-up feature and extract its configuration details */ - feat_found = extract_input_feat_config(&wake_up_config, BMI2_WAKE_UP, dev); - if (feat_found) - { - /* Get the configuration from the page where wake-up feature resides */ - rslt = get_feat_config(wake_up_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for wake-up select */ - idx = wake_up_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - /* Set sensitivity */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), WAKE_UP_SENSITIVITY, config->sensitivity); - - /* Set single/double tap enable */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), WAKE_UP_SINGLE_TAP_EN, config->single_tap_en); - - /* Set output configuration */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), WAKE_UP_OUT_CONF, config->out_conf); - - /* Increment offset by 1 more word to get the total length in words */ - idx++; - - /* Get total length in bytes to copy from local pointer to the array */ - idx = (uint8_t)(idx * 2) - wake_up_config.start_addr; - - /* Copy the bytes to be set back to the array */ - for (i = 0; i < idx; i++) - { - feat_config[wake_up_config.start_addr + i] = *((uint8_t *) data_p + wake_up_config.start_addr + i); - } - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - if (rslt == BMI2_OK) - { - /* Copy out_conf value to a local copy in device structure */ - dev->int_map.wake_up_out_conf = (uint8_t) config->out_conf; - } - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API sets orientation configurations like upside/down - * detection, symmetrical modes, blocking mode, theta, hysteresis and output - * configuration. - */ -static int8_t set_orient_config(const struct bmi2_orient_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define index */ - uint8_t i = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for orient */ - struct bmi2_feature_config orient_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for orient feature and extract its configuration details */ - feat_found = extract_input_feat_config(&orient_config, BMI2_ORIENTATION, dev); - if (feat_found) - { - /* Get the configuration from the page where orient feature resides */ - rslt = get_feat_config(orient_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for orient select */ - idx = orient_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - /* Set upside/down detection */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), ORIENT_UP_DOWN, config->ud_en); - - /* Set symmetrical modes */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), ORIENT_SYMM_MODE, config->mode); - - /* Set blocking mode */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), ORIENT_BLOCK_MODE, config->blocking); - - /* Set theta */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), ORIENT_THETA, config->theta); - - /* Increment offset by 1 more word to set hysteresis and output configuration */ - idx++; - - /* Set hysteresis */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), ORIENT_HYST, config->hysteresis); - - /* Set output configuration */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), ORIENT_OUT_CONF, config->out_conf); - - /* Increment offset by 1 more word to get the total length in words */ - idx++; - - /* Get total length in bytes to copy from local pointer to the array */ - idx = (uint8_t)(idx * 2) - orient_config.start_addr; - - /* Copy the bytes to be set back to the array */ - for (i = 0; i < idx; i++) - { - feat_config[orient_config.start_addr + i] = *((uint8_t *) data_p + orient_config.start_addr + i); - } - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - if (rslt == BMI2_OK) - { - /* Copy out_conf value to a local copy in device structure */ - dev->int_map.orient_out_conf = (uint8_t) config->out_conf; - } - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API sets high-g configurations like threshold, - * hysteresis, duration, and out0put configuration. - */ -static int8_t set_high_g_config(const struct bmi2_high_g_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define index */ - uint8_t i = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for high-g */ - struct bmi2_feature_config high_g_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for high-g feature and extract its configuration details */ - feat_found = extract_input_feat_config(&high_g_config, BMI2_HIGH_G, dev); - if (feat_found) - { - /* Get the configuration from the page where high-g feature resides */ - rslt = get_feat_config(high_g_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for high-g select */ - idx = high_g_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - /* Set threshold */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), HIGH_G_THRES, config->threshold); - - /* Increment offset by 1 more word to set hysteresis */ - idx++; - - /* Set hysteresis */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), HIGH_G_HYST, config->hysteresis); - - /* Set x-select */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), HIGH_G_X_SEL, config->select_x); - - /* Set y-select */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), HIGH_G_Y_SEL, config->select_y); - - /* Set z-select */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), HIGH_G_Z_SEL, config->select_z); - - /* Increment offset by 1 more word to set duration and output configuration */ - idx++; - - /* Set duration */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), HIGH_G_DUR, config->duration); - - /* Set output configuration */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), HIGH_G_OUT_CONF, config->out_conf); - - /* Increment offset by 1 more word to get the total length in words */ - idx++; - - /* Get total length in bytes to copy from local pointer to the array */ - idx = (uint8_t)(idx * 2) - high_g_config.start_addr; - - /* Copy the bytes to be set back to the array */ - for (i = 0; i < idx; i++) - { - feat_config[high_g_config.start_addr + i] = *((uint8_t *) data_p + high_g_config.start_addr + i); - } - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - if (rslt == BMI2_OK) - { - /* Copy out_conf value to a local copy in device structure */ - dev->int_map.high_g_out_conf = (uint8_t) config->out_conf; - } - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API sets low-g configurations like threshold, - * hysteresis, duration, and output configuration. - */ -static int8_t set_low_g_config(const struct bmi2_low_g_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define index */ - uint8_t i = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for low-g */ - struct bmi2_feature_config low_g_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for low-g feature and extract its configuration details */ - feat_found = extract_input_feat_config(&low_g_config, BMI2_LOW_G, dev); - if (feat_found) - { - /* Get the configuration from the page where low-g feature resides */ - rslt = get_feat_config(low_g_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for low-g select */ - idx = low_g_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - /* Set threshold */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), LOW_G_THRES, config->threshold); - - /* Increment offset by 1 more word to set hysteresis */ - idx++; - - /* Set hysteresis */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), LOW_G_HYST, config->hysteresis); - - /* Increment offset by 1 more word to set duration and output configuration */ - idx++; - - /* Set duration */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), LOW_G_DUR, config->duration); - - /* Set output configuration */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), LOW_G_OUT_CONF, config->out_conf); - - /* Increment offset by 1 more word to get the total length in words */ - idx++; - - /* Get total length in bytes to copy from local pointer to the array */ - idx = (uint8_t)(idx * 2) - low_g_config.start_addr; - - /* Copy the bytes to be set back to the array */ - for (i = 0; i < idx; i++) - { - feat_config[low_g_config.start_addr + i] = *((uint8_t *) data_p + low_g_config.start_addr + i); - } - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - if (rslt == BMI2_OK) - { - /* Copy out_conf value to a local copy in device structure */ - dev->int_map.low_g_out_conf = (uint8_t) config->out_conf; - } - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API sets flat configurations like theta, blocking, - * hold-time, hysteresis, and output configuration. - */ -static int8_t set_flat_config(const struct bmi2_flat_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define index */ - uint8_t i = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for flat */ - struct bmi2_feature_config flat_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for flat feature and extract its configuration details */ - feat_found = extract_input_feat_config(&flat_config, BMI2_FLAT, dev); - if (feat_found) - { - /* Get the configuration from the page where flat feature resides */ - rslt = get_feat_config(flat_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for flat select */ - idx = flat_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - /* Set theta */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), FLAT_THETA, config->theta); - - /* Set blocking */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), FLAT_BLOCK, config->blocking); - - /* Set output configuration */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), FLAT_OUT_CONF, config->out_conf); - - /* Increment offset by 1 more word to set hysteresis and hold-time */ - idx++; - - /* Set hysteresis */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), FLAT_HYST, config->hysteresis); - - /* Set hold-time */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), FLAT_HOLD_TIME, config->hold_time); - - /* Increment offset by 1 more word to get the total length in words */ - idx++; - - /* Get total length in bytes to copy from local pointer to the array */ - idx = (uint8_t)(idx * 2) - flat_config.start_addr; - - /* Copy the bytes to be set back to the array */ - for (i = 0; i < idx; i++) - { - feat_config[flat_config.start_addr + i] = *((uint8_t *) data_p + flat_config.start_addr + i); - } - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - if (rslt == BMI2_OK) - { - /* Copy out_conf value to a local copy in device structure */ - dev->int_map.flat_out_conf = (uint8_t) config->out_conf; - } - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API sets external sensor sync configurations like output - * configuration. - */ -static int8_t set_ext_sens_sync_config(const struct bmi2_ext_sens_sync_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define index */ - uint8_t i = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for external sensor sync */ - struct bmi2_feature_config ext_sens_sync_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for external sensor sync feature and extract its configuration details */ - feat_found = extract_input_feat_config(&ext_sens_sync_config, BMI2_EXT_SENS_SYNC, dev); - if (feat_found) - { - /* Get the configuration from the page where external sensor sync feature resides */ - rslt = get_feat_config(ext_sens_sync_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for external sensor sync select */ - idx = ext_sens_sync_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - /* Set output configuration */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), EXT_SENS_SYNC_OUT_CONF, config->out_conf); - - /* Increment offset by 1 more word to get the total length in words */ - idx++; - - /* Get total length in bytes to copy from local pointer to the array */ - idx = (uint8_t)(idx * 2) - ext_sens_sync_config.start_addr; - - /* Copy the bytes to be set back to the array */ - for (i = 0; i < idx; i++) - { - feat_config[ext_sens_sync_config.start_addr + - i] = *((uint8_t *) data_p + ext_sens_sync_config.start_addr + i); - } - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - if (rslt == BMI2_OK) - { - /* Copy out_conf value to a local copy in device structure */ - dev->int_map.ext_sync_out_conf = (uint8_t) config->out_conf; - } - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API sets wrist gesture configurations like wearable-arm, - * and output-configuration. - */ -static int8_t set_wrist_gest_config(const struct bmi2_wrist_gest_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define index */ - uint8_t i = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for wrist gesture */ - struct bmi2_feature_config wrist_gest_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for wrist gesture feature and extract its configuration details */ - feat_found = extract_input_feat_config(&wrist_gest_config, BMI2_WRIST_GESTURE, dev); - if (feat_found) - { - /* Get the configuration from the page where wrist gesture feature resides */ - rslt = get_feat_config(wrist_gest_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for gesture select */ - idx = wrist_gest_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - /* Set wearable arm */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), WRIST_GEST_WEAR_ARM, config->wearable_arm); - - /* Set output configuration */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), WRIST_GEST_OUT_CONF, config->out_conf); - - /* Increment offset by 1 more word to set minimum tilt angle (min_flick_peak) */ - idx++; - *(data_p + idx) = config->min_flick_peak; - - /* Increment offset by 1 more word to set min_flick_samples */ - idx++; - *(data_p + idx) = config->min_flick_samples; - - /* Increment offset by 1 more word to set max time within gesture moment has to be completed */ - idx++; - *(data_p + idx) = config->max_duration; - - /* Increment offset by 1 more word to get the total length in words */ - idx++; - - /* Get total length in bytes to copy from local pointer to the array */ - idx = (uint8_t)(idx * 2) - wrist_gest_config.start_addr; - - /* Copy the bytes to be set back to the array */ - for (i = 0; i < idx; i++) - { - feat_config[wrist_gest_config.start_addr + - i] = *((uint8_t *) data_p + wrist_gest_config.start_addr + i); - } - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - if (rslt == BMI2_OK) - { - /* Copy out_conf value to a local copy in device structure */ - dev->int_map.wrist_gest_out_conf = (uint8_t) config->out_conf; - } - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API sets wrist wear wake-up configurations like - * output-configuration. - */ -static int8_t set_wrist_wear_wake_up_config(const struct bmi2_wrist_wear_wake_up_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define index */ - uint8_t i = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for wrist wear wake-up */ - struct bmi2_feature_config wrist_wake_up_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for wrist wear wake-up feature and extract its configuration details */ - feat_found = extract_input_feat_config(&wrist_wake_up_config, BMI2_WRIST_WEAR_WAKE_UP, dev); - if (feat_found) - { - /* Get the configuration from the page where wrist wear wake-up feature resides */ - rslt = get_feat_config(wrist_wake_up_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for wrist wear wake-up select */ - idx = wrist_wake_up_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - /* Set output configuration */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), WRIST_WAKE_UP_OUT_CONF, config->out_conf); - - /* Increment offset by 1 more word to set min_angle_focus */ - idx++; - *(data_p + idx) = config->min_angle_focus; - - /* Increment offset by 1 more word to set min_angle_nonfocus */ - idx++; - *(data_p + idx) = config->min_angle_nonfocus; - - /* Increment offset by 1 more word to set max_tilt_lr */ - idx++; - *(data_p + idx) = config->max_tilt_lr; - - /* Increment offset by 1 more word to set max_tilt_ll */ - idx++; - *(data_p + idx) = config->max_tilt_ll; - - /* Increment offset by 1 more word to set max_tilt_pd */ - idx++; - *(data_p + idx) = config->max_tilt_pd; - - /* Increment offset by 1 more word to set max_tilt_pu */ - idx++; - *(data_p + idx) = config->max_tilt_pu; - - /* Increment offset by 1 more word to get the total length in words */ - idx++; - - /* Get total length in bytes to copy from local pointer to the array */ - idx = (uint8_t)(idx * 2) - wrist_wake_up_config.start_addr; - - /* Copy the bytes to be set back to the array */ - for (i = 0; i < idx; i++) - { - feat_config[wrist_wake_up_config.start_addr + - i] = *((uint8_t *) data_p + wrist_wake_up_config.start_addr + i); - } - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - if (rslt == BMI2_OK) - { - /* Copy out_conf value to a local copy in device structure */ - dev->int_map.wrist_wear_wake_up_out_conf = (uint8_t) config->out_conf; - } - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API sets wrist gesture configurations like wearable-arm, - * and output-configuration for wearable variant. - */ -static int8_t set_wrist_gest_w_config(const struct bmi2_wrist_gest_w_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define index */ - uint8_t i = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for wrist gesture */ - struct bmi2_feature_config wrist_gest_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for wrist gesture feature and extract its configuration details */ - feat_found = extract_input_feat_config(&wrist_gest_config, BMI2_WRIST_GESTURE_WH, dev); - if (feat_found) - { - /* Get the configuration from the page where wrist gesture feature resides */ - rslt = get_feat_config(wrist_gest_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for gesture select */ - idx = wrist_gest_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - /* Set wearable arm */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), WRIST_GEST_WEAR_ARM, config->wearable_arm); - - /* Set output configuration */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), WRIST_GEST_OUT_CONF, config->out_conf); - - /* Increment offset by 1 more word to set minimum tilt angle (min_flick_peak) */ - idx++; - *(data_p + idx) = config->min_flick_peak; - - /* Increment offset by 1 more word to set min_flick_samples */ - idx++; - *(data_p + idx) = config->min_flick_samples; - - /* Increment offset by 1 more word to set max time within gesture moment has to be completed */ - idx++; - *(data_p + idx) = config->max_duration; - - /* Increment offset by 1 more word to set reporting delay */ - idx++; - *(data_p + idx) = config->reporting_delay; - - /* Increment offset by 1 more word to get the total length in words */ - idx++; - - /* Get total length in bytes to copy from local pointer to the array */ - idx = (uint8_t)(idx * 2) - wrist_gest_config.start_addr; - - /* Copy the bytes to be set back to the array */ - for (i = 0; i < idx; i++) - { - feat_config[wrist_gest_config.start_addr + - i] = *((uint8_t *) data_p + wrist_gest_config.start_addr + i); - } - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - if (rslt == BMI2_OK) - { - /* Copy out_conf value to a local copy in device structure */ - dev->int_map.wrist_gest_out_conf = (uint8_t) config->out_conf; - } - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API sets wrist wear wake-up configurations like - * output-configuration for wearable variant. - */ -static int8_t set_wrist_wear_wake_up_wh_config(const struct bmi2_wrist_wear_wake_up_wh_config *config, - struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define index */ - uint8_t i = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for wrist wear wake-up */ - struct bmi2_feature_config wrist_wake_up_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for wrist wear wake-up feature and extract its configuration details */ - feat_found = extract_input_feat_config(&wrist_wake_up_config, BMI2_WRIST_WEAR_WAKE_UP_WH, dev); - if (feat_found) - { - /* Get the configuration from the page where wrist wear wake-up feature resides */ - rslt = get_feat_config(wrist_wake_up_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for wrist wear wake-up select */ - idx = wrist_wake_up_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - /* Set output configuration */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), WRIST_WAKE_UP_OUT_CONF, config->out_conf); - - /* Increment offset by 1 more word to set min_angle_focus */ - idx++; - *(data_p + idx) = config->min_angle_focus; - - /* Increment offset by 1 more word to set min_angle_nonfocus */ - idx++; - *(data_p + idx) = config->min_angle_nonfocus; - - /* Increment offset by 1 more word to set angle landscape right and angle landscape left */ - idx++; - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), WRIST_WAKE_UP_ANGLE_LR, config->angle_lr); - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), WRIST_WAKE_UP_ANGLE_LL, config->angle_ll); - - /* Increment offset by 1 more word to set angle portrait down and angle portrait left */ - idx++; - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), WRIST_WAKE_UP_ANGLE_PD, config->angle_pd); - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), WRIST_WAKE_UP_ANGLE_PU, config->angle_pu); - - /* Increment offset by 1 more word to set min duration moved and min duration quite */ - idx++; - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), WRIST_WAKE_UP_MIN_DUR_MOVED, config->min_dur_mov); - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), WRIST_WAKE_UP_MIN_DUR_QUITE, config->min_dur_quite); - - /* Increment offset by 1 more word to get the total length in words */ - idx++; - - /* Get total length in bytes to copy from local pointer to the array */ - idx = (uint8_t)(idx * 2) - wrist_wake_up_config.start_addr; - - /* Copy the bytes to be set back to the array */ - for (i = 0; i < idx; i++) - { - feat_config[wrist_wake_up_config.start_addr + - i] = *((uint8_t *) data_p + wrist_wake_up_config.start_addr + i); - } - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - if (rslt == BMI2_OK) - { - /* Copy out_conf value to a local copy in device structure */ - dev->int_map.wrist_wear_wake_up_out_conf = (uint8_t) config->out_conf; - } - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - /*! * @brief This internal API gets accelerometer configurations like ODR, * bandwidth, performance mode and g-range. */ -static int8_t get_accel_config(struct bmi2_accel_config *config, const struct bmi2_dev *dev) +static int8_t get_accel_config(struct bmi2_accel_config *config, struct bmi2_dev *dev) { /* Variable to define error */ int8_t rslt; @@ -12576,7 +5483,7 @@ static int8_t get_accel_config(struct bmi2_accel_config *config, const struct bm * @brief This internal API gets gyroscope configurations like ODR, bandwidth, * low power/high performance mode, performance mode and range. */ -static int8_t get_gyro_config(struct bmi2_gyro_config *config, const struct bmi2_dev *dev) +static int8_t get_gyro_config(struct bmi2_gyro_config *config, struct bmi2_dev *dev) { /* Variable to define error */ int8_t rslt; @@ -12627,7 +5534,7 @@ static int8_t get_gyro_config(struct bmi2_gyro_config *config, const struct bmi2 * address. * 3) Gets ODR and offset. */ -static int8_t get_aux_config(struct bmi2_aux_config *config, const struct bmi2_dev *dev) +static int8_t get_aux_config(struct bmi2_aux_config *config, struct bmi2_dev *dev) { /* Variable to define error */ int8_t rslt; @@ -12657,10 +5564,84 @@ static int8_t get_aux_config(struct bmi2_aux_config *config, const struct bmi2_d return rslt; } +/*! + * @brief This internal API gets gyroscope user-gain configurations like gain + * update value for x, y and z-axis. + */ +static int8_t get_gyro_gain_update_config(struct bmi2_gyro_user_gain_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define LSB */ + uint16_t lsb = 0; + + /* Variable to define MSB */ + uint16_t msb = 0; + + /* Variable to define a word */ + uint16_t lsb_msb = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for user-gain */ + struct bmi2_feature_config user_gain_config = { 0, 0, 0 }; + + /* Search for user-gain feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&user_gain_config, BMI2_GYRO_GAIN_UPDATE, dev); + if (feat_found) + { + /* Get the configuration from the page where user-gain feature resides */ + rslt = bmi2_get_feat_config(user_gain_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for user-gain select */ + idx = user_gain_config.start_addr; + + /* Get word to calculate ratio_x */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get ratio_x */ + config->ratio_x = lsb_msb & BMI2_GYR_USER_GAIN_RATIO_X_MASK; + + /* Get word to calculate ratio_y */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get ratio_y */ + config->ratio_y = lsb_msb & BMI2_GYR_USER_GAIN_RATIO_Y_MASK; + + /* Get word to calculate ratio_z */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get ratio_z */ + config->ratio_z = lsb_msb & BMI2_GYR_USER_GAIN_RATIO_Z_MASK; + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + /*! * @brief This internal API gets the enable status of auxiliary interface. */ -static int8_t get_aux_interface(struct bmi2_aux_config *config, const struct bmi2_dev *dev) +static int8_t get_aux_interface(struct bmi2_aux_config *config, struct bmi2_dev *dev) { /* Variable to define error */ int8_t rslt; @@ -12682,7 +5663,7 @@ static int8_t get_aux_interface(struct bmi2_aux_config *config, const struct bmi * @brief This internal API gets auxiliary configurations like manual/auto mode * FCU write command enable and read burst length for both data and manual mode. */ -static int8_t get_aux_interface_config(struct bmi2_aux_config *config, const struct bmi2_dev *dev) +static int8_t get_aux_interface_config(struct bmi2_aux_config *config, struct bmi2_dev *dev) { /* Variable to define error */ int8_t rslt; @@ -12722,7 +5703,7 @@ static int8_t get_aux_interface_config(struct bmi2_aux_config *config, const str * @brief This internal API gets read out offset and ODR of the auxiliary * sensor. */ -static int8_t get_aux_cfg(struct bmi2_aux_config *config, const struct bmi2_dev *dev) +static int8_t get_aux_cfg(struct bmi2_aux_config *config, struct bmi2_dev *dev) { /* Variable to define error */ int8_t rslt; @@ -12743,1477 +5724,6 @@ static int8_t get_aux_cfg(struct bmi2_aux_config *config, const struct bmi2_dev return rslt; } -/*! - * @brief This internal API gets any-motion configurations like axes select, - * duration, threshold and output-configuration. - */ -static int8_t get_any_motion_config(struct bmi2_any_motion_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define LSB */ - uint16_t lsb; - - /* Variable to define MSB */ - uint16_t msb; - - /* Variable to define a word */ - uint16_t lsb_msb; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for any-motion */ - struct bmi2_feature_config any_mot_config = { 0, 0, 0 }; - - /* Search for any-motion feature and extract its configuration details */ - feat_found = extract_input_feat_config(&any_mot_config, BMI2_ANY_MOTION, dev); - if (feat_found) - { - /* Get the configuration from the page where any-motion feature resides */ - rslt = get_feat_config(any_mot_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for feature enable for any-motion */ - idx = any_mot_config.start_addr; - - /* Get word to calculate duration, x, y and z select */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get duration */ - config->duration = lsb_msb & ANY_NO_MOT_DUR_MASK; - - /* Get x-select */ - config->select_x = (lsb_msb & ANY_NO_MOT_X_SEL_MASK) >> ANY_NO_MOT_X_SEL_POS; - - /* Get y-select */ - config->select_y = (lsb_msb & ANY_NO_MOT_Y_SEL_MASK) >> ANY_NO_MOT_Y_SEL_POS; - - /* Get z-select */ - config->select_z = (lsb_msb & ANY_NO_MOT_Z_SEL_MASK) >> ANY_NO_MOT_Z_SEL_POS; - - /* Get word to calculate threshold, output configuration from the same word */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get threshold */ - config->threshold = lsb_msb & ANY_NO_MOT_THRES_MASK; - - /* Get output configuration */ - config->out_conf = (lsb_msb & ANY_NO_MOT_OUT_CONF_MASK) >> ANY_NO_MOT_OUT_CONF_POS; - - /* Copy out_conf value to a local copy in device structure */ - dev->int_map.any_mot_out_conf = (uint8_t) config->out_conf; - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets no-motion configurations like axes select, - * duration, threshold and output-configuration. - */ -static int8_t get_no_motion_config(struct bmi2_no_motion_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define LSB */ - uint16_t lsb = 0; - - /* Variable to define MSB */ - uint16_t msb = 0; - - /* Variable to define a word */ - uint16_t lsb_msb = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for no-motion */ - struct bmi2_feature_config no_mot_config = { 0, 0, 0 }; - - /* Search for no-motion feature and extract its configuration details */ - feat_found = extract_input_feat_config(&no_mot_config, BMI2_NO_MOTION, dev); - if (feat_found) - { - /* Get the configuration from the page where no-motion feature resides */ - rslt = get_feat_config(no_mot_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for feature enable for no-motion */ - idx = no_mot_config.start_addr; - - /* Get word to calculate duration, x, y and z select */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get duration */ - config->duration = lsb_msb & ANY_NO_MOT_DUR_MASK; - - /* Get x-select */ - config->select_x = (lsb_msb & ANY_NO_MOT_X_SEL_MASK) >> ANY_NO_MOT_X_SEL_POS; - - /* Get y-select */ - config->select_y = (lsb_msb & ANY_NO_MOT_Y_SEL_MASK) >> ANY_NO_MOT_Y_SEL_POS; - - /* Get z-select */ - config->select_z = (lsb_msb & ANY_NO_MOT_Z_SEL_MASK) >> ANY_NO_MOT_Z_SEL_POS; - - /* Get word to calculate threshold, output configuration from the same word */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get threshold */ - config->threshold = lsb_msb & ANY_NO_MOT_THRES_MASK; - - /* Get output configuration */ - config->out_conf = (lsb_msb & ANY_NO_MOT_OUT_CONF_MASK) >> ANY_NO_MOT_OUT_CONF_POS; - - /* Copy out_conf value to a local copy in device structure */ - dev->int_map.no_mot_out_conf = (uint8_t) config->out_conf; - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets sig-motion configurations like block-size, - * output-configuration and other parameters. - */ -static int8_t get_sig_motion_config(struct bmi2_sig_motion_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define LSB */ - uint16_t lsb = 0; - - /* Variable to define MSB */ - uint16_t msb = 0; - - /* Variable to define a word */ - uint16_t lsb_msb = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration sig-motion */ - struct bmi2_feature_config sig_mot_config = { 0, 0, 0 }; - - /* Search for sig-motion feature and extract its configuration details */ - feat_found = extract_input_feat_config(&sig_mot_config, BMI2_SIG_MOTION, dev); - if (feat_found) - { - /* Get the configuration from the page where sig-motion feature resides */ - rslt = get_feat_config(sig_mot_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for feature enable for sig-motion */ - idx = sig_mot_config.start_addr; - - /* Get word to calculate parameter 1 */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get parameter 1 */ - config->block_size = lsb_msb & SIG_MOT_PARAM_1_MASK; - - /* Get word to calculate parameter 2 */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get parameter 2 */ - config->param_2 = lsb_msb & SIG_MOT_PARAM_2_MASK; - - /* Get word to calculate parameter 3 */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get parameter 3 */ - config->param_3 = lsb_msb & SIG_MOT_PARAM_3_MASK; - - /* Get word to calculate parameter 4 */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get parameter 4 */ - config->param_4 = lsb_msb & SIG_MOT_PARAM_4_MASK; - - /* Get word to calculate parameter 5 */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get parameter 5 */ - config->param_5 = lsb_msb & SIG_MOT_PARAM_5_MASK; - - /* Get word to calculate and output configuration */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get output configuration */ - config->out_conf = (lsb_msb & SIG_MOT_OUT_CONF_MASK) >> SIG_MOT_OUT_CONF_POS; - - /* Copy out_conf value to a local copy in device structure */ - dev->int_map.sig_mot_out_conf = (uint8_t) config->out_conf; - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets step counter parameter configurations. - */ -static int8_t get_step_count_params_config(uint16_t *step_count_params, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Variable to define LSB */ - uint16_t lsb = 0; - - /* Variable to define MSB */ - uint16_t msb = 0; - - /* Variable to define a word */ - uint16_t lsb_msb = 0; - - /* Initialize feature configuration for step counter 1 */ - struct bmi2_feature_config step_params_config = { 0, 0, 0 }; - - /* Variable to index the page number */ - uint8_t page_idx; - - /* Variable to define the start page */ - uint8_t start_page; - - /* Variable to define start address of the parameters */ - uint8_t start_addr; - - /* Variable to define number of bytes */ - uint8_t n_bytes = (BMI2_STEP_CNT_N_PARAMS * 2); - - /* Variable to store number of pages */ - uint8_t n_pages = (n_bytes / 16); - - /* Variable to define the end page */ - uint8_t end_page; - - /* Variable to define the remaining bytes to be read */ - uint8_t remain_len; - - /* Variable to define the maximum words to be read in a page */ - uint8_t max_len = BMI2_FEAT_SIZE_IN_BYTES; - - /* Variable index bytes in a page */ - uint8_t page_byte_idx; - - /* Variable to index the parameters */ - uint8_t param_idx = 0; - - /* Search for step counter parameter feature and extract its configuration details */ - feat_found = extract_input_feat_config(&step_params_config, BMI2_STEP_COUNTER_PARAMS, dev); - if (feat_found) - { - /* Get the start page for the step counter parameters */ - start_page = step_params_config.page; - - /* Get the end page for the step counter parameters */ - end_page = start_page + n_pages; - - /* Get the start address for the step counter parameters */ - start_addr = step_params_config.start_addr; - - /* Get the remaining length of bytes to be read */ - remain_len = (uint8_t)((n_bytes - (n_pages * 16)) + start_addr); - for (page_idx = start_page; page_idx <= end_page; page_idx++) - { - /* Get the configuration from the respective page */ - rslt = get_feat_config(page_idx, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Start from address 0x00 when switched to next page */ - if (page_idx > start_page) - { - start_addr = 0; - } - - /* Remaining number of bytes to be read in the page */ - if (page_idx == end_page) - { - max_len = remain_len; - } - - /* Get the offset */ - page_byte_idx = start_addr; - while (page_byte_idx < max_len) - { - /* Get word to calculate the parameter*/ - lsb = (uint16_t) feat_config[page_byte_idx++]; - if (page_byte_idx < max_len) - { - msb = ((uint16_t) feat_config[page_byte_idx++] << 8); - } - lsb_msb = lsb | msb; - - /* Get parameters 1 to 25 */ - step_count_params[param_idx] = lsb_msb & STEP_COUNT_PARAMS_MASK; - - /* Increment to next parameter */ - param_idx++; - } - } - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets step counter/detector/activity configurations. - */ -static int8_t get_step_config(struct bmi2_step_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define LSB */ - uint16_t lsb = 0; - - /* Variable to define MSB */ - uint16_t msb = 0; - - /* Variable to define a word */ - uint16_t lsb_msb = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for step counter */ - struct bmi2_feature_config step_count_config = { 0, 0, 0 }; - - /* Search for step counter 4 feature and extract its configuration details */ - feat_found = extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); - if (feat_found) - { - /* Get the configuration from the page where step counter 4 parameter resides */ - rslt = get_feat_config(step_count_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for feature enable for step counter/detector/activity */ - idx = step_count_config.start_addr; - - /* Get word to calculate water-mark level and reset counter */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get water-mark level */ - config->watermark_level = lsb_msb & STEP_COUNT_WM_LEVEL_MASK; - - /* Get reset counter */ - config->reset_counter = (lsb_msb & STEP_COUNT_RST_CNT_MASK) >> STEP_COUNT_RST_CNT_POS; - - /* Get word to calculate output configuration of step detector and activity */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get output configuration of step detector */ - config->out_conf_step_detector = lsb_msb & STEP_DET_OUT_CONF_MASK; - - /* Get output configuration of step activity */ - config->out_conf_activity = (lsb_msb & STEP_ACT_OUT_CONF_MASK) >> STEP_ACT_OUT_CONF_POS; - - /* Get step buffer size */ - config->step_buffer_size = (lsb_msb & STEP_BUFFER_SIZE_MASK) >> STEP_BUFFER_SIZE_POS; - - /* Copy out_conf value to a local copy in device structure */ - dev->int_map.step_det_out_conf = (uint8_t) config->out_conf_step_detector; - dev->int_map.step_act_out_conf = (uint8_t) config->out_conf_activity; - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets gyroscope user-gain configurations like gain - * update value for x, y and z-axis. - */ -static int8_t get_gyro_gain_update_config(struct bmi2_gyro_user_gain_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define LSB */ - uint16_t lsb = 0; - - /* Variable to define MSB */ - uint16_t msb = 0; - - /* Variable to define a word */ - uint16_t lsb_msb = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for user-gain */ - struct bmi2_feature_config user_gain_config = { 0, 0, 0 }; - - /* Search for user-gain feature and extract its configuration details */ - feat_found = extract_input_feat_config(&user_gain_config, BMI2_GYRO_GAIN_UPDATE, dev); - if (feat_found) - { - /* Get the configuration from the page where user-gain feature resides */ - rslt = get_feat_config(user_gain_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for user-gain select */ - idx = user_gain_config.start_addr; - - /* Get word to calculate ratio_x */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get ratio_x */ - config->ratio_x = lsb_msb & GYR_USER_GAIN_RATIO_X_MASK; - - /* Get word to calculate ratio_y */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get ratio_y */ - config->ratio_y = lsb_msb & GYR_USER_GAIN_RATIO_Y_MASK; - - /* Get word to calculate ratio_z */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get ratio_z */ - config->ratio_z = lsb_msb & GYR_USER_GAIN_RATIO_Z_MASK; - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets tilt configurations like output-configuration. - */ -static int8_t get_tilt_config(struct bmi2_tilt_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define LSB */ - uint16_t lsb = 0; - - /* Variable to define MSB */ - uint16_t msb = 0; - - /* Variable to define a word */ - uint16_t lsb_msb = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for tilt */ - struct bmi2_feature_config tilt_config = { 0, 0, 0 }; - - /* Search for tilt feature and extract its configuration details */ - feat_found = extract_input_feat_config(&tilt_config, BMI2_TILT, dev); - if (feat_found) - { - /* Get the configuration from the page where tilt feature resides */ - rslt = get_feat_config(tilt_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for tilt select */ - idx = tilt_config.start_addr; - - /* Get word to calculate threshold and output configuration */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get output configuration */ - config->out_conf = (lsb_msb & TILT_OUT_CONF_MASK) >> TILT_OUT_CONF_POS; - - /* Copy out_conf value to a local copy in device structure */ - dev->int_map.tilt_out_conf = (uint8_t) config->out_conf; - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets uphold to wake configurations like output - * configuration. - */ -static int8_t get_up_hold_to_wake_config(struct bmi2_up_hold_to_wake_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define LSB */ - uint16_t lsb = 0; - - /* Variable to define MSB */ - uint16_t msb = 0; - - /* Variable to define a word */ - uint16_t lsb_msb = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for uphold to wake */ - struct bmi2_feature_config up_hold_to_wake_config = { 0, 0, 0 }; - - /* Search for uphold to wake feature and extract its configuration details */ - feat_found = extract_input_feat_config(&up_hold_to_wake_config, BMI2_UP_HOLD_TO_WAKE, dev); - if (feat_found) - { - /* Get the configuration from the page where uphold to wake feature resides */ - rslt = get_feat_config(up_hold_to_wake_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for uphold to wake select */ - idx = up_hold_to_wake_config.start_addr; - - /* Get word to calculate output configuration */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get output configuration */ - config->out_conf = (lsb_msb & UP_HOLD_TO_WAKE_OUT_CONF_MASK) >> UP_HOLD_TO_WAKE_OUT_CONF_POS; - - /* Copy out_conf value to a local copy in device structure */ - dev->int_map.up_hold_to_wake_out_conf = (uint8_t) config->out_conf; - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets glance detector configurations like output - * configuration. - */ -static int8_t get_glance_detect_config(struct bmi2_glance_det_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define LSB */ - uint16_t lsb = 0; - - /* Variable to define MSB */ - uint16_t msb = 0; - - /* Variable to define a word */ - uint16_t lsb_msb = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for glance detector */ - struct bmi2_feature_config glance_det_config = { 0, 0, 0 }; - - /* Search for glance detector feature and extract its configuration details */ - feat_found = extract_input_feat_config(&glance_det_config, BMI2_GLANCE_DETECTOR, dev); - if (feat_found) - { - /* Get the configuration from the page where glance detector feature resides */ - rslt = get_feat_config(glance_det_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for glance detector select */ - idx = glance_det_config.start_addr; - - /* Get word to calculate output configuration */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get output configuration */ - config->out_conf = (lsb_msb & GLANCE_DET_OUT_CONF_MASK) >> GLANCE_DET_OUT_CONF_POS; - - /* Copy out_conf value to a local copy in device structure */ - dev->int_map.glance_out_conf = (uint8_t) config->out_conf; - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets wake-up configurations like sensitivity, - * single/double tap enable and output-configuration. - */ -static int8_t get_wake_up_config(struct bmi2_wake_up_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define LSB */ - uint16_t lsb = 0; - - /* Variable to define MSB */ - uint16_t msb = 0; - - /* Variable to define a word */ - uint16_t lsb_msb = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for wake-up */ - struct bmi2_feature_config wake_up_config = { 0, 0, 0 }; - - /* Search for wake-up feature and extract its configuration details */ - feat_found = extract_input_feat_config(&wake_up_config, BMI2_WAKE_UP, dev); - if (feat_found) - { - /* Get the configuration from the page where wake-up feature resides */ - rslt = get_feat_config(wake_up_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for wake-up select */ - idx = wake_up_config.start_addr; - - /* Get word to calculate sensitivity, single/double tap - * enable and output configuration - */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get sensitivity */ - config->sensitivity = (lsb_msb & WAKE_UP_SENSITIVITY_MASK) >> WAKE_UP_SENSITIVITY_POS; - - /* Get single/double tap enable */ - config->single_tap_en = (lsb_msb & WAKE_UP_SINGLE_TAP_EN_MASK) >> WAKE_UP_SINGLE_TAP_EN_POS; - - /* Get output configuration */ - config->out_conf = (lsb_msb & WAKE_UP_OUT_CONF_MASK) >> WAKE_UP_OUT_CONF_POS; - - /* Copy out_conf value to a local copy in device structure */ - dev->int_map.wake_up_out_conf = (uint8_t) config->out_conf; - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets wrist gesture configurations like wearable-arm, - * and output-configuration. - */ -static int8_t get_wrist_gest_config(struct bmi2_wrist_gest_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for wrist gesture */ - struct bmi2_feature_config wrist_gest_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for wrist gesture feature and extract its configuration details */ - feat_found = extract_input_feat_config(&wrist_gest_config, BMI2_WRIST_GESTURE, dev); - if (feat_found) - { - /* Get the configuration from the page where wrist gesture feature resides */ - rslt = get_feat_config(wrist_gest_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for wrist gesture select */ - idx = wrist_gest_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - /* Get wearable arm */ - config->wearable_arm = (*(data_p + idx) & WRIST_GEST_WEAR_ARM_MASK) >> WRIST_GEST_WEAR_ARM_POS; - - /* Get output configuration */ - config->out_conf = BMI2_GET_BIT_POS0(*(data_p + idx), WRIST_GEST_OUT_CONF); - - /* Copy out_conf value to a local copy in device structure */ - dev->int_map.wrist_gest_out_conf = (uint8_t) config->out_conf; - - /* Increment the offset by 1 word to get min_flick_peak */ - idx++; - config->min_flick_peak = *(data_p + idx); - - /* Increment the offset by 1 word to get min_flick_samples */ - idx++; - config->min_flick_samples = *(data_p + idx); - - /* Increment the offset by 1 word to get max_duration */ - idx++; - config->max_duration = *(data_p + idx); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets wrist wear wake-up configurations like - * output-configuration. - */ -static int8_t get_wrist_wear_wake_up_config(struct bmi2_wrist_wear_wake_up_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for wrist wear wake-up */ - struct bmi2_feature_config wrist_wake_up_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for wrist wear wake-up feature and extract its configuration details */ - feat_found = extract_input_feat_config(&wrist_wake_up_config, BMI2_WRIST_WEAR_WAKE_UP, dev); - if (feat_found) - { - /* Get the configuration from the page where wrist wear wake-up feature resides */ - rslt = get_feat_config(wrist_wake_up_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for wrist wear wake-up select */ - idx = wrist_wake_up_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - /* Get output configuration */ - config->out_conf = BMI2_GET_BIT_POS0(*(data_p + idx), WRIST_WAKE_UP_OUT_CONF); - - /* Copy out_conf value to a local copy in device structure */ - dev->int_map.wrist_wear_wake_up_out_conf = (uint8_t) config->out_conf; - - /* Increment the offset value by 1 word to get min_angle_focus */ - idx++; - config->min_angle_focus = *(data_p + idx); - - /* Increment the offset value by 1 word to get min_angle_nonfocus */ - idx++; - config->min_angle_nonfocus = *(data_p + idx); - - /* Increment the offset value by 1 word to get max_tilt_lr */ - idx++; - config->max_tilt_lr = *(data_p + idx); - - /* Increment the offset value by 1 word to get max_tilt_ll */ - idx++; - config->max_tilt_ll = *(data_p + idx); - - /* Increment the offset value by 1 word to get max_tilt_pd */ - idx++; - config->max_tilt_pd = *(data_p + idx); - - /* Increment the offset value by 1 word to get max_tilt_pu */ - idx++; - config->max_tilt_pu = *(data_p + idx); - - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets wrist gesture configurations like wearable-arm, - * and output-configuration for wearable variant. - */ -static int8_t get_wrist_gest_w_config(struct bmi2_wrist_gest_w_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for wrist gesture */ - struct bmi2_feature_config wrist_gest_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for wrist gesture feature and extract its configuration details */ - feat_found = extract_input_feat_config(&wrist_gest_config, BMI2_WRIST_GESTURE_WH, dev); - if (feat_found) - { - /* Get the configuration from the page where wrist gesture feature resides */ - rslt = get_feat_config(wrist_gest_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for wrist gesture select */ - idx = wrist_gest_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - /* Get wearable arm */ - config->wearable_arm = (*(data_p + idx) & WRIST_GEST_WEAR_ARM_MASK) >> WRIST_GEST_WEAR_ARM_POS; - - /* Get output configuration */ - config->out_conf = BMI2_GET_BIT_POS0(*(data_p + idx), WRIST_GEST_OUT_CONF); - - /* Copy out_conf value to a local copy in device structure */ - dev->int_map.wrist_gest_out_conf = (uint8_t) config->out_conf; - - /* Increment the offset by 1 word to get min_flick_peak */ - idx++; - config->min_flick_peak = *(data_p + idx); - - /* Increment the offset by 1 word to get min_flick_samples */ - idx++; - config->min_flick_samples = *(data_p + idx); - - /* Increment the offset by 1 word to get max_duration */ - idx++; - config->max_duration = *(data_p + idx); - - /* Increment the offset by 1 word to get reporting delay */ - idx++; - config->reporting_delay = *(data_p + idx); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets wrist wear wake-up configurations like - * output-configuration for wearable variant. - */ -static int8_t get_wrist_wear_wake_up_wh_config(struct bmi2_wrist_wear_wake_up_wh_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for wrist wear wake-up */ - struct bmi2_feature_config wrist_wake_up_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for wrist wear wake-up feature and extract its configuration details */ - feat_found = extract_input_feat_config(&wrist_wake_up_config, BMI2_WRIST_WEAR_WAKE_UP_WH, dev); - if (feat_found) - { - /* Get the configuration from the page where wrist wear wake-up feature resides */ - rslt = get_feat_config(wrist_wake_up_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for wrist wear wake-up select */ - idx = wrist_wake_up_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - /* Get output configuration */ - config->out_conf = BMI2_GET_BIT_POS0(*(data_p + idx), WRIST_WAKE_UP_OUT_CONF); - - /* Copy out_conf value to a local copy in device structure */ - dev->int_map.wrist_wear_wake_up_out_conf = (uint8_t) config->out_conf; - - /* Increment the offset value by 1 word to get min_angle_focus */ - idx++; - config->min_angle_focus = *(data_p + idx); - - /* Increment the offset value by 1 word to get min_angle_nonfocus */ - idx++; - config->min_angle_nonfocus = *(data_p + idx); - - /* Increment the offset value by 1 word to get angle landscape right and angle landscape left */ - idx++; - config->angle_lr = BMI2_GET_BIT_POS0(*(data_p + idx), WRIST_WAKE_UP_ANGLE_LR); - config->angle_ll = BMI2_GET_BITS(*(data_p + idx), WRIST_WAKE_UP_ANGLE_LL); - - /* Increment the offset value by 1 word to get angle portrait down and angle portrait up */ - idx++; - config->angle_pd = BMI2_GET_BIT_POS0(*(data_p + idx), WRIST_WAKE_UP_ANGLE_PD); - config->angle_pu = BMI2_GET_BITS(*(data_p + idx), WRIST_WAKE_UP_ANGLE_PU); - - /* Increment the offset value by 1 word to get min duration quite and min duration moved */ - idx++; - config->min_dur_mov = BMI2_GET_BIT_POS0(*(data_p + idx), WRIST_WAKE_UP_MIN_DUR_MOVED); - config->min_dur_quite = BMI2_GET_BITS(*(data_p + idx), WRIST_WAKE_UP_MIN_DUR_QUITE); - - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets orientation configurations like upside/down - * detection, symmetrical modes, blocking mode, theta, hysteresis and output - * configuration. - */ -static int8_t get_orient_config(struct bmi2_orient_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define LSB */ - uint16_t lsb = 0; - - /* Variable to define MSB */ - uint16_t msb = 0; - - /* Variable to define a word */ - uint16_t lsb_msb = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for orient */ - struct bmi2_feature_config orient_config = { 0, 0, 0 }; - - /* Search for orient feature and extract its configuration details */ - feat_found = extract_input_feat_config(&orient_config, BMI2_ORIENTATION, dev); - if (feat_found) - { - /* Get the configuration from the page where orient feature resides */ - rslt = get_feat_config(orient_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for orient select */ - idx = orient_config.start_addr; - - /* Get word to calculate upside/down detection, - * symmetrical modes, blocking mode and theta - */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get upside/down detection */ - config->ud_en = (lsb_msb & ORIENT_UP_DOWN_MASK) >> ORIENT_UP_DOWN_POS; - - /* Get symmetrical modes */ - config->mode = (lsb_msb & ORIENT_SYMM_MODE_MASK) >> ORIENT_SYMM_MODE_POS; - - /* Get blocking mode */ - config->blocking = (lsb_msb & ORIENT_BLOCK_MODE_MASK) >> ORIENT_BLOCK_MODE_POS; - - /* Get theta */ - config->theta = (lsb_msb & ORIENT_THETA_MASK) >> ORIENT_THETA_POS; - - /* Get the next word to calculate hysteresis and output configuration */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get hysteresis */ - config->hysteresis = lsb_msb & ORIENT_HYST_MASK; - - /* Get output configuration */ - config->out_conf = (lsb_msb & ORIENT_OUT_CONF_MASK) >> ORIENT_OUT_CONF_POS; - - /* Copy out_conf value to a local copy in device structure */ - dev->int_map.orient_out_conf = (uint8_t) config->out_conf; - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets high-g configurations like threshold, - * hysteresis, duration, and output configuration. - */ -static int8_t get_high_g_config(struct bmi2_high_g_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define LSB */ - uint16_t lsb = 0; - - /* Variable to define MSB */ - uint16_t msb = 0; - - /* Variable to define a word */ - uint16_t lsb_msb = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for high-g */ - struct bmi2_feature_config high_g_config = { 0, 0, 0 }; - - /* Search for high-g feature and extract its configuration details */ - feat_found = extract_input_feat_config(&high_g_config, BMI2_HIGH_G, dev); - if (feat_found) - { - /* Get the configuration from the page where high-g feature resides */ - rslt = get_feat_config(high_g_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for high-g select */ - idx = high_g_config.start_addr; - - /* Get word to calculate threshold */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get threshold */ - config->threshold = lsb_msb & HIGH_G_THRES_MASK; - - /* Get word to calculate hysteresis */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get hysteresis */ - config->hysteresis = lsb_msb & HIGH_G_HYST_MASK; - - /* Get x_select */ - config->select_x = (lsb_msb & HIGH_G_X_SEL_MASK) >> HIGH_G_X_SEL_POS; - - /* Get y_select */ - config->select_y = (lsb_msb & HIGH_G_Y_SEL_MASK) >> HIGH_G_Y_SEL_POS; - - /* Get z_select */ - config->select_z = (lsb_msb & HIGH_G_Z_SEL_MASK) >> HIGH_G_Z_SEL_POS; - - /* Get word to calculate duration and output configuration */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get duration */ - config->duration = lsb_msb & HIGH_G_DUR_MASK; - - /* Get output configuration */ - config->out_conf = (lsb_msb & HIGH_G_OUT_CONF_MASK) >> HIGH_G_OUT_CONF_POS; - - /* Copy out_conf value to a local copy in device structure */ - dev->int_map.high_g_out_conf = (uint8_t) config->out_conf; - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets low-g configurations like threshold, - * hysteresis, duration, and output configuration. - */ -static int8_t get_low_g_config(struct bmi2_low_g_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define LSB */ - uint16_t lsb = 0; - - /* Variable to define MSB */ - uint16_t msb = 0; - - /* Variable to define a word */ - uint16_t lsb_msb = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for low-g */ - struct bmi2_feature_config low_g_config = { 0, 0, 0 }; - - /* Search for low-g feature and extract its configuration details */ - feat_found = extract_input_feat_config(&low_g_config, BMI2_LOW_G, dev); - if (feat_found) - { - /* Get the configuration from the page where low-g feature resides */ - rslt = get_feat_config(low_g_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for low-g select */ - idx = low_g_config.start_addr; - - /* Get word to calculate threshold */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get threshold */ - config->threshold = lsb_msb & LOW_G_THRES_MASK; - - /* Get word to calculate hysteresis */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get hysteresis */ - config->hysteresis = lsb_msb & LOW_G_HYST_MASK; - - /* Get word to calculate duration and output configuration */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get duration */ - config->duration = lsb_msb & LOW_G_DUR_MASK; - - /* Get output configuration */ - config->out_conf = (lsb_msb & LOW_G_OUT_CONF_MASK) >> LOW_G_OUT_CONF_POS; - - /* Copy out_conf value to a local copy in device structure */ - dev->int_map.low_g_out_conf = (uint8_t) config->out_conf; - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets flat configurations like theta, blocking, - * hold-time, hysteresis, and output configuration. - */ -static int8_t get_flat_config(struct bmi2_flat_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define LSB */ - uint16_t lsb = 0; - - /* Variable to define MSB */ - uint16_t msb = 0; - - /* Variable to define a word */ - uint16_t lsb_msb = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for flat */ - struct bmi2_feature_config flat_config = { 0, 0, 0 }; - - /* Search for flat feature and extract its configuration details */ - feat_found = extract_input_feat_config(&flat_config, BMI2_FLAT, dev); - if (feat_found) - { - /* Get the configuration from the page where flat feature resides */ - rslt = get_feat_config(flat_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for flat select */ - idx = flat_config.start_addr; - - /* Get word to calculate theta, blocking mode and output configuration */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get theta */ - config->theta = (lsb_msb & FLAT_THETA_MASK) >> FLAT_THETA_POS; - - /* Get blocking mode */ - config->blocking = (lsb_msb & FLAT_BLOCK_MASK) >> FLAT_BLOCK_POS; - - /* Get output configuration */ - config->out_conf = (lsb_msb & FLAT_OUT_CONF_MASK) >> FLAT_OUT_CONF_POS; - - /* Get word to calculate hysteresis and hold-time */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get hysteresis */ - config->hysteresis = lsb_msb & FLAT_HYST_MASK; - - /* Get hold-time */ - config->hold_time = (lsb_msb & FLAT_HOLD_TIME_MASK) >> FLAT_HOLD_TIME_POS; - - /* Copy out_conf value to a local copy in device structure */ - dev->int_map.flat_out_conf = (uint8_t) config->out_conf; - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets external sensor sync configurations like output - * configuration. - */ -static int8_t get_ext_sens_sync_config(struct bmi2_ext_sens_sync_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define LSB */ - uint16_t lsb = 0; - - /* Variable to define MSB */ - uint16_t msb = 0; - - /* Variable to define a word */ - uint16_t lsb_msb = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for external sensor sync */ - struct bmi2_feature_config ext_sens_sync_config = { 0, 0, 0 }; - - /* Search for external sensor sync feature and extract its configuration details */ - feat_found = extract_input_feat_config(&ext_sens_sync_config, BMI2_EXT_SENS_SYNC, dev); - if (feat_found) - { - /* Get the configuration from the page where external sensor sync feature resides */ - rslt = get_feat_config(ext_sens_sync_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for external sensor sync select */ - idx = ext_sens_sync_config.start_addr; - - /* Get word to calculate output configuration */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get output configuration */ - config->out_conf = (lsb_msb & EXT_SENS_SYNC_OUT_CONF_MASK) >> EXT_SENS_SYNC_OUT_CONF_POS; - - /* Copy out_conf value to a local copy in device structure */ - dev->int_map.ext_sync_out_conf = (uint8_t) config->out_conf; - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - /*! * @brief This internal API maps/un-maps feature interrupts to that of interrupt * pins. @@ -14280,7 +5790,7 @@ static int8_t map_feat_int(uint8_t *reg_data_array, enum bmi2_hw_int_pin int_pin /*! * @brief This internal API gets the accelerometer data from the register. */ -static int8_t get_accel_sensor_data(struct bmi2_sens_axes_data *data, uint8_t reg_addr, const struct bmi2_dev *dev) +static int8_t get_accel_sensor_data(struct bmi2_sens_axes_data *data, uint8_t reg_addr, struct bmi2_dev *dev) { /* Variable to define error */ int8_t rslt; @@ -14305,7 +5815,7 @@ static int8_t get_accel_sensor_data(struct bmi2_sens_axes_data *data, uint8_t re /*! * @brief This internal API gets the gyroscope data from the register. */ -static int8_t get_gyro_sensor_data(struct bmi2_sens_axes_data *data, uint8_t reg_addr, const struct bmi2_dev *dev) +static int8_t get_gyro_sensor_data(struct bmi2_sens_axes_data *data, uint8_t reg_addr, struct bmi2_dev *dev) { /* Variable to define error */ int8_t rslt; @@ -14374,16 +5884,43 @@ static void get_remapped_data(struct bmi2_sens_axes_data *data, const struct bmi { /* Array to defined the re-mapped sensor data */ int16_t remap_data[3] = { 0 }; + int16_t pos_multiplier = INT16_C(1); + int16_t neg_multiplier = INT16_C(-1); /* Fill the array with the un-mapped sensor data */ remap_data[0] = data->x; remap_data[1] = data->y; remap_data[2] = data->z; - /* Get the re-mapped x, y and z axes data */ - data->x = (int16_t)(remap_data[dev->remap.x_axis] * dev->remap.x_axis_sign); - data->y = (int16_t)(remap_data[dev->remap.y_axis] * dev->remap.y_axis_sign); - data->z = (int16_t)(remap_data[dev->remap.z_axis] * dev->remap.z_axis_sign); + /* Get the re-mapped x axis data */ + if (dev->remap.x_axis_sign == BMI2_POS_SIGN) + { + data->x = (int16_t)(remap_data[dev->remap.x_axis] * pos_multiplier); + } + else + { + data->x = (int16_t)(remap_data[dev->remap.x_axis] * neg_multiplier); + } + + /* Get the re-mapped y axis data */ + if (dev->remap.y_axis_sign == BMI2_POS_SIGN) + { + data->y = (int16_t)(remap_data[dev->remap.y_axis] * pos_multiplier); + } + else + { + data->y = (int16_t)(remap_data[dev->remap.y_axis] * neg_multiplier); + } + + /* Get the re-mapped z axis data */ + if (dev->remap.z_axis_sign == BMI2_POS_SIGN) + { + data->z = (int16_t)(remap_data[dev->remap.z_axis] * pos_multiplier); + } + else + { + data->z = (int16_t)(remap_data[dev->remap.z_axis] * neg_multiplier); + } } /*! @@ -14418,7 +5955,7 @@ static int8_t read_aux_data(uint8_t reg_addr, uint8_t *aux_data, uint16_t len, u { /* Read data from bmi2 data register */ rslt = bmi2_get_regs(BMI2_AUX_X_LSB_ADDR, reg_data, (uint16_t) burst_len, dev); - dev->delay_us(1000); + dev->delay_us(1000, dev->intf_ptr); if (rslt == BMI2_OK) { /* Get number of bytes to be read */ @@ -14479,7 +6016,7 @@ static int8_t write_aux_data(uint8_t reg_addr, uint8_t reg_data, struct bmi2_dev * @brief This internal API reads the user-defined bytes of data from the given * register address of auxiliary sensor in data mode. */ -static int8_t read_aux_data_mode(uint8_t *aux_data, const struct bmi2_dev *dev) +static int8_t read_aux_data_mode(uint8_t *aux_data, struct bmi2_dev *dev) { /* Variable to define error */ int8_t rslt; @@ -14547,575 +6084,6 @@ static int8_t map_read_len(uint8_t *len, const struct bmi2_dev *dev) return rslt; } -/*! - * @brief This internal API gets the output values of step counter. - */ -static int8_t get_step_counter_output(uint32_t *step_count, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variables to define index */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature output for step counter */ - struct bmi2_feature_config step_cnt_out_config = { 0, 0, 0 }; - - /* Search for step counter output feature and extract its configuration details */ - feat_found = extract_output_feat_config(&step_cnt_out_config, BMI2_STEP_COUNTER, dev); - if (feat_found) - { - /* Get the feature output configuration for step-counter */ - rslt = get_feat_config(step_cnt_out_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for step counter output */ - idx = step_cnt_out_config.start_addr; - - /* Get the step counter output in 4 bytes */ - *step_count = (uint32_t) feat_config[idx++]; - *step_count |= ((uint32_t) feat_config[idx++] << 8); - *step_count |= ((uint32_t) feat_config[idx++] << 16); - *step_count |= ((uint32_t) feat_config[idx++] << 24); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets the output values of step activity. - */ -static int8_t get_step_activity_output(uint8_t *step_act, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variables to define index */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature output for step activity */ - struct bmi2_feature_config step_act_out_config = { 0, 0, 0 }; - - /* Search for step activity output feature and extract its configuration details */ - feat_found = extract_output_feat_config(&step_act_out_config, BMI2_STEP_ACTIVITY, dev); - if (feat_found) - { - /* Get the feature output configuration for step-activity */ - rslt = get_feat_config(step_act_out_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for step activity output */ - idx = step_act_out_config.start_addr; - - /* Get the step activity output */ - *step_act = feat_config[idx]; - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets the output values of orientation: portrait- - * landscape and face up-down. - */ -static int8_t get_orient_output(struct bmi2_orientation_output *orient_out, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variables to define index */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature output for orientation */ - struct bmi2_feature_config orient_out_config = { 0, 0, 0 }; - - /* Search for orientation output feature and extract its configuration details */ - feat_found = extract_output_feat_config(&orient_out_config, BMI2_ORIENTATION, dev); - if (feat_found) - { - /* Get the feature output configuration for orientation */ - rslt = get_feat_config(orient_out_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for orientation output */ - idx = orient_out_config.start_addr; - - /* Get the output value of the orientation detection feature */ - orient_out->portrait_landscape = BMI2_GET_BIT_POS0(feat_config[idx], BMI2_ORIENT_DETECT); - - /* Get the output value of the orientation face up-down feature */ - orient_out->faceup_down = BMI2_GET_BITS(feat_config[idx], BMI2_ORIENT_FACE_UP_DWN); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets the output values of high-g. - */ -static int8_t get_high_g_output(uint8_t *high_g_out, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variables to define index */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature output for high-g */ - struct bmi2_feature_config high_g_out_config = { 0, 0, 0 }; - - /* Search for high-g output feature and extract its configuration details */ - feat_found = extract_output_feat_config(&high_g_out_config, BMI2_HIGH_G, dev); - if (feat_found) - { - /* Get the feature output configuration for high-g */ - rslt = get_feat_config(high_g_out_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for high-g output */ - idx = high_g_out_config.start_addr; - - /* Get the high-g output byte */ - *high_g_out = feat_config[idx]; - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets the saturation status for the gyroscope user - * gain update. - */ -static int8_t get_gyro_gain_update_status(struct bmi2_gyr_user_gain_status *user_gain_stat, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variables to define index */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature output for gyroscope user gain status */ - struct bmi2_feature_config user_gain_cfg = { 0, 0, 0 }; - - /* Search for gyroscope user gain status output feature and extract its - * configuration details - */ - feat_found = extract_output_feat_config(&user_gain_cfg, BMI2_GYRO_GAIN_UPDATE, dev); - if (feat_found) - { - /* Get the feature output configuration for gyroscope user gain status */ - rslt = get_feat_config(user_gain_cfg.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for gyroscope user gain status */ - idx = user_gain_cfg.start_addr; - - /* Get the saturation status for x-axis */ - user_gain_stat->sat_x = BMI2_GET_BIT_POS0(feat_config[idx], GYR_USER_GAIN_SAT_STAT_X); - - /* Get the saturation status for y-axis */ - user_gain_stat->sat_y = BMI2_GET_BITS(feat_config[idx], GYR_USER_GAIN_SAT_STAT_Y); - - /* Get the saturation status for z-axis */ - user_gain_stat->sat_z = BMI2_GET_BITS(feat_config[idx], GYR_USER_GAIN_SAT_STAT_Z); - - /* Get g trigger status */ - user_gain_stat->g_trigger_status = BMI2_GET_BITS(feat_config[idx], G_TRIGGER_STAT); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets the error status related to NVM. - */ -static int8_t get_nvm_error_status(struct bmi2_nvm_err_status *nvm_err_stat, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variables to define index */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature output for NVM error status */ - struct bmi2_feature_config nvm_err_cfg = { 0, 0, 0 }; - - /* Search for NVM error status feature and extract its configuration details */ - feat_found = extract_output_feat_config(&nvm_err_cfg, BMI2_NVM_STATUS, dev); - if (feat_found) - { - /* Get the feature output configuration for NVM error status */ - rslt = get_feat_config(nvm_err_cfg.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for NVM error status */ - idx = nvm_err_cfg.start_addr; - - /* Increment index to get the error status */ - idx++; - - /* Error when NVM load action fails */ - nvm_err_stat->load_error = BMI2_GET_BIT_POS0(feat_config[idx], NVM_LOAD_ERR_STATUS); - - /* Error when NVM program action fails */ - nvm_err_stat->prog_error = BMI2_GET_BITS(feat_config[idx], NVM_PROG_ERR_STATUS); - - /* Error when NVM erase action fails */ - nvm_err_stat->erase_error = BMI2_GET_BITS(feat_config[idx], NVM_ERASE_ERR_STATUS); - - /* Error when NVM program limit is exceeded */ - nvm_err_stat->exceed_error = BMI2_GET_BITS(feat_config[idx], NVM_END_EXCEED_STATUS); - - /* Error when NVM privilege mode is not acquired */ - nvm_err_stat->privil_error = BMI2_GET_BITS(feat_config[idx], NVM_PRIV_ERR_STATUS); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets the error status related to virtual frames. - */ -static int8_t get_vfrm_error_status(struct bmi2_vfrm_err_status *vfrm_err_stat, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variables to define index */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature output for VFRM error status */ - struct bmi2_feature_config vfrm_err_cfg = { 0, 0, 0 }; - - /* Search for VFRM error status feature and extract its configuration details */ - feat_found = extract_output_feat_config(&vfrm_err_cfg, BMI2_VFRM_STATUS, dev); - if (feat_found) - { - /* Get the feature output configuration for VFRM error status */ - rslt = get_feat_config(vfrm_err_cfg.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for VFRM error status */ - idx = vfrm_err_cfg.start_addr; - - /* Increment index to get the error status */ - idx++; - - /* Internal error while acquiring lock for FIFO */ - vfrm_err_stat->lock_error = BMI2_GET_BITS(feat_config[idx], VFRM_LOCK_ERR_STATUS); - - /* Internal error while writing byte into FIFO */ - vfrm_err_stat->write_error = BMI2_GET_BITS(feat_config[idx], VFRM_WRITE_ERR_STATUS); - - /* Internal error while writing into FIFO */ - vfrm_err_stat->fatal_error = BMI2_GET_BITS(feat_config[idx], VFRM_FATAL_ERR_STATUS); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets the output values of the wrist gesture. - */ -static int8_t get_wrist_gest_status(uint8_t *wrist_gest, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variables to define index */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature output for wrist gesture */ - struct bmi2_feature_config wrist_gest_out_config = { 0, 0, 0 }; - - /* Search for wrist gesture feature and extract its configuration details */ - feat_found = extract_output_feat_config(&wrist_gest_out_config, BMI2_WRIST_GESTURE, dev); - if (feat_found) - { - /* Get the feature output configuration for wrist gesture */ - rslt = get_feat_config(wrist_gest_out_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for wrist gesture output */ - idx = wrist_gest_out_config.start_addr; - - /* Get the wrist gesture output */ - *wrist_gest = feat_config[idx]; - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets the cross sensitivity coefficient between - * gyroscope's X and Z axes. - */ -static int8_t get_gyro_cross_sense(int16_t *cross_sense, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define index */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - uint8_t corr_fact_zx; - - /* Initialize feature output for gyroscope cross sensitivity */ - struct bmi2_feature_config cross_sense_out_config = { 0, 0, 0 }; - - if (dev->variant_feature & BMI2_MINIMAL_VARIANT) - { - /* For minimal variant fetch the correction factor from GPIO0 */ - rslt = bmi2_get_regs(BMI2_GYR_CAS_GPIO0_ADDR, &corr_fact_zx, 1, dev); - if (rslt == BMI2_OK) - { - /* Get the gyroscope cross sensitivity coefficient */ - if (corr_fact_zx & BMI2_GYRO_CROSS_AXES_SENSE_SIGN_BIT_MASK) - { - *cross_sense = (int16_t)(((int16_t)corr_fact_zx) - 128); - } - else - { - *cross_sense = (int16_t)(corr_fact_zx); - } - } - } - else - { - /* Search for gyroscope cross sensitivity feature and extract its configuration details */ - feat_found = extract_output_feat_config(&cross_sense_out_config, BMI2_GYRO_CROSS_SENSE, dev); - if (feat_found) - { - /* Get the feature output configuration for gyroscope cross sensitivity - * feature */ - rslt = get_feat_config(cross_sense_out_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for gyroscope cross sensitivity output */ - idx = cross_sense_out_config.start_addr; - - /* discard the MSB as GYR_CAS is of only 7 bit */ - feat_config[idx] = feat_config[idx] & BMI2_GYRO_CROSS_AXES_SENSE_MASK; - - /* Get the gyroscope cross sensitivity coefficient */ - if (feat_config[idx] & BMI2_GYRO_CROSS_AXES_SENSE_SIGN_BIT_MASK) - { - *cross_sense = (int16_t)(((int16_t)feat_config[idx]) - 128); - } - else - { - *cross_sense = (int16_t)(feat_config[idx]); - } - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - } - - return rslt; -} - -/*! - * @brief This internal API is used to get enable status of gyroscope user gain - * update. - */ -static int8_t get_user_gain_upd_status(uint8_t *status, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Variable to check APS status */ - uint8_t aps_stat = 0; - - /* Initialize feature configuration for gyroscope user gain */ - struct bmi2_feature_config gyr_user_gain_cfg = { 0, 0, 0 }; - - /* Search for user gain feature and extract its configuration details */ - feat_found = extract_input_feat_config(&gyr_user_gain_cfg, BMI2_GYRO_GAIN_UPDATE, dev); - if (feat_found) - { - /* Disable advance power save */ - aps_stat = dev->aps_status; - if (aps_stat == BMI2_ENABLE) - { - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - if (rslt == BMI2_OK) - { - /* Get the configuration from the page where user gain feature resides */ - rslt = get_feat_config(gyr_user_gain_cfg.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of user gain */ - idx = gyr_user_gain_cfg.start_addr + GYR_USER_GAIN_FEAT_EN_OFFSET; - - /* Set the feature enable status */ - *status = BMI2_GET_BITS(feat_config[idx], GYR_USER_GAIN_FEAT_EN); - } - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - /* Enable Advance power save if disabled while configuring and not when already disabled */ - if ((rslt == BMI2_OK) && (aps_stat == BMI2_ENABLE)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - - return rslt; -} - -/*! - * @brief This internal API gets the accelerometer self-test status. - */ -static int8_t get_accel_self_test_status(struct bmi2_acc_self_test_status *acc_self_test_stat, struct bmi2_dev *dev) -{ - int8_t rslt = BMI2_OK; - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - uint8_t idx = 0; - uint8_t feat_found; - struct bmi2_feature_config acc_self_test_cfg = { 0, 0, 0 }; - - /* Search for accelerometer self-test feature and extract its configuration details */ - feat_found = extract_output_feat_config(&acc_self_test_cfg, BMI2_ACCEL_SELF_TEST, dev); - if (feat_found) - { - /* Get the feature output configuration for accelerometer self-test status */ - rslt = get_feat_config(acc_self_test_cfg.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for accelerometer self-test */ - idx = acc_self_test_cfg.start_addr; - - /* Bit to check if self-test is done */ - acc_self_test_stat->acc_self_test_done = BMI2_GET_BIT_POS0(feat_config[idx], ACC_SELF_TEST_DONE); - - /* Bit to check if accelerometer X-axis test is passed */ - acc_self_test_stat->acc_x_ok = BMI2_GET_BITS(feat_config[idx], ACC_X_OK); - - /* Bit to check if accelerometer Y-axis test is passed */ - acc_self_test_stat->acc_y_ok = BMI2_GET_BITS(feat_config[idx], ACC_Y_OK); - - /* Bit to check if accelerometer Z-axis test is passed */ - acc_self_test_stat->acc_z_ok = BMI2_GET_BITS(feat_config[idx], ACC_Z_OK); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - /*! * @brief This internal API computes the number of bytes of accelerometer FIFO * data which is to be parsed in header-less mode. @@ -15502,9 +6470,6 @@ static void unpack_accel_data(struct bmi2_sens_axes_data *acc, /* Variables to store MSB value */ uint16_t data_msb; - /* Array to defined the re-mapped accelerometer data */ - int16_t remap_data[3] = { 0 }; - /* Accelerometer raw x data */ data_lsb = fifo->data[data_start_index++]; data_msb = fifo->data[data_start_index++]; @@ -15520,15 +6485,8 @@ static void unpack_accel_data(struct bmi2_sens_axes_data *acc, data_msb = fifo->data[data_start_index++]; acc->z = (int16_t)((data_msb << 8) | data_lsb); - /* Fill the array with the un-mapped accelerometer data */ - remap_data[0] = acc->x; - remap_data[1] = acc->y; - remap_data[2] = acc->z; - - /* Get the re-mapped x, y and z accelerometer data */ - acc->x = (int16_t)(remap_data[dev->remap.x_axis] * dev->remap.x_axis_sign); - acc->y = (int16_t)(remap_data[dev->remap.y_axis] * dev->remap.y_axis_sign); - acc->z = (int16_t)(remap_data[dev->remap.z_axis] * dev->remap.z_axis_sign); + /* Get the re-mapped accelerometer data */ + get_remapped_data(acc, dev); } /*! @@ -15916,9 +6874,6 @@ static void unpack_gyro_data(struct bmi2_sens_axes_data *gyr, /* Variables to store MSB value */ uint16_t data_msb; - /* Array to defined the re-mapped gyroscope data */ - int16_t remap_data[3] = { 0 }; - /* Gyroscope raw x data */ data_lsb = fifo->data[data_start_index++]; data_msb = fifo->data[data_start_index++]; @@ -15937,15 +6892,8 @@ static void unpack_gyro_data(struct bmi2_sens_axes_data *gyr, /* Get the compensated gyroscope data */ comp_gyro_cross_axis_sensitivity(gyr, dev); - /* Fill the array with the un-mapped gyroscope data */ - remap_data[0] = gyr->x; - remap_data[1] = gyr->y; - remap_data[2] = gyr->z; - - /* Get the re-mapped x, y and z gyroscope data */ - gyr->x = (int16_t)(remap_data[dev->remap.x_axis] * dev->remap.x_axis_sign); - gyr->y = (int16_t)(remap_data[dev->remap.y_axis] * dev->remap.y_axis_sign); - gyr->z = (int16_t)(remap_data[dev->remap.z_axis] * dev->remap.z_axis_sign); + /* Get the re-mapped gyroscope data */ + get_remapped_data(gyr, dev); } /*! @@ -16248,7 +7196,7 @@ static int8_t unpack_aux_frame(struct bmi2_aux_fifo_data *aux, case BMI2_FIFO_HEADER_ALL_FRM: case BMI2_FIFO_HEAD_LESS_ALL_FRM: - /* Partially read, then skip the data*/ + /* Partially read, then skip the data */ if ((*idx + fifo->all_frm_len) > fifo->length) { /* Move the data index to the last byte */ @@ -16465,11 +7413,15 @@ static int8_t check_empty_fifo(uint16_t *data_index, const struct bmi2_fifo_fram int8_t rslt = BMI2_OK; /* Validate data index */ - if (((*data_index) + 2) < fifo->length) + if (((*data_index) + 6) < fifo->length) { /* Check if FIFO is empty */ - if ((fifo->data[(*data_index)] == BMI2_FIFO_MSB_CONFIG_CHECK) && - (fifo->data[(*data_index) + 1] == BMI2_FIFO_LSB_CONFIG_CHECK)) + if (((fifo->data[(*data_index)] == BMI2_FIFO_MSB_CONFIG_CHECK) && + (fifo->data[(*data_index) + 1] == BMI2_FIFO_LSB_CONFIG_CHECK)) && + ((fifo->data[(*data_index) + 2] == BMI2_FIFO_MSB_CONFIG_CHECK) && + (fifo->data[(*data_index) + 3] == BMI2_FIFO_LSB_CONFIG_CHECK)) && + ((fifo->data[(*data_index) + 4] == BMI2_FIFO_MSB_CONFIG_CHECK) && + (fifo->data[(*data_index) + 5] == BMI2_FIFO_LSB_CONFIG_CHECK))) { /* Move the data index to the last byte to mark completion */ (*data_index) = fifo->length; @@ -16594,65 +7546,6 @@ static int8_t unpack_skipped_frame(uint16_t *data_index, struct bmi2_fifo_frame return rslt; } -/*! - * @brief This internal API is used to parse and store the activity recognition - * output from the FIFO data. - */ -static int8_t unpack_act_recog_output(struct bmi2_act_recog_output *act_recog, - uint16_t *data_index, - const struct bmi2_fifo_frame *fifo) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Variables to define 4 bytes of sensor time */ - uint32_t time_stamp_byte4 = 0; - uint32_t time_stamp_byte3 = 0; - uint32_t time_stamp_byte2 = 0; - uint32_t time_stamp_byte1 = 0; - - /* Validate data index */ - if ((*data_index + BMI2_FIFO_VIRT_ACT_DATA_LENGTH) >= fifo->length) - { - /* Update the data index to the last byte */ - (*data_index) = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - } - else - { - /* Get time-stamp from the activity recognition frame */ - time_stamp_byte4 = ((uint32_t)(fifo->data[(*data_index) + 3]) << 24); - time_stamp_byte3 = ((uint32_t)(fifo->data[(*data_index) + 2]) << 16); - time_stamp_byte2 = fifo->data[(*data_index) + 1] << 8; - time_stamp_byte1 = fifo->data[(*data_index)]; - - /* Update time-stamp from the virtual frame */ - act_recog->time_stamp = (time_stamp_byte4 | time_stamp_byte3 | time_stamp_byte2 | time_stamp_byte1); - - /* Move the data index by 4 bytes */ - (*data_index) = (*data_index) + BMI2_FIFO_VIRT_ACT_TIME_LENGTH; - - /* Update the previous activity from the virtual frame */ - act_recog->prev_act = fifo->data[(*data_index)]; - - /* Move the data index by 1 byte */ - (*data_index) = (*data_index) + BMI2_FIFO_VIRT_ACT_TYPE_LENGTH; - - /* Update the current activity from the virtual frame */ - act_recog->curr_act = fifo->data[(*data_index)]; - - /* Move the data index by 1 byte */ - (*data_index) = (*data_index) + BMI2_FIFO_VIRT_ACT_STAT_LENGTH; - - /* More frames could be read */ - rslt = BMI2_W_PARTIAL_READ; - } - - return rslt; -} - /*! * @brief This internal API enables and configures the accelerometer which is * needed for self-test operation. It also sets the amplitude for the self-test. @@ -16670,13 +7563,14 @@ static int8_t pre_self_test_config(struct bmi2_dev *dev) /* Enable accelerometer */ rslt = bmi2_sensor_enable(&sens_sel, 1, dev); - dev->delay_us(1000); + dev->delay_us(1000, dev->intf_ptr); /* Enable self-test amplitude */ if (rslt == BMI2_OK) { rslt = set_accel_self_test_amp(BMI2_ENABLE, dev); } + if (rslt == BMI2_OK) { /* Select accelerometer for sensor configurations */ @@ -16793,7 +7687,7 @@ static int8_t set_accel_self_test_amp(uint8_t amp, struct bmi2_dev *dev) * @brief This internal API reads the accelerometer data for x,y and z axis from * the sensor. The data units is in LSB format. */ -static int8_t read_accel_xyz(struct bmi2_sens_axes_data *accel, const struct bmi2_dev *dev) +static int8_t read_accel_xyz(struct bmi2_sens_axes_data *accel, struct bmi2_dev *dev) { /* Variable to define error */ int8_t rslt; @@ -16833,7 +7727,7 @@ static int8_t read_accel_xyz(struct bmi2_sens_axes_data *accel, const struct bmi * @brief This internal API reads the gyroscope data for x, y and z axis from * the sensor. The data units is in LSB format. */ -static int8_t read_gyro_xyz(struct bmi2_sens_axes_data *gyro, const struct bmi2_dev *dev) +static int8_t read_gyro_xyz(struct bmi2_sens_axes_data *gyro, struct bmi2_dev *dev) { /* Variable to define error */ int8_t rslt; @@ -16869,53 +7763,12 @@ static int8_t read_gyro_xyz(struct bmi2_sens_axes_data *gyro, const struct bmi2_ return rslt; } -/*! - * @brief This internal API skips S4S frame in the FIFO data while getting - * step activity output. - */ -static int8_t move_if_s4s_frame(const uint8_t *frame_header, uint16_t *data_index, const struct bmi2_fifo_frame *fifo) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Variable to extract virtual header byte */ - uint8_t virtual_header_mode; - - /* Variable to define pay-load in words */ - uint8_t payload_word = 0; - - /* Variable to define pay-load in bytes */ - uint8_t payload_bytes = 0; - - /* Extract virtual header mode from the frame header */ - virtual_header_mode = BMI2_GET_BITS(*frame_header, BMI2_FIFO_VIRT_FRM_MODE); - - /* If the extracted header byte is a virtual header */ - if (virtual_header_mode == BMI2_FIFO_VIRT_FRM_MODE) - { - /* If frame header is not activity recognition header */ - if (*frame_header != 0xC8) - { - /* Extract pay-load in words from the header byte */ - payload_word = BMI2_GET_BITS(*frame_header, BMI2_FIFO_VIRT_PAYLOAD) + 1; - - /* Convert to bytes */ - payload_bytes = (uint8_t)(payload_word * 2); - - /* Move the data index by those pay-load bytes */ - rslt = move_next_frame(data_index, payload_bytes, fifo); - } - } - - return rslt; -} - /*! * @brief This internal API converts LSB value of accelerometer axes to form * 'g' to 'mg' for self-test. */ -static void convert_lsb_g(const struct selftest_delta_limit *acc_data_diff, - struct selftest_delta_limit *acc_data_diff_mg, +static void convert_lsb_g(const struct bmi2_selftest_delta_limit *acc_data_diff, + struct bmi2_selftest_delta_limit *acc_data_diff_mg, const struct bmi2_dev *dev) { /* Variable to define LSB/g value of axes */ @@ -16960,7 +7813,7 @@ static int32_t power(int16_t base, uint8_t resolution) * @brief This internal API validates the accelerometer self-test data and * decides the result of self-test operation. */ -static int8_t validate_self_test(const struct selftest_delta_limit *accel_data_diff) +static int8_t validate_self_test(const struct bmi2_selftest_delta_limit *accel_data_diff) { /* Variable to define error */ int8_t rslt; @@ -16986,7 +7839,7 @@ static int8_t validate_self_test(const struct selftest_delta_limit *accel_data_d /*! * @brief This internal API gets the re-mapped x, y and z axes from the sensor. */ -static int8_t get_remap_axes(struct axes_remap *remap, struct bmi2_dev *dev) +static int8_t get_remap_axes(struct bmi2_axes_remap *remap, struct bmi2_dev *dev) { /* Variable to define error */ int8_t rslt = BMI2_OK; @@ -17013,38 +7866,39 @@ static int8_t get_remap_axes(struct axes_remap *remap, struct bmi2_dev *dev) /* Disable advance power save if enabled */ rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); } + if (rslt == BMI2_OK) { /* Search for axis re-mapping and extract its configuration details */ - feat_found = extract_input_feat_config(&remap_config, BMI2_AXIS_MAP, dev); + feat_found = bmi2_extract_input_feat_config(&remap_config, BMI2_AXIS_MAP, dev); if (feat_found) { - rslt = get_feat_config(remap_config.page, feat_config, dev); + rslt = bmi2_get_feat_config(remap_config.page, feat_config, dev); if (rslt == BMI2_OK) { /* Define the offset for axis re-mapping */ idx = remap_config.start_addr; /* Get the re-mapped x-axis */ - remap->x_axis = BMI2_GET_BIT_POS0(feat_config[idx], X_AXIS); + remap->x_axis = BMI2_GET_BIT_POS0(feat_config[idx], BMI2_X_AXIS); /* Get the re-mapped x-axis polarity */ - remap->x_axis_sign = BMI2_GET_BITS(feat_config[idx], X_AXIS_SIGN); + remap->x_axis_sign = BMI2_GET_BITS(feat_config[idx], BMI2_X_AXIS_SIGN); /* Get the re-mapped y-axis */ - remap->y_axis = BMI2_GET_BITS(feat_config[idx], Y_AXIS); + remap->y_axis = BMI2_GET_BITS(feat_config[idx], BMI2_Y_AXIS); /* Get the re-mapped y-axis polarity */ - remap->y_axis_sign = BMI2_GET_BITS(feat_config[idx], Y_AXIS_SIGN); + remap->y_axis_sign = BMI2_GET_BITS(feat_config[idx], BMI2_Y_AXIS_SIGN); /* Get the re-mapped z-axis */ - remap->z_axis = BMI2_GET_BITS(feat_config[idx], Z_AXIS); + remap->z_axis = BMI2_GET_BITS(feat_config[idx], BMI2_Z_AXIS); /* Increment byte to fetch the next data */ idx++; /* Get the re-mapped z-axis polarity */ - remap->z_axis_sign = BMI2_GET_BIT_POS0(feat_config[idx], Z_AXIS_SIGN); + remap->z_axis_sign = BMI2_GET_BIT_POS0(feat_config[idx], BMI2_Z_AXIS_SIGN); } } else @@ -17067,7 +7921,7 @@ static int8_t get_remap_axes(struct axes_remap *remap, struct bmi2_dev *dev) /*! * @brief This internal API sets the re-mapped x, y and z axes in the sensor. */ -static int8_t set_remap_axes(const struct axes_remap *remap, struct bmi2_dev *dev) +static int8_t set_remap_axes(const struct bmi2_axes_remap *remap, struct bmi2_dev *dev) { /* Variable to define error */ int8_t rslt = BMI2_OK; @@ -17115,36 +7969,37 @@ static int8_t set_remap_axes(const struct axes_remap *remap, struct bmi2_dev *de /* Disable advance power save if enabled */ rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); } + if (rslt == BMI2_OK) { /* Search for axis-re-mapping and extract its configuration details */ - feat_found = extract_input_feat_config(&remap_config, BMI2_AXIS_MAP, dev); + feat_found = bmi2_extract_input_feat_config(&remap_config, BMI2_AXIS_MAP, dev); if (feat_found) { /* Get the configuration from the page where axis re-mapping feature resides */ - rslt = get_feat_config(remap_config.page, feat_config, dev); + rslt = bmi2_get_feat_config(remap_config.page, feat_config, dev); if (rslt == BMI2_OK) { /* Define the offset in bytes */ idx = remap_config.start_addr; /* Set the value of re-mapped x-axis */ - x_axis = remap->x_axis & X_AXIS_MASK; + x_axis = remap->x_axis & BMI2_X_AXIS_MASK; /* Set the value of re-mapped x-axis sign */ - x_axis_sign = ((remap->x_axis_sign << X_AXIS_SIGN_POS) & X_AXIS_SIGN_MASK); + x_axis_sign = ((remap->x_axis_sign << BMI2_X_AXIS_SIGN_POS) & BMI2_X_AXIS_SIGN_MASK); /* Set the value of re-mapped y-axis */ - y_axis = ((remap->y_axis << Y_AXIS_POS) & Y_AXIS_MASK); + y_axis = ((remap->y_axis << BMI2_Y_AXIS_POS) & BMI2_Y_AXIS_MASK); /* Set the value of re-mapped y-axis sign */ - y_axis_sign = ((remap->y_axis_sign << Y_AXIS_SIGN_POS) & Y_AXIS_SIGN_MASK); + y_axis_sign = ((remap->y_axis_sign << BMI2_Y_AXIS_SIGN_POS) & BMI2_Y_AXIS_SIGN_MASK); /* Set the value of re-mapped z-axis */ - z_axis = ((remap->z_axis << Z_AXIS_POS) & Z_AXIS_MASK); + z_axis = ((remap->z_axis << BMI2_Z_AXIS_POS) & BMI2_Z_AXIS_MASK); /* Set the value of re-mapped z-axis sign */ - z_axis_sign = remap->z_axis_sign & Z_AXIS_SIGN_MASK; + z_axis_sign = remap->z_axis_sign & BMI2_Z_AXIS_SIGN_MASK; /* Arrange axes in the first byte */ feat_config[idx] = x_axis | x_axis_sign | y_axis | y_axis_sign | z_axis; @@ -17155,7 +8010,7 @@ static int8_t set_remap_axes(const struct axes_remap *remap, struct bmi2_dev *de /* Cannot OR in the second byte since it holds * gyroscope self-offset correction bit */ - feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], Z_AXIS_SIGN, z_axis_sign); + feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], BMI2_Z_AXIS_SIGN, z_axis_sign); /* Update the register address */ reg_addr = BMI2_FEATURES_REG_ADDR + remap_config.start_addr; @@ -17181,120 +8036,6 @@ static int8_t set_remap_axes(const struct axes_remap *remap, struct bmi2_dev *de return rslt; } -/*! - * @brief This internal API is used to get the feature configuration from the - * selected page. - */ -static int8_t get_feat_config(uint8_t sw_page, uint8_t *feat_config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define bytes remaining to read */ - uint8_t bytes_remain = BMI2_FEAT_SIZE_IN_BYTES; - - /* Variable to define the read-write length */ - uint8_t read_write_len = 0; - - /* Variable to define the feature configuration address */ - uint8_t addr = BMI2_FEATURES_REG_ADDR; - - /* Variable to define index */ - uint8_t index = 0; - - if (feat_config != NULL) - { - /* Check whether the page is valid */ - if (sw_page < dev->page_max) - { - /* Switch page */ - rslt = bmi2_set_regs(BMI2_FEAT_PAGE_ADDR, &sw_page, 1, dev); - - /* If user length is less than feature length */ - if ((rslt == BMI2_OK) && (dev->read_write_len < BMI2_FEAT_SIZE_IN_BYTES)) - { - /* Read-write should be even */ - if ((dev->read_write_len % 2) != 0) - { - dev->read_write_len--; - } - while (bytes_remain > 0) - { - if (bytes_remain >= dev->read_write_len) - { - /* Read from the page */ - rslt = bmi2_get_regs(addr, &feat_config[index], dev->read_write_len, dev); - - /* Update index */ - index += (uint8_t) dev->read_write_len; - - /* Update address */ - addr += (uint8_t) dev->read_write_len; - - /* Update read-write length */ - read_write_len += (uint8_t) dev->read_write_len; - } - else - { - /* Read from the page */ - rslt = bmi2_get_regs(addr, (uint8_t *) (feat_config + index), (uint16_t) bytes_remain, dev); - - /* Update read-write length */ - read_write_len += bytes_remain; - } - - /* Remaining bytes */ - bytes_remain = BMI2_FEAT_SIZE_IN_BYTES - read_write_len; - if (rslt != BMI2_OK) - { - break; - } - } - - /* Burst read 16 bytes */ - } - else if (rslt == BMI2_OK) - { - /* Get configuration from the page */ - rslt = bmi2_get_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - - } - else - { - rslt = BMI2_E_INVALID_PAGE; - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This internal API enables/disables compensation of the gain defined - * in the GAIN register. - */ -static int8_t enable_gyro_gain(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define register data */ - uint8_t reg_data = 0; - - rslt = bmi2_get_regs(BMI2_GYR_OFF_COMP_6_ADDR, ®_data, 1, dev); - if (rslt == BMI2_OK) - { - reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_GAIN_EN, enable); - rslt = bmi2_set_regs(BMI2_GYR_OFF_COMP_6_ADDR, ®_data, 1, dev); - } - - return rslt; -} - /*! * @brief This internal API corrects the gyroscope cross-axis sensitivity * between the z and the x axis. @@ -17305,66 +8046,6 @@ static void comp_gyro_cross_axis_sensitivity(struct bmi2_sens_axes_data *gyr_dat gyr_data->x = gyr_data->x - (int16_t)(((int32_t) dev->gyr_cross_sens_zx * (int32_t) gyr_data->z) / 512); } -/*! - * @brief This internal API is used to extract the input feature configuration - * details from the look-up table. - */ -static uint8_t extract_input_feat_config(struct bmi2_feature_config *feat_config, - uint8_t type, - const struct bmi2_dev *dev) -{ - /* Variable to define loop */ - uint8_t loop = 0; - - /* Variable to set flag */ - uint8_t feat_found = BMI2_FALSE; - - /* Search for the input feature from the input configuration array */ - while (loop < dev->input_sens) - { - if (dev->feat_config[loop].type == type) - { - *feat_config = dev->feat_config[loop]; - feat_found = BMI2_TRUE; - break; - } - loop++; - } - - /* Return flag */ - return feat_found; -} - -/*! - * @brief This internal API is used to extract the output feature configuration - * details from the look-up table. - */ -static uint8_t extract_output_feat_config(struct bmi2_feature_config *feat_output, - uint8_t type, - const struct bmi2_dev *dev) -{ - /* Variable to define loop */ - uint8_t loop = 0; - - /* Variable to set flag */ - uint8_t feat_found = BMI2_FALSE; - - /* Search for the output feature from the output configuration array */ - while (loop < dev->out_sens) - { - if (dev->feat_output[loop].type == type) - { - *feat_output = dev->feat_output[loop]; - feat_found = BMI2_TRUE; - break; - } - loop++; - } - - /* Return flag */ - return feat_found; -} - /*! * @brief This internal API is used to validate the boundary conditions. */ @@ -17439,12 +8120,12 @@ static int8_t set_accel_foc_config(struct bmi2_dev *dev) /* Variable to define error */ int8_t rslt; - /* Variable to set the accelerometer configuration value */ - uint8_t acc_conf_data = BMI2_FOC_ACC_CONF_VAL; - /* Variable to select the sensor */ uint8_t sens_list = BMI2_ACCEL; + /* Variable to set the accelerometer configuration value */ + uint8_t acc_conf_data = BMI2_FOC_ACC_CONF_VAL; + /* Disabling offset compensation */ rslt = set_accel_offset_comp(BMI2_DISABLE, dev); if (rslt == BMI2_OK) @@ -17455,6 +8136,7 @@ static int8_t set_accel_foc_config(struct bmi2_dev *dev) { /* Set accelerometer to normal mode by enabling it */ rslt = bmi2_sensor_enable(&sens_list, 1, dev); + if (rslt == BMI2_OK) { /* Disable advance power save mode */ @@ -17469,7 +8151,7 @@ static int8_t set_accel_foc_config(struct bmi2_dev *dev) /*! * @brief This internal API performs Fast Offset Compensation for accelerometer. */ -static int8_t perform_accel_foc(const struct accel_foc_g_value *accel_g_value, +static int8_t perform_accel_foc(const struct bmi2_accel_foc_g_value *accel_g_value, const struct bmi2_accel_config *acc_cfg, struct bmi2_dev *dev) { @@ -17486,7 +8168,7 @@ static int8_t perform_accel_foc(const struct accel_foc_g_value *accel_g_value, struct bmi2_sens_axes_data accel_value[128] = { { 0 } }; /* Structure to store accelerometer data temporarily */ - struct foc_temp_value temp = { 0, 0, 0 }; + struct bmi2_foc_temp_value temp = { 0, 0, 0 }; /* Structure to store the average of accelerometer data */ struct bmi2_sens_axes_data accel_avg = { 0, 0, 0, 0 }; @@ -17498,10 +8180,10 @@ static int8_t perform_accel_foc(const struct accel_foc_g_value *accel_g_value, uint8_t range = 0; /* Structure to store accelerometer data deviation from ideal value */ - struct offset_delta delta = { 0, 0, 0 }; + struct bmi2_offset_delta delta = { 0, 0, 0 }; /* Structure to store accelerometer offset values */ - struct accel_offset offset = { 0, 0, 0 }; + struct bmi2_accel_offset offset = { 0, 0, 0 }; /* Variable tries max 5 times for interrupt then generates timeout */ uint8_t try_cnt; @@ -17512,10 +8194,11 @@ static int8_t perform_accel_foc(const struct accel_foc_g_value *accel_g_value, while (try_cnt && (!(reg_status & BMI2_DRDY_ACC))) { /* 20ms delay for 50Hz ODR */ - dev->delay_us(20000); + dev->delay_us(20000, dev->intf_ptr); rslt = bmi2_get_status(®_status, dev); try_cnt--; } + if ((rslt == BMI2_OK) && (reg_status & BMI2_DRDY_ACC)) { rslt = read_accel_xyz(&accel_value[loop], dev); @@ -17525,6 +8208,7 @@ static int8_t perform_accel_foc(const struct accel_foc_g_value *accel_g_value, { rslt = read_accel_xyz(&accel_value[loop], dev); } + if (rslt == BMI2_OK) { /* Store the data in a temporary structure */ @@ -17537,6 +8221,7 @@ static int8_t perform_accel_foc(const struct accel_foc_g_value *accel_g_value, break; } } + if (rslt == BMI2_OK) { /* Take average of x, y and z data for lesser noise */ @@ -17628,9 +8313,9 @@ static void map_accel_range(uint8_t range_in, uint8_t *range_out) * @brief This internal API compensate the accelerometer data against gravity. */ static void comp_for_gravity(uint16_t lsb_per_g, - const struct accel_foc_g_value *g_val, + const struct bmi2_accel_foc_g_value *g_val, const struct bmi2_sens_axes_data *data, - struct offset_delta *comp_data) + struct bmi2_offset_delta *comp_data) { /* Array to store the accelerometer values in LSB */ int16_t accel_value_lsb[3] = { 0 }; @@ -17653,7 +8338,7 @@ static void comp_for_gravity(uint16_t lsb_per_g, * @note The bit position is always greater than 0 since accelerometer data is * 16 bit wide. */ -static void scale_accel_offset(uint8_t range, const struct offset_delta *comp_data, struct accel_offset *data) +static void scale_accel_offset(uint8_t range, const struct bmi2_offset_delta *comp_data, struct bmi2_accel_offset *data) { /* Variable to store the position of bit having 3.9mg resolution */ int8_t bit_pos_3_9mg; @@ -17683,7 +8368,7 @@ static void scale_accel_offset(uint8_t range, const struct offset_delta *comp_da */ static int8_t get_bit_pos_3_9mg(uint8_t range) { - /* Variable to store the bit position of 3.9mg resolution*/ + /* Variable to store the bit position of 3.9mg resolution */ int8_t bit_pos_3_9mg; /* Variable to shift the bits according to the resolution */ @@ -17717,7 +8402,7 @@ static int8_t get_bit_pos_3_9mg(uint8_t range) /*! * @brief This internal API inverts the accelerometer offset data. */ -static void invert_accel_offset(struct accel_offset *offset_data) +static void invert_accel_offset(struct bmi2_accel_offset *offset_data) { /* Get the offset data */ offset_data->x = (uint8_t)((offset_data->x) * (-1)); @@ -17729,7 +8414,7 @@ static void invert_accel_offset(struct accel_offset *offset_data) * @brief This internal API writes the offset data in the offset compensation * register. */ -static int8_t write_accel_offset(const struct accel_offset *offset, struct bmi2_dev *dev) +static int8_t write_accel_offset(const struct bmi2_accel_offset *offset, struct bmi2_dev *dev) { /* Variable to define error */ int8_t rslt; @@ -17784,6 +8469,124 @@ static int8_t restore_accel_foc_config(struct bmi2_accel_config *acc_cfg, return rslt; } +/*! + * @brief This internal API sets accelerometer configurations like ODR, + * bandwidth, performance mode and g-range. + */ +static int8_t set_accel_config(struct bmi2_accel_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store data */ + uint8_t reg_data; + + /* Array to store the default value of accelerometer configuration + * reserved registers + */ + uint8_t data_array[2] = { 0 }; + + /* Validate bandwidth and performance mode */ + rslt = validate_bw_perf_mode(&config->bwp, &config->filter_perf, dev); + if (rslt == BMI2_OK) + { + /* Validate ODR and range */ + rslt = validate_odr_range(&config->odr, &config->range, dev); + if (rslt == BMI2_OK) + { + /* Set accelerometer performance mode */ + reg_data = BMI2_SET_BITS(data_array[0], BMI2_ACC_FILTER_PERF_MODE, config->filter_perf); + + /* Set accelerometer bandwidth */ + reg_data = BMI2_SET_BITS(reg_data, BMI2_ACC_BW_PARAM, config->bwp); + + /* Set accelerometer ODR */ + reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_ACC_ODR, config->odr); + + /* Copy the register data to the array */ + data_array[0] = reg_data; + + /* Set accelerometer range */ + reg_data = BMI2_SET_BIT_POS0(data_array[1], BMI2_ACC_RANGE, config->range); + + /* Copy the register data to the array */ + data_array[1] = reg_data; + + /* Write accelerometer configuration to ACC_CONFand + * ACC_RANGE registers simultaneously as they lie in consecutive places + */ + rslt = bmi2_set_regs(BMI2_ACC_CONF_ADDR, data_array, 2, dev); + + /* Get error status to check for invalid configurations */ + if (rslt == BMI2_OK) + { + rslt = cfg_error_status(dev); + } + } + } + + return rslt; +} + +/*! + * @brief This internal API sets gyroscope configurations like ODR, bandwidth, + * low power/high performance mode, performance mode and range. It also + * maps/un-maps data interrupts to that of hardware interrupt line. + */ +static int8_t set_gyro_config(struct bmi2_gyro_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store data */ + uint8_t reg_data; + + /* Array to store the default value of gyroscope configuration reserved registers */ + uint8_t data_array[2] = { 0 }; + + /* Validate gyroscope configurations */ + rslt = validate_gyro_config(config, dev); + if (rslt == BMI2_OK) + { + /* Set gyroscope performance mode */ + reg_data = BMI2_SET_BITS(data_array[0], BMI2_GYR_FILTER_PERF_MODE, config->filter_perf); + + /* Set gyroscope noise performance mode */ + reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_NOISE_PERF_MODE, config->noise_perf); + + /* Set gyroscope bandwidth */ + reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_BW_PARAM, config->bwp); + + /* Set gyroscope ODR */ + reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_GYR_ODR, config->odr); + + /* Copy the register data to the array */ + data_array[0] = reg_data; + + /* Set gyroscope OIS range */ + reg_data = BMI2_SET_BITS(data_array[1], BMI2_GYR_OIS_RANGE, config->ois_range); + + /* Set gyroscope range */ + reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_GYR_RANGE, config->range); + + /* Copy the register data to the array */ + data_array[1] = reg_data; + + /* Write accelerometer configuration to GYR_CONF and GYR_RANGE + * registers simultaneously as they lie in consecutive places + */ + rslt = bmi2_set_regs(BMI2_GYR_CONF_ADDR, data_array, 2, dev); + + /* Get error status to check for invalid configurations */ + if (rslt == BMI2_OK) + { + rslt = cfg_error_status(dev); + } + } + + return rslt; +} + /*! * @brief This internal API saves the configurations before performing gyroscope * FOC. @@ -17821,14 +8624,14 @@ static int8_t set_gyro_foc_config(struct bmi2_dev *dev) { int8_t rslt; + /* Variable to select the sensor */ + uint8_t sens_list = BMI2_GYRO; + /* Array to set the gyroscope configuration value (ODR, Performance mode * and bandwidth) and gyroscope range */ uint8_t gyr_conf_data[2] = { BMI2_FOC_GYR_CONF_VAL, BMI2_GYR_RANGE_2000 }; - /* Variable to select the sensor */ - uint8_t sens_list = BMI2_GYRO; - /* Disabling gyroscope offset compensation */ rslt = bmi2_set_gyro_offset_comp(BMI2_DISABLE, dev); if (rslt == BMI2_OK) @@ -17841,6 +8644,7 @@ static int8_t set_gyro_foc_config(struct bmi2_dev *dev) { /* Set gyroscope to normal mode by enabling it */ rslt = bmi2_sensor_enable(&sens_list, 1, dev); + if (rslt == BMI2_OK) { /* Disable advance power save mode */ @@ -17904,22 +8708,27 @@ static void saturate_gyro_data(struct bmi2_sens_axes_data *gyr_off) { gyr_off->x = 511; } + if (gyr_off->x < -512) { gyr_off->x = -512; } + if (gyr_off->y > 511) { gyr_off->y = 511; } + if (gyr_off->y < -512) { gyr_off->y = -512; } + if (gyr_off->z > 511) { gyr_off->z = 511; } + if (gyr_off->z < -512) { gyr_off->z = -512; @@ -17946,7 +8755,7 @@ static int8_t null_ptr_check(const struct bmi2_dev *dev) /*! * @brief This internal API is to get the status of st_status from gry_crt_conf register */ -static int8_t get_st_running(uint8_t *st_status, const struct bmi2_dev *dev) +static int8_t get_st_running(uint8_t *st_status, struct bmi2_dev *dev) { int8_t rslt; uint8_t reg_data = 0; @@ -17994,7 +8803,7 @@ static int8_t set_st_running(uint8_t st_status, struct bmi2_dev *dev) /*! * @brief This API gets the status of rdy for dl bit. */ -static int8_t get_rdy_for_dl(uint8_t *rdy_for_dl, const struct bmi2_dev *dev) +static int8_t get_rdy_for_dl(uint8_t *rdy_for_dl, struct bmi2_dev *dev) { int8_t rslt; uint8_t reg_data = 0; @@ -18037,9 +8846,10 @@ static int8_t process_crt_download(uint8_t last_byte_flag, struct bmi2_dev *dev) { rslt = bmi2_set_regs(BMI2_CMD_REG_ADDR, &cmd, 1, dev); } + if ((!last_byte_flag) && (rslt == BMI2_OK)) { - rslt = wait_rdy_for_dl_toggle(CRT_READY_FOR_DOWNLOAD_RETRY, rdy_for_dl, dev); + rslt = wait_rdy_for_dl_toggle(BMI2_CRT_READY_FOR_DOWNLOAD_RETRY, rdy_for_dl, dev); } return rslt; @@ -18093,6 +8903,7 @@ static int8_t write_crt_config_file(uint16_t write_len, rslt = process_crt_download(last_byte_flag, dev); } } + if (rslt == BMI2_OK) { /* Write the remaining bytes in 2 bytes length */ @@ -18109,6 +8920,7 @@ static int8_t write_crt_config_file(uint16_t write_len, { last_byte_flag = 1; } + if (rslt == BMI2_OK) { rslt = process_crt_download(last_byte_flag, dev); @@ -18123,7 +8935,7 @@ static int8_t write_crt_config_file(uint16_t write_len, /*! * @brief This API is to wait till the rdy for dl bit toggles after every pack of bytes. */ -static int8_t wait_rdy_for_dl_toggle(uint8_t retry_complete, uint8_t download_ready, const struct bmi2_dev *dev) +static int8_t wait_rdy_for_dl_toggle(uint8_t retry_complete, uint8_t download_ready, struct bmi2_dev *dev) { int8_t rslt = BMI2_OK; uint8_t dl_ready = 0; @@ -18136,12 +8948,15 @@ static int8_t wait_rdy_for_dl_toggle(uint8_t retry_complete, uint8_t download_re { break; } - dev->delay_us(CRT_READY_FOR_DOWNLOAD_US); + + dev->delay_us(BMI2_CRT_READY_FOR_DOWNLOAD_US, dev->intf_ptr); } + if ((rslt == BMI2_OK) && (download_ready == dl_ready)) { rslt = BMI2_E_CRT_READY_FOR_DL_FAIL_ABORT; } + if (rslt == BMI2_OK) { rslt = get_st_running(&st_status, dev); @@ -18149,7 +8964,6 @@ static int8_t wait_rdy_for_dl_toggle(uint8_t retry_complete, uint8_t download_re { rslt = BMI2_E_ST_ALREADY_RUNNING; } - } return rslt; @@ -18158,7 +8972,7 @@ static int8_t wait_rdy_for_dl_toggle(uint8_t retry_complete, uint8_t download_re /*! * @brief This API is to wait till crt status complete. */ -static int8_t wait_st_running(uint8_t retry_complete, const struct bmi2_dev *dev) +static int8_t wait_st_running(uint8_t retry_complete, struct bmi2_dev *dev) { uint8_t st_status = 1; int8_t rslt = BMI2_OK; @@ -18170,8 +8984,10 @@ static int8_t wait_st_running(uint8_t retry_complete, const struct bmi2_dev *dev { break; } - dev->delay_us(CRT_WAIT_RUNNING_US); + + dev->delay_us(BMI2_CRT_WAIT_RUNNING_US, dev->intf_ptr); } + if ((rslt == BMI2_OK) && (st_status == 1)) { rslt = BMI2_E_ST_ALREADY_RUNNING; @@ -18274,7 +9090,7 @@ static int8_t do_gtrigger_test(uint8_t gyro_st_crt, struct bmi2_dev *dev) if (rslt == BMI2_OK) { /* Wait until st_status = 0 or time out is 2 seconds */ - rslt = wait_st_running(CRT_WAIT_RUNNING_RETRY_EXECUTION, dev); + rslt = wait_st_running(BMI2_CRT_WAIT_RUNNING_RETRY_EXECUTION, dev); /* CRT Running wait & check is successful */ if (rslt == BMI2_OK) @@ -18292,14 +9108,16 @@ static int8_t do_gtrigger_test(uint8_t gyro_st_crt, struct bmi2_dev *dev) { dev->read_write_len = 2; } - if (dev->read_write_len > (CRT_MAX_BURST_WORD_LENGTH * 2)) + + if (dev->read_write_len > (BMI2_CRT_MAX_BURST_WORD_LENGTH * 2)) { - dev->read_write_len = CRT_MAX_BURST_WORD_LENGTH * 2; + dev->read_write_len = BMI2_CRT_MAX_BURST_WORD_LENGTH * 2; } /* Reset the max burst length to default value */ rslt = set_maxburst_len(dev->read_write_len, dev); } + if (rslt == BMI2_OK) { rslt = get_rdy_for_dl(&download_ready, dev); @@ -18314,14 +9132,15 @@ static int8_t do_gtrigger_test(uint8_t gyro_st_crt, struct bmi2_dev *dev) /* Wait till either ready for download toggle or crt running = 0 */ if (rslt == BMI2_OK) { - rslt = wait_rdy_for_dl_toggle(CRT_READY_FOR_DOWNLOAD_RETRY, download_ready, dev); + rslt = wait_rdy_for_dl_toggle(BMI2_CRT_READY_FOR_DOWNLOAD_RETRY, download_ready, dev); if (rslt == BMI2_OK) { rslt = write_crt_config_file(dev->read_write_len, BMI2_CRT_CONFIG_FILE_SIZE, 0x1800, dev); } + if (rslt == BMI2_OK) { - rslt = wait_st_running(CRT_WAIT_RUNNING_RETRY_EXECUTION, dev); + rslt = wait_st_running(BMI2_CRT_WAIT_RUNNING_RETRY_EXECUTION, dev); rslt_crt = crt_gyro_st_update_result(dev); if (rslt == BMI2_OK) { @@ -18387,10 +9206,11 @@ static int8_t crt_prepare_setup(struct bmi2_dev *dev) sens_list = BMI2_ACCEL; rslt = bmi2_sensor_enable(&sens_list, 1, dev); } + if (rslt == BMI2_OK) { /* Disable Abort after 1 msec */ - dev->delay_us(1000); + dev->delay_us(1000, dev->intf_ptr); rslt = abort_bmi2(BMI2_DISABLE, dev); } @@ -18412,6 +9232,7 @@ static int8_t crt_gyro_st_update_result(struct bmi2_dev *dev) { rslt = get_gyro_gain_update_status(&user_gain_stat, dev); } + if (rslt == BMI2_OK) { switch (user_gain_stat.g_trigger_status) @@ -18430,6 +9251,7 @@ static int8_t crt_gyro_st_update_result(struct bmi2_dev *dev) { rslt = BMI2_E_DL_ERROR; } + break; case BMI2_G_TRIGGER_ABORT_ERROR: @@ -18441,6 +9263,7 @@ static int8_t crt_gyro_st_update_result(struct bmi2_dev *dev) { rslt = BMI2_E_ABORT_ERROR; } + break; case BMI2_G_TRIGGER_PRECON_ERROR: @@ -18485,13 +9308,14 @@ static int8_t get_maxburst_len(uint8_t *max_burst_len, struct bmi2_dev *dev) /* Disable advance power save if enabled */ rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); } + if (rslt == BMI2_OK) { /* Search for max burst length */ - feat_found = extract_input_feat_config(&maxburst_length_bytes, BMI2_MAX_BURST_LEN, dev); + feat_found = bmi2_extract_input_feat_config(&maxburst_length_bytes, BMI2_MAX_BURST_LEN, dev); if (feat_found) { - rslt = get_feat_config(maxburst_length_bytes.page, feat_config, dev); + rslt = bmi2_get_feat_config(maxburst_length_bytes.page, feat_config, dev); if (rslt == BMI2_OK) { /* Define the offset for max burst length */ @@ -18540,7 +9364,7 @@ static int8_t set_maxburst_len(const uint16_t write_len_byte, struct bmi2_dev *d } /* Max burst length is only 1 byte */ - if (burst_len > CRT_MAX_BURST_WORD_LENGTH) + if (burst_len > BMI2_CRT_MAX_BURST_WORD_LENGTH) { max_burst_len = UINT8_C(0xFF); } @@ -18556,16 +9380,17 @@ static int8_t set_maxburst_len(const uint16_t write_len_byte, struct bmi2_dev *d /* Disable advance power save if enabled */ rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); } + if (rslt == BMI2_OK) { /* Search for axis-re-mapping and extract its configuration details */ - feat_found = extract_input_feat_config(&maxburst_length_bytes, BMI2_MAX_BURST_LEN, dev); + feat_found = bmi2_extract_input_feat_config(&maxburst_length_bytes, BMI2_MAX_BURST_LEN, dev); if (feat_found) { /* Get the configuration from the page where axis * re-mapping feature resides */ - rslt = get_feat_config(maxburst_length_bytes.page, feat_config, dev); + rslt = bmi2_get_feat_config(maxburst_length_bytes.page, feat_config, dev); if (rslt == BMI2_OK) { /* Define the offset in bytes */ @@ -18622,19 +9447,19 @@ static int8_t set_nvm_prep_prog(uint8_t nvm_prep, struct bmi2_dev *dev) /* Search for bmi2 gyro self offset correction feature as nvm program preparation feature is * present in the same Word and extract its configuration details */ - feat_found = extract_input_feat_config(&nvm_config, BMI2_NVM_PROG_PREP, dev); + feat_found = bmi2_extract_input_feat_config(&nvm_config, BMI2_NVM_PROG_PREP, dev); if (feat_found) { /* Get the configuration from the page where nvm preparation feature enable feature * resides */ - rslt = get_feat_config(nvm_config.page, feat_config, dev); + rslt = bmi2_get_feat_config(nvm_config.page, feat_config, dev); if (rslt == BMI2_OK) { /* Define the offset for nvm preparation feature enable */ idx = nvm_config.start_addr; /* update nvm_prog_prep enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], NVM_PREP_FEATURE_EN, nvm_prep); + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_NVM_PREP_FEATURE_EN, nvm_prep); /* Update the register address */ reg_addr = BMI2_FEATURES_REG_ADDR + nvm_config.start_addr - 1; @@ -18668,19 +9493,19 @@ static int8_t select_self_test(uint8_t gyro_st_crt, struct bmi2_dev *dev) struct bmi2_feature_config gyro_self_test_crt_config = { 0, 0, 0 }; /* Search for bmi2 crt gyro self-test feature and extract its configuration details */ - feat_found = extract_input_feat_config(&gyro_self_test_crt_config, BMI2_CRT_GYRO_SELF_TEST, dev); + feat_found = bmi2_extract_input_feat_config(&gyro_self_test_crt_config, BMI2_CRT_GYRO_SELF_TEST, dev); if (feat_found) { /* Get the configuration from the page where gyro self-test and crt enable feature * resides */ - rslt = get_feat_config(gyro_self_test_crt_config.page, feat_config, dev); + rslt = bmi2_get_feat_config(gyro_self_test_crt_config.page, feat_config, dev); if (rslt == BMI2_OK) { /* Define the offset in bytes */ idx = gyro_self_test_crt_config.start_addr; /* update the gyro self-test crt enable bit */ - feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], GYRO_SELF_TEST_CRT_EN, gyro_st_crt); + feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], BMI2_GYRO_SELF_TEST_CRT_EN, gyro_st_crt); /* Update the register address */ reg_addr = BMI2_FEATURES_REG_ADDR + (gyro_self_test_crt_config.start_addr - 1); @@ -18728,6 +9553,7 @@ int8_t bmi2_abort_crt_gyro_st(struct bmi2_dev *dev) } } } + if (rslt == BMI2_OK) { rslt = abort_bmi2(BMI2_ENABLE, dev); @@ -18738,10 +9564,11 @@ int8_t bmi2_abort_crt_gyro_st(struct bmi2_dev *dev) { rslt = bmi2_set_regs(BMI2_CMD_REG_ADDR, &cmd, 1, dev); } + if (rslt == BMI2_OK) { /* wait until st_status = 0 or time out is 2 seconds */ - rslt = wait_st_running(CRT_WAIT_RUNNING_RETRY_EXECUTION, dev); + rslt = wait_st_running(BMI2_CRT_WAIT_RUNNING_RETRY_EXECUTION, dev); } /* Check G trigger status for error */ @@ -18791,18 +9618,18 @@ static int8_t abort_bmi2(uint8_t abort_enable, struct bmi2_dev *dev) struct bmi2_feature_config block_config = { 0, 0, 0 }; /* Search for bmi2 Abort feature and extract its configuration details */ - feat_found = extract_input_feat_config(&block_config, BMI2_ABORT_CRT_GYRO_SELF_TEST, dev); + feat_found = bmi2_extract_input_feat_config(&block_config, BMI2_ABORT_CRT_GYRO_SELF_TEST, dev); if (feat_found) { /* Get the configuration from the page where abort(block) feature resides */ - rslt = get_feat_config(block_config.page, feat_config, dev); + rslt = bmi2_get_feat_config(block_config.page, feat_config, dev); if (rslt == BMI2_OK) { /* Define the offset in bytes */ idx = block_config.start_addr; /* update the gyro self-test crt abort enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], ABORT_FEATURE_EN, abort_enable); + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_ABORT_FEATURE_EN, abort_enable); /* Update the register address */ reg_addr = BMI2_FEATURES_REG_ADDR + (block_config.start_addr - 1); @@ -18823,7 +9650,7 @@ static int8_t abort_bmi2(uint8_t abort_enable, struct bmi2_dev *dev) * @brief This api is use to wait till gyro self-test is completed and update the status of gyro * self-test. */ -static int8_t gyro_self_test_completed(struct bmi2_gyro_self_test_status *gyro_st_result, const struct bmi2_dev *dev) +static int8_t gyro_self_test_completed(struct bmi2_gyro_self_test_status *gyro_st_result, struct bmi2_dev *dev) { int8_t rslt; uint8_t reg_data; @@ -18851,7 +9678,7 @@ static int8_t gyro_self_test_completed(struct bmi2_gyro_self_test_status *gyro_s * @brief This api validates accel foc position as per the range */ static int8_t validate_foc_position(uint8_t sens_list, - const struct accel_foc_g_value *accel_g_axis, + const struct bmi2_accel_foc_g_value *accel_g_axis, struct bmi2_sens_axes_data avg_foc_data, struct bmi2_dev *dev) { @@ -18874,9 +9701,12 @@ static int8_t validate_foc_position(uint8_t sens_list, } else if (sens_list == BMI2_GYRO) { - if (((avg_foc_data.x >= GYRO_FOC_NOISE_LIMIT_NEGATIVE) && (avg_foc_data.x <= GYRO_FOC_NOISE_LIMIT_POSITIVE)) && - ((avg_foc_data.y >= GYRO_FOC_NOISE_LIMIT_NEGATIVE) && (avg_foc_data.y <= GYRO_FOC_NOISE_LIMIT_POSITIVE)) && - ((avg_foc_data.z >= GYRO_FOC_NOISE_LIMIT_NEGATIVE) && (avg_foc_data.z <= GYRO_FOC_NOISE_LIMIT_POSITIVE))) + if (((avg_foc_data.x >= BMI2_GYRO_FOC_NOISE_LIMIT_NEGATIVE) && + (avg_foc_data.x <= BMI2_GYRO_FOC_NOISE_LIMIT_POSITIVE)) && + ((avg_foc_data.y >= BMI2_GYRO_FOC_NOISE_LIMIT_NEGATIVE) && + (avg_foc_data.y <= BMI2_GYRO_FOC_NOISE_LIMIT_POSITIVE)) && + ((avg_foc_data.z >= BMI2_GYRO_FOC_NOISE_LIMIT_NEGATIVE) && + (avg_foc_data.z <= BMI2_GYRO_FOC_NOISE_LIMIT_POSITIVE))) { rslt = BMI2_OK; } @@ -18903,26 +9733,26 @@ static int8_t validate_foc_accel_axis(int16_t avg_foc_data, struct bmi2_dev *dev range = sens_cfg.cfg.acc.range; /* reference LSB value of 16G */ - if ((range == BMI2_ACC_RANGE_2G) && (avg_foc_data > ACC_2G_MIN_NOISE_LIMIT) && - (avg_foc_data < ACC_2G_MAX_NOISE_LIMIT)) + if ((range == BMI2_ACC_RANGE_2G) && (avg_foc_data > BMI2_ACC_2G_MIN_NOISE_LIMIT) && + (avg_foc_data < BMI2_ACC_2G_MAX_NOISE_LIMIT)) { rslt = BMI2_OK; } /* reference LSB value of 16G */ - else if ((range == BMI2_ACC_RANGE_4G) && (avg_foc_data > ACC_4G_MIN_NOISE_LIMIT) && - (avg_foc_data < ACC_4G_MAX_NOISE_LIMIT)) + else if ((range == BMI2_ACC_RANGE_4G) && (avg_foc_data > BMI2_ACC_4G_MIN_NOISE_LIMIT) && + (avg_foc_data < BMI2_ACC_4G_MAX_NOISE_LIMIT)) { rslt = BMI2_OK; } /* reference LSB value of 16G */ - else if ((range == BMI2_ACC_RANGE_8G) && (avg_foc_data > ACC_8G_MIN_NOISE_LIMIT) && - (avg_foc_data < ACC_8G_MAX_NOISE_LIMIT)) + else if ((range == BMI2_ACC_RANGE_8G) && (avg_foc_data > BMI2_ACC_8G_MIN_NOISE_LIMIT) && + (avg_foc_data < BMI2_ACC_8G_MAX_NOISE_LIMIT)) { rslt = BMI2_OK; } /* reference LSB value of 16G */ - else if ((range == BMI2_ACC_RANGE_16G) && (avg_foc_data > ACC_16G_MIN_NOISE_LIMIT) && - (avg_foc_data < ACC_16G_MAX_NOISE_LIMIT)) + else if ((range == BMI2_ACC_RANGE_16G) && (avg_foc_data > BMI2_ACC_16G_MIN_NOISE_LIMIT) && + (avg_foc_data < BMI2_ACC_16G_MAX_NOISE_LIMIT)) { rslt = BMI2_OK; } @@ -18965,7 +9795,7 @@ int8_t bmi2_nvm_prog(struct bmi2_dev *dev) rslt = set_nvm_prep_prog(BMI2_ENABLE, dev); if (rslt == BMI2_OK) { - dev->delay_us(40000); + dev->delay_us(40000, dev->intf_ptr); /* Set the NVM_CONF.nvm_prog_en bit in order to enable the NVM * programming */ @@ -18996,10 +9826,11 @@ int8_t bmi2_nvm_prog(struct bmi2_dev *dev) /* Wait till cmd_rdy becomes 1 indicating * nvm process completes */ - dev->delay_us(20000); + dev->delay_us(20000, dev->intf_ptr); } } } + if ((rslt == BMI2_OK) && (cmd_rdy != BMI2_TRUE)) { rslt = BMI2_E_WRITE_CYCLE_ONGOING; @@ -19011,6 +9842,7 @@ int8_t bmi2_nvm_prog(struct bmi2_dev *dev) rslt = BMI2_E_WRITE_CYCLE_ONGOING; } } + if (rslt == BMI2_OK) { /* perform soft reset */ @@ -19030,7 +9862,9 @@ int8_t bmi2_nvm_prog(struct bmi2_dev *dev) * @brief This API reads and provides average for 128 samples of sensor data for foc operation * gyro. */ -static int8_t get_average_of_sensor_data(uint8_t sens_list, struct foc_temp_value *temp_foc_data, struct bmi2_dev *dev) +static int8_t get_average_of_sensor_data(uint8_t sens_list, + struct bmi2_foc_temp_value *temp_foc_data, + struct bmi2_dev *dev) { int8_t rslt = 0; struct bmi2_sensor_data sensor_data = { 0 }; @@ -19055,7 +9889,7 @@ static int8_t get_average_of_sensor_data(uint8_t sens_list, struct foc_temp_valu datardy_try_cnt = 5; do { - dev->delay_us(20000); + dev->delay_us(20000, dev->intf_ptr); rslt = bmi2_get_status(&drdy_status, dev); datardy_try_cnt--; } while ((rslt == BMI2_OK) && (!(drdy_status & sensor_drdy)) && (datardy_try_cnt)); @@ -19065,6 +9899,7 @@ static int8_t get_average_of_sensor_data(uint8_t sens_list, struct foc_temp_valu rslt = BMI2_E_DATA_RDY_INT_FAILED; break; } + rslt = bmi2_get_sensor_data(&sensor_data, 1, dev); if (rslt == BMI2_OK) @@ -19086,8 +9921,10 @@ static int8_t get_average_of_sensor_data(uint8_t sens_list, struct foc_temp_valu { break; } + sample_count++; } + if (rslt == BMI2_OK) { temp_foc_data->x = (temp_foc_data->x / BMI2_FOC_SAMPLE_LIMIT); @@ -19098,149 +9935,11 @@ static int8_t get_average_of_sensor_data(uint8_t sens_list, struct foc_temp_valu return rslt; } -/*! - * @brief This internal API sets primary OIS configurations for - * selecting filter cut-off frequency . - */ -static int8_t set_primary_ois_config(const struct bmi2_primary_ois_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define count */ - uint8_t i = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for primary OIS */ - struct bmi2_feature_config primary_ois_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *)(void *)feat_config; - - /* Search for priamry OIS feature and extract its configuration details */ - feat_found = extract_input_feat_config(&primary_ois_config, BMI2_PRIMARY_OIS, dev); - if (feat_found) - { - /* Get the configuration from the page where primary OIS feature resides */ - rslt = get_feat_config(primary_ois_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for priamry OIS select */ - idx = primary_ois_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - /* Enable lp filter */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), LP_FILTER_EN, config->lp_filter_enable); - - /* Set lp filter cut-off frequency */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), LP_FILTER_CONFIG, config->lp_filter_config); - - /* Enable Gyro on OIS */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), PRIMARY_OIS_GYR_EN, config->primary_ois_gyro_en); - - /* Enable Accel on OIS */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), PRIMARY_OIS_ACC_EN, config->primary_ois_accel_en); - - /* Increment offset by 1 more word to get the total length in words */ - idx++; - - /* Get total length in bytes to copy from local pointer to the array */ - idx = (uint8_t)(idx * 2) - primary_ois_config.start_addr; - - /* Copy the bytes to be set back to the array */ - for (i = 0; i < idx; i++) - { - feat_config[primary_ois_config.start_addr + - i] = *((uint8_t *)data_p + primary_ois_config.start_addr + i); - } - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets priamry OIS configurations for - * low pass cut-off frequency. - */ -static int8_t get_primary_ois_config(struct bmi2_primary_ois_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define LSB */ - uint16_t lsb; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for primary OIS */ - struct bmi2_feature_config primary_ois_config = { 0, 0, 0 }; - - /* Search for primary OIS feature and extract its configuration details */ - feat_found = extract_input_feat_config(&primary_ois_config, BMI2_PRIMARY_OIS, dev); - if (feat_found) - { - /* Get the configuration from the page where primary OIS feature resides */ - rslt = get_feat_config(primary_ois_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for feature enable for primary OIS */ - idx = primary_ois_config.start_addr; - - /* Get word to calculate filter state, cut-off frequency, gyro-OIS, - * accel-OIS select */ - lsb = (uint16_t)feat_config[idx++]; - - /* Get low pass filter state */ - config->lp_filter_enable = lsb & LP_FILTER_EN_MASK; - - /* Get lp filter cut-off frequency */ - config->lp_filter_config = (lsb & LP_FILTER_CONFIG_MASK) >> LP_FILTER_CONFIG_POS; - - /* Get primary OIS gyro on ois state */ - config->primary_ois_gyro_en = (lsb & PRIMARY_OIS_GYR_EN_MASK) >> PRIMARY_OIS_GYR_EN_POS; - - /* Get primary OIS accel on ois state */ - config->primary_ois_accel_en = (lsb & PRIMARY_OIS_ACC_EN_MASK) >> PRIMARY_OIS_ACC_EN_POS; - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - /*! * @brief This internal API extract the identification feature from the DMR page * and retrieve the config file major and minor version. */ -static int8_t extract_config_file(uint16_t *config_major, uint8_t *config_minor, struct bmi2_dev *dev) +static int8_t extract_config_file(uint8_t *config_major, uint8_t *config_minor, struct bmi2_dev *dev) { /* Variable to define the result */ int8_t rslt = BMI2_OK; @@ -19276,16 +9975,17 @@ static int8_t extract_config_file(uint16_t *config_major, uint8_t *config_minor, /* Disable advance power save if enabled */ rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); } + if (rslt == BMI2_OK) { /* Search for config file identification feature and extract its configuration * details */ - feat_found = extract_input_feat_config(&config_id, BMI2_CONFIG_ID, dev); + feat_found = bmi2_extract_input_feat_config(&config_id, BMI2_CONFIG_ID, dev); if (feat_found) { /* Get the configuration from the page where config file identification * feature resides */ - rslt = get_feat_config(config_id.page, feat_config, dev); + rslt = bmi2_get_feat_config(config_id.page, feat_config, dev); if (rslt == BMI2_OK) { /* Define the offset for config file identification */ @@ -19319,9 +10019,31 @@ static int8_t extract_config_file(uint16_t *config_major, uint8_t *config_minor, } /*! - * @brief This internal API gets the output data of OIS. + *@brief This internal API is used to map the interrupts to the sensor. */ -static int8_t get_ois_output(struct bmi2_ois_output *ois_output, struct bmi2_dev *dev) +static void extract_feat_int_map(struct bmi2_map_int *map_int, uint8_t type, const struct bmi2_dev *dev) +{ + /* Variable to define loop */ + uint8_t loop = 0; + + /* Search for the interrupts from the input configuration array */ + while (loop < dev->sens_int_map) + { + if (dev->map_int[loop].type == type) + { + *map_int = dev->map_int[loop]; + break; + } + + loop++; + } +} + +/*! + * @brief This internal API gets the saturation status for the gyroscope user + * gain update. + */ +static int8_t get_gyro_gain_update_status(struct bmi2_gyr_user_gain_status *user_gain_stat, struct bmi2_dev *dev) { /* Variable to define error */ int8_t rslt; @@ -19329,71 +10051,39 @@ static int8_t get_ois_output(struct bmi2_ois_output *ois_output, struct bmi2_dev /* Array to define the feature configuration */ uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - /* Variables to store MSB value */ - uint8_t msb = 0; - - /* Variables to store LSB value */ - uint8_t lsb = 0; - - /* Variables to store both MSB and LSB value */ - uint16_t lsb_msb = 0; - /* Variables to define index */ uint8_t idx = 0; /* Variable to set flag */ uint8_t feat_found; - /* Initialize feature output for OIS */ - struct bmi2_feature_config ois_config = { 0, 0, 0 }; + /* Initialize feature output for gyroscope user gain status */ + struct bmi2_feature_config user_gain_cfg = { 0, 0, 0 }; - /* Search for OIS output feature and extract its configuration details */ - feat_found = extract_output_feat_config(&ois_config, BMI2_OIS_OUTPUT, dev); + /* Search for gyroscope user gain status output feature and extract its + * configuration details + */ + feat_found = extract_output_feat_config(&user_gain_cfg, BMI2_GYRO_GAIN_UPDATE, dev); if (feat_found) { - /* Get the feature output configuration for OIS */ - rslt = get_feat_config(ois_config.page, feat_config, dev); - + /* Get the feature output configuration for gyroscope user gain status */ + rslt = bmi2_get_feat_config(user_gain_cfg.page, feat_config, dev); if (rslt == BMI2_OK) { - /* Define the offset in bytes for OIS output start address */ - idx = ois_config.start_addr; + /* Define the offset in bytes for gyroscope user gain status */ + idx = user_gain_cfg.start_addr; - /* Read OIS accel x axis */ - lsb = feat_config[idx++]; - msb = feat_config[idx++]; - lsb_msb = (uint16_t)lsb | (uint16_t)msb << 8; - ois_output->ois_acc_x = (int16_t)lsb_msb; + /* Get the saturation status for x-axis */ + user_gain_stat->sat_x = BMI2_GET_BIT_POS0(feat_config[idx], BMI2_GYR_USER_GAIN_SAT_STAT_X); - /* Read OIS accel y axis */ - lsb = feat_config[idx++]; - msb = feat_config[idx++]; - lsb_msb = (uint16_t)lsb | (uint16_t)msb << 8; - ois_output->ois_acc_y = (int16_t)lsb_msb; + /* Get the saturation status for y-axis */ + user_gain_stat->sat_y = BMI2_GET_BITS(feat_config[idx], BMI2_GYR_USER_GAIN_SAT_STAT_Y); - /* Read OIS accel z axis */ - lsb = feat_config[idx++]; - msb = feat_config[idx++]; - lsb_msb = (uint16_t)lsb | (uint16_t)msb << 8; - ois_output->ois_acc_z = (int16_t)lsb_msb; + /* Get the saturation status for z-axis */ + user_gain_stat->sat_z = BMI2_GET_BITS(feat_config[idx], BMI2_GYR_USER_GAIN_SAT_STAT_Z); - /* Read OIS gyro x axis */ - lsb = feat_config[idx++]; - msb = feat_config[idx++]; - lsb_msb = (uint16_t)lsb | (uint16_t)msb << 8; - ois_output->ois_gyro_x = (int16_t)lsb_msb; - - /* Read OIS gyro y axis */ - lsb = feat_config[idx++]; - msb = feat_config[idx++]; - lsb_msb = (uint16_t)lsb | (uint16_t)msb << 8; - ois_output->ois_gyro_y = (int16_t)lsb_msb; - - /* Read OIS gyro z axis */ - lsb = feat_config[idx++]; - msb = feat_config[idx++]; - lsb_msb = (uint16_t)lsb | (uint16_t)msb << 8; - ois_output->ois_gyro_z = (int16_t)lsb_msb; + /* Get g trigger status */ + user_gain_stat->g_trigger_status = BMI2_GET_BITS(feat_config[idx], BMI2_G_TRIGGER_STAT); } } else @@ -19405,56 +10095,41 @@ static int8_t get_ois_output(struct bmi2_ois_output *ois_output, struct bmi2_dev } /*! - * @brief This internal API is used to enable/disable free-fall detection feature. + * @brief This internal API is used to extract the output feature configuration + * details from the look-up table. */ -static int8_t set_free_fall_det(uint8_t enable, struct bmi2_dev *dev) +static uint8_t extract_output_feat_config(struct bmi2_feature_config *feat_output, + uint8_t type, + const struct bmi2_dev *dev) { - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; + /* Variable to define loop */ + uint8_t loop = 0; /* Variable to set flag */ - uint8_t feat_found; + uint8_t feat_found = BMI2_FALSE; - /* Initialize feature configuration for free-fall detection */ - struct bmi2_feature_config freefall_config = { 0, 0, 0 }; - - /* Search for free-fall detection feature and extract its configuration details */ - feat_found = extract_input_feat_config(&freefall_config, BMI2_FREE_FALL_DET, dev); - if (feat_found) + /* Search for the output feature from the output configuration array */ + while (loop < dev->out_sens) { - /* Get the configuration from the page where free-fall detection feature resides */ - rslt = get_feat_config(freefall_config.page, feat_config, dev); - if (rslt == BMI2_OK) + if (dev->feat_output[loop].type == type) { - /* Assign the offset address for free-fall detection */ - idx = freefall_config.start_addr; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], FREE_FALL_DET_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + *feat_output = dev->feat_output[loop]; + feat_found = BMI2_TRUE; + break; } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; + + loop++; } - return rslt; + /* Return flag */ + return feat_found; } /*! - * @brief This internal API sets free-fall detection configurations like - * free-fall accel settings, and output configuration. + * @brief This internal API gets the cross sensitivity coefficient between + * gyroscope's X and Z axes. */ -static int8_t set_free_fall_det_config(const struct bmi2_free_fall_det_config *config, struct bmi2_dev *dev) +static int8_t get_gyro_cross_sense(int16_t *cross_sense, struct bmi2_dev *dev) { /* Variable to define error */ int8_t rslt; @@ -19462,154 +10137,202 @@ static int8_t set_free_fall_det_config(const struct bmi2_free_fall_det_config *c /* Array to define the feature configuration */ uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - /* Variable to define the array offset */ - uint8_t idx = 0; - /* Variable to define index */ - uint8_t i = 0; - - /* Variable to define set accel settings in loop */ - uint8_t indx; + uint8_t idx = 0; /* Variable to set flag */ uint8_t feat_found; - /* Initialize feature configuration for free-fall */ - struct bmi2_feature_config freefall_config = { 0, 0, 0 }; + uint8_t corr_fact_zx; - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; + /* Initialize feature output for gyroscope cross sensitivity */ + struct bmi2_feature_config cross_sense_out_config = { 0, 0, 0 }; - /* Search for free-fall detection feature and extract its configuration details */ - feat_found = extract_input_feat_config(&freefall_config, BMI2_FREE_FALL_DET, dev); - if (feat_found) + if (dev->variant_feature & BMI2_MAXIMUM_FIFO_VARIANT) { - /* Get the configuration from the page where low-g feature resides */ - rslt = get_feat_config(freefall_config.page, feat_config, dev); + /* For maximum_fifo variant fetch the correction factor from GPIO0 */ + rslt = bmi2_get_regs(BMI2_GYR_CAS_GPIO0_ADDR, &corr_fact_zx, 1, dev); if (rslt == BMI2_OK) { - /* Define the offset in bytes for free-fall detection select */ - idx = freefall_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - /* Set output configuration */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), FREE_FALL_OUT_CONF, config->out_conf); - - /* Increment offset by 1 more word to set free-fall detection accel settings 1 configuration */ - idx++; - - /* Set the free-fall accel param settings */ - for (indx = 0; indx < BMI2_FREE_FALL_ACCEL_SET_PARAMS; indx++) + /* Get the gyroscope cross sensitivity coefficient */ + if (corr_fact_zx & BMI2_GYRO_CROSS_AXES_SENSE_SIGN_BIT_MASK) { - /* Set free-fall detection accel settings */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), - FREE_FALL_ACCEL_SETT, - config->freefall_accel_settings[indx]); - - /* Increment offset by 1 more word to set free-fall detection accel settings configuration */ - idx++; + *cross_sense = (int16_t)(((int16_t)corr_fact_zx) - 128); } - - /* Get total length in bytes to copy from local pointer to the array */ - idx = (uint8_t)(idx * 2) - freefall_config.start_addr; - - /* Copy the bytes to be set back to the array */ - for (i = 0; i < idx; i++) + else { - feat_config[freefall_config.start_addr + i] = *((uint8_t *) data_p + freefall_config.start_addr + i); - } - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - if (rslt == BMI2_OK) - { - /* Copy out_conf value to a local copy in device structure */ - dev->int_map.freefall_out_conf = (uint8_t) config->out_conf; + *cross_sense = (int16_t)(corr_fact_zx); } } } else { - rslt = BMI2_E_INVALID_SENSOR; + /* Search for gyroscope cross sensitivity feature and extract its configuration details */ + feat_found = extract_output_feat_config(&cross_sense_out_config, BMI2_GYRO_CROSS_SENSE, dev); + if (feat_found) + { + /* Get the feature output configuration for gyroscope cross sensitivity + * feature */ + rslt = bmi2_get_feat_config(cross_sense_out_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for gyroscope cross sensitivity output */ + idx = cross_sense_out_config.start_addr; + + /* discard the MSB as GYR_CAS is of only 7 bit */ + feat_config[idx] = feat_config[idx] & BMI2_GYRO_CROSS_AXES_SENSE_MASK; + + /* Get the gyroscope cross sensitivity coefficient */ + if (feat_config[idx] & BMI2_GYRO_CROSS_AXES_SENSE_SIGN_BIT_MASK) + { + *cross_sense = (int16_t)(((int16_t)feat_config[idx]) - 128); + } + else + { + *cross_sense = (int16_t)(feat_config[idx]); + } + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } } return rslt; } /*! - * @brief This internal API gets free-fall detection configurations like - * free-fall detection accel settings, and output configuration. + * @brief This internal API selects the sensor/features to be enabled or + * disabled. */ -static int8_t get_free_fall_det_config(struct bmi2_free_fall_det_config *config, struct bmi2_dev *dev) +static int8_t select_sensor(const uint8_t *sens_list, uint8_t n_sens, uint64_t *sensor_sel) { /* Variable to define error */ - int8_t rslt; + int8_t rslt = BMI2_OK; - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + /* Variable to define loop */ + uint8_t count; - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define get accel settings in loop */ - uint8_t indx; - - /* Variable to define LSB */ - uint16_t lsb = 0; - - /* Variable to define MSB */ - uint16_t msb = 0; - - /* Variable to define a word */ - uint16_t lsb_msb = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for free-fall detection */ - struct bmi2_feature_config freefall_config = { 0, 0, 0 }; - - /* Search for free-fall detection feature and extract its configuration details */ - feat_found = extract_input_feat_config(&freefall_config, BMI2_FREE_FALL_DET, dev); - if (feat_found) + for (count = 0; count < n_sens; count++) { - /* Get the configuration from the page where free-fall detection feature resides */ - rslt = get_feat_config(freefall_config.page, feat_config, dev); - if (rslt == BMI2_OK) + switch (sens_list[count]) { - /* Define the offset in bytes for free-fall detection select */ - idx = freefall_config.start_addr; - - /* Get word to calculate out conf */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get out conf */ - config->out_conf = (lsb_msb & FREE_FALL_OUT_CONF_MASK) >> FREE_FALL_OUT_CONF_POS; - - /* Get the free-fall accel param settings */ - for (indx = 0; indx < BMI2_FREE_FALL_ACCEL_SET_PARAMS; indx++) - { - /* Get word to calculate free-fall detection accel settings */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get free-fall detection accel settings */ - config->freefall_accel_settings[indx] = lsb_msb & FREE_FALL_ACCEL_SETT_MASK; - } - - /* Copy out_conf value to a local copy in device structure */ - dev->int_map.freefall_out_conf = (uint8_t) config->out_conf; + case BMI2_ACCEL: + *sensor_sel |= BMI2_ACCEL_SENS_SEL; + break; + case BMI2_GYRO: + *sensor_sel |= BMI2_GYRO_SENS_SEL; + break; + case BMI2_AUX: + *sensor_sel |= BMI2_AUX_SENS_SEL; + break; + case BMI2_TEMP: + *sensor_sel |= BMI2_TEMP_SENS_SEL; + break; + default: + rslt = BMI2_E_INVALID_SENSOR; + break; } } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } return rslt; } + +/*! + * @brief This internal API enables the selected sensor/features. + */ +static int8_t sensor_enable(uint64_t sensor_sel, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store register values */ + uint8_t reg_data = 0; + + rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + /* Enable accelerometer */ + if (sensor_sel & BMI2_ACCEL_SENS_SEL) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_ACC_EN, BMI2_ENABLE); + } + + /* Enable gyroscope */ + if (sensor_sel & BMI2_GYRO_SENS_SEL) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_EN, BMI2_ENABLE); + } + + /* Enable auxiliary sensor */ + if (sensor_sel & BMI2_AUX_SENS_SEL) + { + reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_AUX_EN, BMI2_ENABLE); + } + + /* Enable temperature sensor */ + if (sensor_sel & BMI2_TEMP_SENS_SEL) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_TEMP_EN, BMI2_ENABLE); + } + + /* Enable the sensors that are set in the power control register */ + if (sensor_sel & BMI2_MAIN_SENSORS) + { + rslt = bmi2_set_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + } + } + + return rslt; +} + +/*! + * @brief This internal API disables the selected sensors/features. + */ +static int8_t sensor_disable(uint64_t sensor_sel, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store register values */ + uint8_t reg_data = 0; + + rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + /* Disable accelerometer */ + if (sensor_sel & BMI2_ACCEL_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_ACC_EN); + } + + /* Disable gyroscope */ + if (sensor_sel & BMI2_GYRO_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_GYR_EN); + } + + /* Disable auxiliary sensor */ + if (sensor_sel & BMI2_AUX_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_AUX_EN); + } + + /* Disable temperature sensor */ + if (sensor_sel & BMI2_TEMP_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_TEMP_EN); + } + + /* Enable the sensors that are set in the power control register */ + if (sensor_sel & BMI2_MAIN_SENSORS) + { + rslt = bmi2_set_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + } + } + + return rslt; +} + +/*! @endcond */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2.h b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2.h index 963df0e44..b1fe627d6 100644 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2.h +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2.h @@ -30,11 +30,23 @@ * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * -* @file bmi2.h -* @date 2020-01-10 -* @version v2.46.1 +* @file bmi2.h +* @date 2020-11-04 +* @version v2.63.1 * -*/#ifndef BMI2_H_ +*/ + +/*! + * @defgroup bmi2xy BMI2XY + */ + +/** + * \ingroup bmi2xy + * \defgroup bmi2 BMI2 + * @brief Sensor driver for BMI2 sensor + */ + +#ifndef BMI2_H_ #define BMI2_H_ /*! CPP guard */ @@ -53,24 +65,43 @@ extern "C" { /*! BMI2XY User Interface function prototypes ****************************************************************************/ +/** + * \ingroup bmi2 + * \defgroup bmi2ApiInit Initialization + * @brief Initialize the sensor and device structure + */ + /*! - * @brief This API is the entry point for bmi2 sensor. It selects between + * \ingroup bmi2ApiInit + * \page bmi2_api_bmi2_sec_init bmi2_sec_init + * \code + * int8_t bmi2_sec_init(struct bmi2_dev *dev); + * \endcode + * @details This API is the entry point for bmi2 sensor. It selects between * I2C/SPI interface, based on user selection. It also reads the chip-id of * the sensor. * * @param[in,out] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_DEV_NOT_FOUND - Invalid device + * @retval 0 -> Success + * @retval < 0 -> Fail */ int8_t bmi2_sec_init(struct bmi2_dev *dev); +/** + * \ingroup bmi2 + * \defgroup bmi2ApiRegs Registers + * @brief Set / Get data from the given register address of the sensor + */ + /*! - * @brief This API reads the data from the given register address of bmi2 + * \ingroup bmi2ApiRegs + * \page bmi2_api_bmi2_get_regs bmi2_get_regs + * \code + * int8_t bmi2_get_regs(uint8_t reg_addr, uint8_t *data, uint16_t len, const struct bmi2_dev *dev); + * \endcode + * @details This API reads the data from the given register address of bmi2 * sensor. * * @param[in] reg_addr : Register address from which data is read. @@ -83,15 +114,18 @@ int8_t bmi2_sec_init(struct bmi2_dev *dev); * Register address - 0x26, 0x5E. * * @return Result of API execution status - * - * @retval BMI2_OK - Success - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ -int8_t bmi2_get_regs(uint8_t reg_addr, uint8_t *data, uint16_t len, const struct bmi2_dev *dev); +int8_t bmi2_get_regs(uint8_t reg_addr, uint8_t *data, uint16_t len, struct bmi2_dev *dev); /*! - * @brief This API writes data to the given register address of bmi2 sensor. + * \ingroup bmi2ApiRegs + * \page bmi2_api_bmi2_set_regs bmi2_set_regs + * \code + * int8_t bmi2_set_regs(uint8_t reg_addr, const uint8_t *data, uint16_t len, struct bmi2_dev *dev); + * \endcode + * @details This API writes data to the given register address of bmi2 sensor. * * @param[in] reg_addr : Register address to which the data is written. * @param[in] data : Pointer to data buffer in which data to be written @@ -100,15 +134,24 @@ int8_t bmi2_get_regs(uint8_t reg_addr, uint8_t *data, uint16_t len, const struct * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ int8_t bmi2_set_regs(uint8_t reg_addr, const uint8_t *data, uint16_t len, struct bmi2_dev *dev); +/** + * \ingroup bmi2 + * \defgroup bmi2ApiSR Soft reset + * @brief Set / Get data from the given register address of the sensor + */ + /*! - * @brief This API resets bmi2 sensor. All registers are overwritten with + * \ingroup bmi2ApiSR + * \page bmi2_api_bmi2_soft_reset bmi2_soft_reset + * \code + * int8_t bmi2_soft_reset(struct bmi2_dev *dev); + * \endcode + * @details This API resets bmi2 sensor. All registers are overwritten with * their default values. * * @note If selected interface is SPI, an extra dummy byte is read to bring the @@ -117,277 +160,82 @@ int8_t bmi2_set_regs(uint8_t reg_addr, const uint8_t *data, uint16_t len, struct * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ int8_t bmi2_soft_reset(struct bmi2_dev *dev); +/** + * \ingroup bmi2 + * \defgroup bmi2ApiConfig Configuration + * @brief Functions related to configuration of the sensor + */ + /*! - * @brief This API is used to get the config file major and minor information. + * \ingroup bmi2ApiConfig + * \page bmi2_api_bmi2_get_config_file_version bmi2_get_config_file_version + * \code + * int8_t bmi2_get_config_file_version(uint8_t *config_major, uint8_t *config_minor, struct bmi2_dev *dev); + * \endcode + * @details This API is used to get the config file major and minor information. * * @param[in] dev : Structure instance of bmi2_dev. * @param[out] config_major : pointer to data buffer to store the config major. * @param[out] config_minor : pointer to data buffer to store the config minor. * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail - */ -int8_t bmi2_get_config_file_version(uint16_t *config_major, uint8_t *config_minor, struct bmi2_dev *dev); - -/*! - * @brief This API selects the sensors/features to be enabled. - * - * @param[in] sens_list : Pointer to select the sensor/feature. - * @param[in] n_sens : Number of sensors selected. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @note Sensors/features that can be enabled. - * - * sens_list | Values - * -------------------------|----------- - * BMI2_ACCEL | 0 - * BMI2_GYRO | 1 - * BMI2_AUX | 2 - * BMI2_TEMP | 3 - * BMI2_ANY_MOTION | 4 - * BMI2_NO_MOTION | 5 - * BMI2_TILT | 6 - * BMI2_ORIENTATION | 7 - * BMI2_SIG_MOTION | 8 - * BMI2_STEP_DETECTOR | 9 - * BMI2_STEP_COUNTER | 10 - * BMI2_STEP_ACTIVITY | 11 - * BMI2_GYRO_GAIN_UPDATE | 12 - * BMI2_UP_HOLD_TO_WAKE | 13 - * BMI2_GLANCE_DETECTOR | 14 - * BMI2_WAKE_UP | 15 - * BMI2_HIGH_G | 16 - * BMI2_LOW_G | 17 - * BMI2_FLAT | 18 - * BMI2_EXT_SENS_SYNC | 19 - * BMI2_WRIST_GESTURE | 20 - * BMI2_WRIST_WEAR_WAKE_UP | 21 - * BMI2_ACTIVITY_RECOGNITION| 22 - * - * @example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO}; - * uint8_t n_sens = 2; - * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_get_config_file_version(uint8_t *config_major, uint8_t *config_minor, struct bmi2_dev *dev); + +/** + * \ingroup bmi2 + * \defgroup bmi2ApiPowersave Advanced power save mode + * @brief Set / Get Advanced power save mode of the sensor */ -int8_t bmi2_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); /*! - * @brief This API selects the sensors/features to be disabled. - * - * @param[in] sens_list : Pointer to select the sensor/feature. - * @param[in] n_sens : Number of sensors selected. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @note Sensors/features that can be disabled. - * - * sens_list | Values - * -------------------------|----------- - * BMI2_ACCEL | 0 - * BMI2_GYRO | 1 - * BMI2_AUX | 2 - * BMI2_TEMP | 3 - * BMI2_ANY_MOTION | 4 - * BMI2_NO_MOTION | 5 - * BMI2_TILT | 6 - * BMI2_ORIENTATION | 7 - * BMI2_SIG_MOTION | 8 - * BMI2_STEP_DETECTOR | 9 - * BMI2_STEP_COUNTER | 10 - * BMI2_STEP_ACTIVITY | 11 - * BMI2_GYRO_GAIN_UPDATE | 12 - * BMI2_UP_HOLD_TO_WAKE | 13 - * BMI2_GLANCE_DETECTOR | 14 - * BMI2_WAKE_UP | 15 - * BMI2_HIGH_G | 16 - * BMI2_LOW_G | 17 - * BMI2_FLAT | 18 - * BMI2_EXT_SENS_SYNC | 19 - * BMI2_WRIST_GESTURE | 20 - * BMI2_WRIST_WEAR_WAKE_UP | 21 - * BMI2_ACTIVITY_RECOGNITION| 22 - * - * @example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO}; - * uint8_t n_sens = 2; - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - */ -int8_t bmi2_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); - -/*! - * @brief This API sets the sensor/feature configuration. - * - * @param[in] sens_cfg : Structure instance of bmi2_sens_config. - * @param[in] n_sens : Number of sensors selected. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @note Sensors/features that can be configured - * - * sens_list | Values - * -------------------------|----------- - * BMI2_ACCEL | 0 - * BMI2_GYRO | 1 - * BMI2_AUX | 2 - * BMI2_TEMP | 3 - * BMI2_ANY_MOTION | 4 - * BMI2_NO_MOTION | 5 - * BMI2_TILT | 6 - * BMI2_ORIENTATION | 7 - * BMI2_SIG_MOTION | 8 - * BMI2_STEP_DETECTOR | 9 - * BMI2_STEP_COUNTER | 10 - * BMI2_STEP_ACTIVITY | 11 - * BMI2_GYRO_GAIN_UPDATE | 12 - * BMI2_UP_HOLD_TO_WAKE | 13 - * BMI2_GLANCE_DETECTOR | 14 - * BMI2_WAKE_UP | 15 - * BMI2_HIGH_G | 16 - * BMI2_LOW_G | 17 - * BMI2_FLAT | 18 - * BMI2_EXT_SENS_SYNC | 19 - * BMI2_WRIST_GESTURE | 21 - * BMI2_WRIST_WEAR_WAKE_UP | 22 - * BMI2_STEP_COUNTER_PARAMS | 25 - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_SET_APS_FAIL - Error: Set Advance Power Save Fail - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -int8_t bmi2_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); - -/*! - * @brief This API gets the sensor/feature configuration. - * - * @param[in] sens_cfg : Structure instance of bmi2_sens_config. - * @param[in] n_sens : Number of sensors selected. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @note Sensors/features whose configurations can be read. - * - * sens_list | Values - * -------------------------|----------- - * BMI2_ACCEL | 0 - * BMI2_GYRO | 1 - * BMI2_AUX | 2 - * BMI2_TEMP | 3 - * BMI2_ANY_MOTION | 4 - * BMI2_NO_MOTION | 5 - * BMI2_TILT | 6 - * BMI2_ORIENTATION | 7 - * BMI2_SIG_MOTION | 8 - * BMI2_STEP_DETECTOR | 9 - * BMI2_STEP_COUNTER | 10 - * BMI2_STEP_ACTIVITY | 11 - * BMI2_GYRO_GAIN_UPDATE | 12 - * BMI2_UP_HOLD_TO_WAKE | 13 - * BMI2_GLANCE_DETECTOR | 14 - * BMI2_WAKE_UP | 15 - * BMI2_HIGH_G | 16 - * BMI2_LOW_G | 17 - * BMI2_FLAT | 18 - * BMI2_EXT_SENS_SYNC | 19 - * BMI2_WRIST_GESTURE | 21 - * BMI2_WRIST_WEAR_WAKE_UP | 22 - * BMI2_STEP_COUNTER_PARAMS | 25 - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_SET_APS_FAIL - Error: Set Advance Power Save Fail - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -int8_t bmi2_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); - -/*! - * @brief This API gets the sensor/feature data for accelerometer, gyroscope, - * auxiliary sensor, step counter, high-g, gyroscope user-gain update, - * orientation, gyroscope cross sensitivity and error status for NVM and VFRM. - * - * @param[out] sensor_data : Structure instance of bmi2_sensor_data. - * @param[in] n_sens : Number of sensors selected. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @note Sensors/features whose data can be read - * - * sens_list | Values - * ---------------------|----------- - * BMI2_ACCEL | 0 - * BMI2_GYRO | 1 - * BMI2_AUX | 2 - * BMI2_ORIENTATION | 7 - * BMI2_STEP_COUNTER | 10 - * BMI2_GYRO_GAIN_UPDATE| 12 - * BMI2_HIGH_G | 16 - * BMI2_NVM_STATUS | 26 - * BMI2_VFRM_STATUS | 27 - * BMI2_GYRO_CROSS_SENSE| 28 - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_SET_APS_FAIL - Error: Set Advance Power Save Fail - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - */ -int8_t bmi2_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev); - -/*! - * @brief This API enables/disables the advance power save mode in the sensor. + * \ingroup bmi2ApiPowersave + * \page bmi2_api_bmi2_set_adv_power_save bmi2_set_adv_power_save + * \code + * int8_t bmi2_set_adv_power_save(uint8_t enable, struct bmi2_dev *dev); + * \endcode + * @details This API enables/disables the advance power save mode in the sensor. * * @param[in] enable : To enable/disable advance power mode. * @param[in] dev : Structure instance of bmi2_dev. * + *@verbatim * Enable | Description * -------------|--------------- * BMI2_DISABLE | Disables advance power save. * BMI2_ENABLE | Enables advance power save. + *@endverbatim * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_SET_APS_FAIL - Error: Set Advance Power Save Fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ int8_t bmi2_set_adv_power_save(uint8_t enable, struct bmi2_dev *dev); /*! - * @brief This API gets the status of advance power save mode in the sensor. + * \ingroup bmi2ApiPowersave + * \page bmi2_api_bmi2_get_adv_power_save bmi2_get_adv_power_save + * \code + * int8_t bmi2_get_adv_power_save(uint8_t *aps_status, struct bmi2_dev *dev); + * \endcode + * @details This API gets the status of advance power save mode in the sensor. * * @param[out] aps_status : Pointer to get the status of APS mode. * @param[in] dev : Structure instance of bmi2_dev. * + *@verbatim * aps_status | Description * -------------|--------------- * BMI2_DISABLE | Advance power save disabled. * BMI2_ENABLE | Advance power save enabled. + *@endverbatim * * @return Result of API execution status * @@ -399,22 +247,34 @@ int8_t bmi2_set_adv_power_save(uint8_t enable, struct bmi2_dev *dev); int8_t bmi2_get_adv_power_save(uint8_t *aps_status, struct bmi2_dev *dev); /*! - * @brief This API loads the configuration file to the bmi2 sensor. + * \ingroup bmi2ApiConfig + * \page bmi2_api_bmi2_write_config_file bmi2_write_config_file + * \code + * int8_t bmi2_write_config_file(struct bmi2_dev *dev); + * \endcode + * @details This API loads the configuration file to the bmi2 sensor. * * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_SET_APS_FAIL - Error: Set Advance Power Save Fail - * @retval BMI2_E_CONFIG_LOAD - Configuration load fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ int8_t bmi2_write_config_file(struct bmi2_dev *dev); +/** + * \ingroup bmi2 + * \defgroup bmi2ApiInt Interrupt + * @brief Interrupt operations of the sensor + */ + /*! - * @brief This API sets: + * \ingroup bmi2ApiInt + * \page bmi2_api_bmi2_set_int_pin_config bmi2_set_int_pin_config + * \code + * int8_t bmi2_set_int_pin_config(const struct bmi2_int_pin_config *int_cfg, struct bmi2_dev *dev); + * \endcode + * @details This API sets: * 1) The input output configuration of the selected interrupt pin: * INT1 or INT2. * 2) The interrupt mode: permanently latched or non-latched. @@ -423,16 +283,18 @@ int8_t bmi2_write_config_file(struct bmi2_dev *dev); * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_INT_PIN - Error: Invalid interrupt pin + * @retval 0 -> Success + * @retval < 0 -> Fail */ int8_t bmi2_set_int_pin_config(const struct bmi2_int_pin_config *int_cfg, struct bmi2_dev *dev); /*! - * @brief This API gets: + * \ingroup bmi2ApiInt + * \page bmi2_api_bmi2_get_int_pin_config bmi2_get_int_pin_config + * \code + * int8_t bmi2_get_int_pin_config(struct bmi2_int_pin_config *int_cfg, const struct bmi2_dev *dev); + * \endcode + * @details This API gets: * 1) The input output configuration of the selected interrupt pin: * INT1 or INT2. * 2) The interrupt mode: permanently latched or non-latched. @@ -441,21 +303,24 @@ int8_t bmi2_set_int_pin_config(const struct bmi2_int_pin_config *int_cfg, struct * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_INT_PIN - Error: Invalid interrupt pin + * @retval 0 -> Success + * @retval < 0 -> Fail */ -int8_t bmi2_get_int_pin_config(struct bmi2_int_pin_config *int_cfg, const struct bmi2_dev *dev); +int8_t bmi2_get_int_pin_config(struct bmi2_int_pin_config *int_cfg, struct bmi2_dev *dev); /*! - * @brief This API gets the interrupt status of both feature and data + * \ingroup bmi2ApiInt + * \page bmi2_api_bmi2_get_int_status bmi2_get_int_status + * \code + * int8_t bmi2_get_int_status(uint16_t *int_status, const struct bmi2_dev *dev); + * \endcode + * @details This API gets the interrupt status of both feature and data * interrupts. * * @param[out] int_status : Pointer to get the status of the interrupts. * @param[in] dev : Structure instance of bmi2_dev. * + *@verbatim * int_status | Status * -----------|------------ * 0x00 | BIT0 @@ -466,67 +331,243 @@ int8_t bmi2_get_int_pin_config(struct bmi2_int_pin_config *int_cfg, const struct * 0x05 | BIT5 * 0x06 | BIT6 * 0x07 | BIT7 + *@endverbatim * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_get_int_status(uint16_t *int_status, struct bmi2_dev *dev); + +/** + * \ingroup bmi2 + * \defgroup bmi2ApiSensorC Sensor Configuration + * @brief Enable / Disable feature configuration of the sensor */ -int8_t bmi2_get_int_status(uint16_t *int_status, const struct bmi2_dev *dev); /*! - * @brief This API sets the FIFO configuration in the sensor. + * \ingroup bmi2ApiSensorC + * \page bmi2_api_bmi2_set_sensor_config bmi2_set_sensor_config + * \code + * int8_t bmi2_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API sets the sensor/feature configuration. + * + * @param[in] sens_cfg : Structure instance of bmi2_sens_config. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features that can be configured + * + *@verbatim + * sens_list | Values + * ----------------------------|----------- + * BMI2_ACCEL | 0 + * BMI2_GYRO | 1 + * BMI2_AUX | 2 + * BMI2_GYRO_GAIN_UPDATE | 9 + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + +/*! + * \ingroup bmi2ApiSensorC + * \page bmi2_api_bmi2_get_sensor_config bmi2_get_sensor_config + * \code + * int8_t bmi2_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API gets the sensor/feature configuration. + * + * @param[in] sens_cfg : Structure instance of bmi2_sens_config. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features whose configurations can be read. + * + *@verbatim + * sens_list | Values + * -------------------------|----------- + * BMI2_ACCEL | 0 + * BMI2_GYRO | 1 + * BMI2_AUX | 2 + * BMI2_GYRO_GAIN_UPDATE | 9 + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + +/** + * \ingroup bmi2 + * \defgroup bmi2ApiSensor Feature Set + * @brief Enable / Disable features of the sensor + */ + +/*! + * \ingroup bmi2ApiSensor + * \page bmi2_api_bmi2_sensor_enable bmi2_sensor_enable + * \code + * int8_t bmi2_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API selects the sensors/features to be enabled. + * + * @param[in] sens_list : Pointer to select the sensor/feature. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features that can be enabled. + * + *@verbatim + * sens_list | Values + * -------------------------|----------- + * BMI2_ACCEL | 0 + * BMI2_GYRO | 1 + * BMI2_AUX | 2 + * BMI2_TEMP | 31 + *@endverbatim + * + * @note : + * example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO}; + * uint8_t n_sens = 2; + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + +/*! + * \ingroup bmi2ApiSensor + * \page bmi2_api_bmi2_sensor_disable bmi2_sensor_disable + * \code + * int8_t bmi2_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API selects the sensors/features to be disabled. + * + * @param[in] sens_list : Pointer to select the sensor/feature. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features that can be disabled. + * + *@verbatim + * sens_list | Values + * ----------------------------|----------- + * BMI2_ACCEL | 0 + * BMI2_GYRO | 1 + * BMI2_AUX | 2 + * BMI2_TEMP | 31 + *@endverbatim + * + * @note : + * example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO}; + * uint8_t n_sens = 2; + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + +/** + * \ingroup bmi2 + * \defgroup bmi2ApiSensorD Sensor Data + * @brief Get sensor data + */ + +/*! + * \ingroup bmi2ApiSensorD + * \page bmi2_api_bmi2_get_sensor_data bmi2_get_sensor_data + * \code + * int8_t bmi2_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API gets the sensor/feature data for accelerometer, gyroscope, + * auxiliary sensor, step counter, high-g, gyroscope user-gain update, + * orientation, gyroscope cross sensitivity and error status for NVM and VFRM. + * + * @param[out] sensor_data : Structure instance of bmi2_sensor_data. + * @param[in] n_sens : Number of sensors selected. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features whose data can be read + * + *@verbatim + * sens_list | Values + * ---------------------|----------- + * BMI2_ACCEL | 0 + * BMI2_GYRO | 1 + * BMI2_AUX | 2 + * BMI2_GYRO_GAIN_UPDATE| 12 + * BMI2_GYRO_CROSS_SENSE| 28 + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev); + +/** + * \ingroup bmi2 + * \defgroup bmi2ApiFIFO FIFO + * @brief FIFO operations of the sensor + */ + +/*! + * \ingroup bmi2ApiFIFO + * \page bmi2_api_bmi2_set_fifo_config bmi2_set_fifo_config + * \code + * int8_t bmi2_set_fifo_config(uint16_t config, uint8_t enable, struct bmi2_dev *dev); + * \endcode + * @details This API sets the FIFO configuration in the sensor. * * @param[in] config : FIFO configurations to be enabled/disabled. * @param[in] enable : Enable/Disable FIFO configurations. * @param[in] dev : Structure instance of bmi2_dev. * + *@verbatim * enable | Description * -------------|--------------- * BMI2_DISABLE | Disables FIFO configuration. * BMI2_ENABLE | Enables FIFO configuration. + *@endverbatim * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ int8_t bmi2_set_fifo_config(uint16_t config, uint8_t enable, struct bmi2_dev *dev); /*! - * @brief This API gets the FIFO configuration from the sensor. + * \ingroup bmi2ApiFIFO + * \page bmi2_api_bmi2_get_fifo_config bmi2_get_fifo_config + * \code + * int8_t bmi2_get_fifo_config(uint16_t *fifo_config, const struct bmi2_dev *dev); + * \endcode + * @details This API gets the FIFO configuration from the sensor. * * @param[out] fifo_config : Pointer variable to get FIFO configuration value. * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ -int8_t bmi2_get_fifo_config(uint16_t *fifo_config, const struct bmi2_dev *dev); +int8_t bmi2_get_fifo_config(uint16_t *fifo_config, struct bmi2_dev *dev); /*! - * @ brief This api is used to enable the gyro self-test and crt. - * *gyro_self_test_crt -> 0 then gyro self test enable - * *gyro_self_test_crt -> 1 then CRT enable - * - * @param[in] gyro_self_test_crt : enable the gyro self test or crt. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail - */ -int8_t bmi2_set_gyro_self_test_crt(uint8_t *gyro_self_test_crt, struct bmi2_dev *dev); - -/*! - * @brief This API reads FIFO data. + * \ingroup bmi2ApiFIFO + * \page bmi2_api_bmi2_read_fifo_data bmi2_read_fifo_data + * \code + * int8_t bmi2_read_fifo_data(struct bmi2_fifo_frame *fifo, const struct bmi2_dev *dev); + * \endcode + * @details This API reads FIFO data. * * @param[in, out] fifo : Structure instance of bmi2_fifo_frame. * @param[in] dev : Structure instance of bmi2_dev. @@ -534,15 +575,21 @@ int8_t bmi2_set_gyro_self_test_crt(uint8_t *gyro_self_test_crt, struct bmi2_dev * @note APS has to be disabled before calling this function. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ -int8_t bmi2_read_fifo_data(struct bmi2_fifo_frame *fifo, const struct bmi2_dev *dev); +int8_t bmi2_read_fifo_data(struct bmi2_fifo_frame *fifo, struct bmi2_dev *dev); /*! - * This API parses and extracts the accelerometer frames from FIFO data read by + * \ingroup bmi2ApiFIFO + * \page bmi2_api_bmi2_extract_accel bmi2_extract_accel + * \code + * int8_t bmi2_extract_accel(struct bmi2_sens_axes_data *accel_data, + * uint16_t *accel_length, + * struct bmi2_fifo_frame *fifo, + * const struct bmi2_dev *dev); + * \endcode + * @details This API parses and extracts the accelerometer frames from FIFO data read by * the "bmi2_read_fifo_data" API and stores it in the "accel_data" structure * instance. * @@ -553,11 +600,8 @@ int8_t bmi2_read_fifo_data(struct bmi2_fifo_frame *fifo, const struct bmi2_dev * * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_W_FIFO_EMPTY - Warning : FIFO is empty - * @retval BMI2_W_PARTIAL_READ - Warning : There are more frames to be read + * @retval 0 -> Success + * @retval < 0 -> Fail */ int8_t bmi2_extract_accel(struct bmi2_sens_axes_data *accel_data, uint16_t *accel_length, @@ -565,7 +609,16 @@ int8_t bmi2_extract_accel(struct bmi2_sens_axes_data *accel_data, const struct bmi2_dev *dev); /*! - * @brief This API parses and extracts the auxiliary frames from FIFO data + * \ingroup bmi2ApiFIFO + * \page bmi2_api_bmi2_extract_aux bmi2_extract_aux + * \code + * int8_t bmi2_extract_aux(struct bmi2_aux_fifo_data *aux, + * uint16_t *aux_length, + * struct bmi2_fifo_frame *fifo, + * const struct bmi2_dev *dev); + * + * \endcode + * @details This API parses and extracts the auxiliary frames from FIFO data * read by the "bmi2_read_fifo_data" API and stores it in "aux_data" buffer. * * @param[out] aux : Pointer to structure where the parsed auxiliary @@ -575,11 +628,8 @@ int8_t bmi2_extract_accel(struct bmi2_sens_axes_data *accel_data, * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_W_FIFO_EMPTY - Warning : FIFO is empty - * @retval BMI2_W_PARTIAL_READ - Warning : There are more frames to be read + * @retval 0 -> Success + * @retval < 0 -> Fail */ int8_t bmi2_extract_aux(struct bmi2_aux_fifo_data *aux, uint16_t *aux_length, @@ -587,7 +637,15 @@ int8_t bmi2_extract_aux(struct bmi2_aux_fifo_data *aux, const struct bmi2_dev *dev); /*! - * This API parses and extracts the gyroscope frames from FIFO data read by the + * \ingroup bmi2ApiFIFO + * \page bmi2_api_bmi2_extract_gyro bmi2_extract_gyro + * \code + * int8_t bmi2_extract_gyro(struct bmi2_sens_axes_data *gyro_data, + * uint16_t *gyro_length, + * struct bmi2_fifo_frame *fifo, + * const struct bmi2_dev *dev); + * \endcode + * @details This API parses and extracts the gyroscope frames from FIFO data read by the * "bmi2_read_fifo_data" API and stores it in the "gyro_data" * structure instance. * @@ -598,106 +656,135 @@ int8_t bmi2_extract_aux(struct bmi2_aux_fifo_data *aux, * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_W_FIFO_EMPTY - Warning : FIFO is empty - * @retval BMI2_W_PARTIAL_READ - Warning : There are more frames to be read + * @retval 0 -> Success + * @retval < 0 -> Fail */ int8_t bmi2_extract_gyro(struct bmi2_sens_axes_data *gyro_data, uint16_t *gyro_length, struct bmi2_fifo_frame *fifo, const struct bmi2_dev *dev); +/** + * \ingroup bmi2 + * \defgroup bmi2ApiCmd Command Register + * @brief Write commands to the sensor + */ + /*! - * @brief This API writes the available sensor specific commands to the sensor. + * \ingroup bmi2ApiCmd + * \page bmi2_api_bmi2_set_command_register bmi2_set_command_register + * \code + * int8_t bmi2_set_command_register(uint8_t command, struct bmi2_dev *dev); + * \endcode + * @details This API writes the available sensor specific commands to the sensor. * * @param[in] command : Commands to be given to the sensor. * @param[in] dev : Structure instance of bmi2_dev. * + *@verbatim * Commands | Values * ---------------------|--------------------- * BMI2_SOFT_RESET_CMD | 0xB6 * BMI2_FIFO_FLUSH_CMD | 0xB0 * BMI2_USR_GAIN_CMD | 0x03 + *@endverbatim * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ int8_t bmi2_set_command_register(uint8_t command, struct bmi2_dev *dev); /*! - * @brief This API sets the FIFO self wake up functionality in the sensor. + * \ingroup bmi2ApiFIFO + * \page bmi2_api_bmi2_set_fifo_self_wake_up bmi2_set_fifo_self_wake_up + * \code + * int8_t bmi2_set_fifo_self_wake_up(uint8_t fifo_self_wake_up, struct bmi2_dev *dev); + * \endcode + * @details This API sets the FIFO self wake up functionality in the sensor. * * @param[in] fifo_self_wake_up : Variable to set FIFO self wake-up. * @param[in] dev : Structure instance of bmi2_dev. * + *@verbatim * fifo_self_wake_up | Description * -------------------|--------------- * BMI2_DISABLE | Disables self wake-up. * BMI2_ENABLE | Enables self wake-up. + *@endverbatim * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ int8_t bmi2_set_fifo_self_wake_up(uint8_t fifo_self_wake_up, struct bmi2_dev *dev); /*! - * @brief This API gets the FIFO self wake up functionality from the sensor. + * \ingroup bmi2ApiFIFO + * \page bmi2_api_bmi2_get_fifo_self_wake_up bmi2_get_fifo_self_wake_up + * \code + * int8_t bmi2_get_fifo_self_wake_up(uint8_t *fifo_self_wake_up, const struct bmi2_dev *dev); + * \endcode + * @details This API gets the FIFO self wake up functionality from the sensor. * * @param[out] fifo_self_wake_up : Pointer variable to get the status of FIFO * self wake-up. * @param[in] dev : Structure instance of bmi2_dev. * + *@verbatim * fifo_self_wake_up | Description * -------------------|--------------- * BMI2_DISABLE | Self wake-up disabled * BMI2_ENABLE | Self wake-up enabled. + *@endverbatim * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ -int8_t bmi2_get_fifo_self_wake_up(uint8_t *fifo_self_wake_up, const struct bmi2_dev *dev); +int8_t bmi2_get_fifo_self_wake_up(uint8_t *fifo_self_wake_up, struct bmi2_dev *dev); /*! - * @brief This API sets the FIFO water mark level which is set in the sensor. + * \ingroup bmi2ApiFIFO + * \page bmi2_api_bmi2_set_fifo_wm bmi2_set_fifo_wm + * \code + * int8_t bmi2_set_fifo_wm(uint16_t fifo_wm, struct bmi2_dev *dev); + * \endcode + * @details This API sets the FIFO water mark level which is set in the sensor. * * @param[in] fifo_wm : Variable to set FIFO water-mark level. * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ int8_t bmi2_set_fifo_wm(uint16_t fifo_wm, struct bmi2_dev *dev); /*! - * @brief This API gets the FIFO water mark level which is set in the sensor. + * \ingroup bmi2ApiFIFO + * \page bmi2_api_bmi2_get_fifo_wm bmi2_get_fifo_wm + * \code + * int8_t bmi2_get_fifo_wm(uint16_t *fifo_wm, const struct bmi2_dev *dev); + * \endcode + * @details This API gets the FIFO water mark level which is set in the sensor. * * @param[out] fifo_wm : Pointer variable to store FIFO water-mark level. * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ -int8_t bmi2_get_fifo_wm(uint16_t *fifo_wm, const struct bmi2_dev *dev); +int8_t bmi2_get_fifo_wm(uint16_t *fifo_wm, struct bmi2_dev *dev); /*! - * @brief This API sets either filtered or un-filtered FIFO accelerometer or + * \ingroup bmi2ApiFIFO + * \page bmi2_api_bmi2_set_fifo_filter_data bmi2_set_fifo_filter_data + * \code + * int8_t bmi2_set_fifo_filter_data(uint8_t sens_sel, uint8_t fifo_filter_data, struct bmi2_dev *dev); + * \endcode + * @details This API sets either filtered or un-filtered FIFO accelerometer or * gyroscope data. * * @param[in] sens_sel : Selects either accelerometer or @@ -705,57 +792,66 @@ int8_t bmi2_get_fifo_wm(uint16_t *fifo_wm, const struct bmi2_dev *dev); * @param[in] fifo_filter_data : Variable to set the filter data. * @param[in] dev : Structure instance of bmi2_dev. * + *@verbatim * sens_sel | values * -----------------|---------- * BMI2_ACCEL | 0x01 * BMI2_GYRO | 0x02 + *@endverbatim * - * + *@verbatim * Value | fifo_filter_data * ---------|--------------------- * 0x00 | Un-filtered data * 0x01 | Filtered data + *@endverbatim * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_OUT_OF_RANGE - Error: Out of range + * @retval 0 -> Success + * @retval < 0 -> Fail */ int8_t bmi2_set_fifo_filter_data(uint8_t sens_sel, uint8_t fifo_filter_data, struct bmi2_dev *dev); /*! - * @brief This API gets the FIFO accelerometer or gyroscope filter data. + * \ingroup bmi2ApiFIFO + * \page bmi2_api_bmi2_get_fifo_filter_data bmi2_get_fifo_filter_data + * \code + * int8_t bmi2_get_fifo_filter_data(uint8_t sens_sel, uint8_t *fifo_filter_data, const struct bmi2_dev *dev); + * \endcode + * @details This API gets the FIFO accelerometer or gyroscope filter data. * * @param[in] sens_sel : Selects either accelerometer or * gyroscope sensor. * @param[out] fifo_filter_data : Pointer variable to get the filter data. * @param[in] dev : Structure instance of bmi2_dev. * + *@verbatim * sens_sel | values * -----------------|---------- * BMI2_ACCEL | 0x01 * BMI2_GYRO | 0x02 + *@endverbatim * - * + *@verbatim * Value | fifo_filter_data * ---------|--------------------- * 0x00 | Un-filtered data * 0x01 | Filtered data + *@endverbatim * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor + * @retval 0 -> Success + * @retval < 0 -> Fail */ -int8_t bmi2_get_fifo_filter_data(uint8_t sens_sel, uint8_t *fifo_filter_data, const struct bmi2_dev *dev); +int8_t bmi2_get_fifo_filter_data(uint8_t sens_sel, uint8_t *fifo_filter_data, struct bmi2_dev *dev); /*! - * @brief This API sets the down sampling rate for FIFO accelerometer or + * \ingroup bmi2ApiFIFO + * \page bmi2_api_bmi2_set_fifo_down_sample bmi2_set_fifo_down_sample + * \code + * int8_t bmi2_set_fifo_down_sample(uint8_t sens_sel, uint8_t fifo_down_samp, struct bmi2_dev *dev); + * \endcode + * @details This API sets the down sampling rate for FIFO accelerometer or * gyroscope FIFO data. * * @param[in] sens_sel : Selects either either accelerometer or @@ -763,23 +859,26 @@ int8_t bmi2_get_fifo_filter_data(uint8_t sens_sel, uint8_t *fifo_filter_data, co * @param[in] fifo_down_samp : Variable to set the down sampling rate. * @param[in] dev : Structure instance of bmi2_dev. * + *@verbatim * sens_sel | values * ----------------|---------- * BMI2_ACCEL | 0x01 * BMI2_GYRO | 0x02 + *@endverbatim * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_OUT_OF_RANGE - Error: Out of range + * @retval 0 -> Success + * @retval < 0 -> Fail */ int8_t bmi2_set_fifo_down_sample(uint8_t sens_sel, uint8_t fifo_down_samp, struct bmi2_dev *dev); /*! - * @brief This API gets the down sampling rate, configured for FIFO + * \ingroup bmi2ApiFIFO + * \page bmi2_api_bmi2_get_fifo_down_sample bmi2_get_fifo_down_sample + * \code + * int8_t bmi2_get_fifo_down_sample(uint8_t sens_sel, uint8_t *fifo_down_samp, const struct bmi2_dev *dev); + * \endcode + * @details This API gets the down sampling rate, configured for FIFO * accelerometer or gyroscope data. * * @param[in] sens_sel : Selects either either accelerometer or @@ -787,22 +886,26 @@ int8_t bmi2_set_fifo_down_sample(uint8_t sens_sel, uint8_t fifo_down_samp, struc * @param[out] fifo_down_samp : Pointer variable to store the down sampling rate * @param[in] dev : Structure instance of bmi2_dev. * + *@verbatim * sens_sel | values * ----------------|---------- * BMI2_ACCEL | 0x01 * BMI2_GYRO | 0x02 + *@endverbatim * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor + * @retval 0 -> Success + * @retval < 0 -> Fail */ -int8_t bmi2_get_fifo_down_sample(uint8_t sens_sel, uint8_t *fifo_down_samp, const struct bmi2_dev *dev); +int8_t bmi2_get_fifo_down_sample(uint8_t sens_sel, uint8_t *fifo_down_samp, struct bmi2_dev *dev); /*! - * @brief This API gets the length of FIFO data available in the sensor in + * \ingroup bmi2ApiFIFO + * \page bmi2_api_bmi2_get_fifo_length bmi2_get_fifo_length + * \code + * int8_t bmi2_get_fifo_length(uint16_t *fifo_length, const struct bmi2_dev *dev); + * \endcode + * @details This API gets the length of FIFO data available in the sensor in * bytes. * * @param[out] fifo_length : Pointer variable to store the value of FIFO byte @@ -813,15 +916,54 @@ int8_t bmi2_get_fifo_down_sample(uint8_t sens_sel, uint8_t *fifo_down_samp, cons * written. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_get_fifo_length(uint16_t *fifo_length, struct bmi2_dev *dev); + +/** + * \ingroup bmi2 + * \defgroup bmi2ApiOIS OIS + * @brief OIS operations of the sensor */ -int8_t bmi2_get_fifo_length(uint16_t *fifo_length, const struct bmi2_dev *dev); /*! - * @brief This API reads the user-defined bytes of data from the given register + * \ingroup bmi2ApiOIS + * \page bmi2_api_bmi2_set_ois_interface bmi2_set_ois_interface + * \code + * int8_t bmi2_set_ois_interface(uint8_t enable, struct bmi2_dev *dev); + * \endcode + * @details This API enables/disables OIS interface. + * + * @param[in] enable : To enable/disable OIS interface. + * @param[in] dev : Structure instance of bmi2_dev. + * + *@verbatim + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables OIS interface. + * BMI2_ENABLE | Enables OIS interface. + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_set_ois_interface(uint8_t enable, struct bmi2_dev *dev); + +/** + * \ingroup bmi2 + * \defgroup bmi2ApiAux Auxiliary sensor + * @brief Auxiliary sensor operations of the sensor + */ + +/*! + * \ingroup bmi2ApiAux + * \page bmi2_api_bmi2_read_aux_man_mode bmi2_read_aux_man_mode + * \code + * int8_t bmi2_read_aux_man_mode(uint8_t reg_addr, uint8_t *aux_data, uint16_t len, struct bmi2_dev *dev); + * \endcode + * @details This API reads the user-defined bytes of data from the given register * address of auxiliary sensor in manual mode. * * @param[in] reg_addr : Address from where data is read. @@ -832,36 +974,18 @@ int8_t bmi2_get_fifo_length(uint16_t *fifo_length, const struct bmi2_dev *dev); * @note Change of BMI2_AUX_RD_ADDR is only allowed if AUX is not busy. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_SET_APS_FAIL - Error: Set Advance Power Save Fail - * @retval BMI2_E_AUX_INVALID_CFG - Error: Invalid auxiliary configuration. + * @retval 0 -> Success + * @retval < 0 -> Fail */ int8_t bmi2_read_aux_man_mode(uint8_t reg_addr, uint8_t *aux_data, uint16_t len, struct bmi2_dev *dev); /*! - * @brief This API enables/disables OIS interface. - * - * @param[in] enable : To enable/disable OIS interface. - * @param[in] dev : Structure instance of bmi2_dev. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables OIS interface. - * BMI2_ENABLE | Enables OIS interface. - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail - */ -int8_t bmi2_set_ois_interface(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This API writes the user-defined bytes of data and the address of + * \ingroup bmi2ApiAux + * \page bmi2_api_bmi2_write_aux_man_mode bmi2_write_aux_man_mode + * \code + * int8_t bmi2_write_aux_man_mode(uint8_t reg_addr, const uint8_t *aux_data, uint16_t len, struct bmi2_dev *dev); + * \endcode + * @details This API writes the user-defined bytes of data and the address of * auxiliary sensor where data is to be written in manual mode. * * @param[in] reg_addr : AUX address where data is to be written. @@ -872,17 +996,18 @@ int8_t bmi2_set_ois_interface(uint8_t enable, struct bmi2_dev *dev); * @note Change of BMI2_AUX_WR_ADDR is only allowed if AUX is not busy. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_SET_APS_FAIL - Error: Set Advance Power Save Fail - * @retval BMI2_E_AUX_INVALID_CFG - Error: Invalid auxiliary configuration. + * @retval 0 -> Success + * @retval < 0 -> Fail */ int8_t bmi2_write_aux_man_mode(uint8_t reg_addr, const uint8_t *aux_data, uint16_t len, struct bmi2_dev *dev); /*! - * @brief This API writes the user-defined bytes of data and the address of + * \ingroup bmi2ApiAux + * \page bmi2_api_bmi2_write_aux_interleaved bmi2_write_aux_interleaved + * \code + * int8_t bmi2_write_aux_interleaved(uint8_t reg_addr, const uint8_t *aux_data, uint16_t len, struct bmi2_dev *dev); + * \endcode + * @details This API writes the user-defined bytes of data and the address of * auxiliary sensor where data is to be written, from an interleaved input, * in manual mode. * @@ -894,22 +1019,30 @@ int8_t bmi2_write_aux_man_mode(uint8_t reg_addr, const uint8_t *aux_data, uint16 * @note Change of BMI2_AUX_WR_ADDR is only allowed if AUX is not busy. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_SET_APS_FAIL - Error: Set Advance Power Save Fail - * @retval BMI2_E_AUX_INVALID_CFG - Error: Invalid auxiliary configuration. + * @retval 0 -> Success + * @retval < 0 -> Fail */ int8_t bmi2_write_aux_interleaved(uint8_t reg_addr, const uint8_t *aux_data, uint16_t len, struct bmi2_dev *dev); +/** + * \ingroup bmi2 + * \defgroup bmi2ApiStatus Sensor Status + * @brief Get sensor status + */ + /*! - * @brief This API gets the data ready status of accelerometer, gyroscope, + * \ingroup bmi2ApiStatus + * \page bmi2_api_bmi2_get_status bmi2_get_status + * \code + * int8_t bmi2_get_status(uint8_t *status, const struct bmi2_dev *dev); + * \endcode + * @details This API gets the data ready status of accelerometer, gyroscope, * auxiliary, ready status of command decoder and busy status of auxiliary. * * @param[out] status : Pointer variable to the status. * @param[in] dev : Structure instance of bmi2_dev. * + *@verbatim * Value | Status * ---------|--------------------- * 0x80 | DRDY_ACC @@ -917,17 +1050,27 @@ int8_t bmi2_write_aux_interleaved(uint8_t reg_addr, const uint8_t *aux_data, uin * 0x20 | DRDY_AUX * 0x10 | CMD_RDY * 0x04 | AUX_BUSY + *@endverbatim * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_get_status(uint8_t *status, struct bmi2_dev *dev); + +/** + * \ingroup bmi2 + * \defgroup bmi2ApiWSync Sync commands + * @brief Write sync commands */ -int8_t bmi2_get_status(uint8_t *status, const struct bmi2_dev *dev); /*! - * @brief This API can be used to write sync commands like ODR, sync period, + * \ingroup bmi2ApiWSync + * \page bmi2_api_bmi2_write_sync_commands bmi2_write_sync_commands + * \code + * int8_t bmi2_write_sync_commands(const uint8_t *command, uint8_t n_comm, struct bmi2_dev *dev); + * \endcode + * @details This API can be used to write sync commands like ODR, sync period, * frequency and phase, resolution ratio, sync time and delay time. * * @param[in] command : Sync command to be written. @@ -935,153 +1078,158 @@ int8_t bmi2_get_status(uint8_t *status, const struct bmi2_dev *dev); * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ int8_t bmi2_write_sync_commands(const uint8_t *command, uint8_t n_comm, struct bmi2_dev *dev); +/** + * \ingroup bmi2 + * \defgroup bmi2ApiASelftest Accel self test + * @brief Perform accel self test + */ + /*! - * @brief This API performs self-test to check the proper functionality of the + * \ingroup bmi2ApiASelftest + * \page bmi2_api_bmi2_perform_accel_self_test bmi2_perform_accel_self_test + * \code + * int8_t bmi2_perform_accel_self_test(struct bmi2_dev *dev); + * \endcode + * @details This API performs self-test to check the proper functionality of the * accelerometer sensor. * * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_SELF_TEST_FAIL - Error: Self test fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ int8_t bmi2_perform_accel_self_test(struct bmi2_dev *dev); /*! - * @brief This API maps/unmaps feature interrupts to that of interrupt pins. + * \ingroup bmi2ApiInt + * \page bmi2_api_bmi2_map_feat_int bmi2_map_feat_int + * \code + * int8_t bmi2_map_feat_int(const struct bmi2_sens_int_config *sens_int, struct bmi2_dev *dev); + * \endcode + * @details This API maps/unmaps feature interrupts to that of interrupt pins. * * @param[in] sens_int : Structure instance of bmi2_sens_int_config. - * @param[in] n_sens : Number of features to be mapped. * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_FEAT_INT - Error: Invalid feature Interrupt + * @retval 0 -> Success + * @retval < 0 -> Fail */ -int8_t bmi2_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev); +int8_t bmi2_map_feat_int(uint8_t type, enum bmi2_hw_int_pin hw_int_pin, struct bmi2_dev *dev); /*! - * @brief This API maps/un-maps data interrupts to that of interrupt pins. + * \ingroup bmi2ApiInt + * \page bmi2_api_bmi2_map_data_int bmi2_map_data_int + * \code + * int8_t bmi2_map_data_int(uint8_t data_int, enum bmi2_hw_int_pin int_pin, struct bmi2_dev *dev); + * \endcode + * @details This API maps/un-maps data interrupts to that of interrupt pins. * * @param[in] int_pin : Interrupt pin selected. * @param[in] data_int : Type of data interrupt to be mapped. * @param[in] dev : Structure instance of bmi2_dev. * + *@verbatim * data_int | Mask values * ---------------------|--------------------- * BMI2_FFULL_INT | 0x01 * BMI2_FWM_INT | 0x02 * BMI2_DRDY_INT | 0x04 * BMI2_ERR_INT | 0x08 + *@endverbatim * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_INT_PIN - Error: Invalid interrupt pin + * @retval 0 -> Success + * @retval < 0 -> Fail */ int8_t bmi2_map_data_int(uint8_t data_int, enum bmi2_hw_int_pin int_pin, struct bmi2_dev *dev); +/** + * \ingroup bmi2 + * \defgroup bmi2ApiRemap Remap Axes + * @brief Set / Get remap axes values from the sensor + */ + /*! - * @brief This API gets the re-mapped x, y and z axes from the sensor and + * \ingroup bmi2ApiRemap + * \page bmi2_api_bmi2_get_remap_axes bmi2_get_remap_axes + * \code + * int8_t bmi2_get_remap_axes(struct bmi2_remap *remapped_axis, struct bmi2_dev *dev); + * \endcode + * @details This API gets the re-mapped x, y and z axes from the sensor and * updates the values in the device structure. * * @param[out] remapped_axis : Structure that stores re-mapped axes. * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page + * @retval 0 -> Success + * @retval < 0 -> Fail */ int8_t bmi2_get_remap_axes(struct bmi2_remap *remapped_axis, struct bmi2_dev *dev); /*! - * @brief This API sets the re-mapped x, y and z axes to the sensor and + * \ingroup bmi2ApiRemap + * \page bmi2_api_bmi2_set_remap_axes bmi2_set_remap_axes + * \code + * int8_t bmi2_set_remap_axes(const struct bmi2_remap *remapped_axis, struct bmi2_dev *dev); + * \endcode + * @details This API sets the re-mapped x, y and z axes to the sensor and * updates them in the device structure. * * @param[in] remapped_axis : Structure that stores re-mapped axes. * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page + * @retval 0 -> Success + * @retval < 0 -> Fail */ int8_t bmi2_set_remap_axes(const struct bmi2_remap *remapped_axis, struct bmi2_dev *dev); -/*! - * @brief This API updates the gyroscope user-gain. - * - * @param[in] user_gain : Structure that stores user-gain configurations. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_GYR_USER_GAIN_UPD_FAIL - Gyroscope user gain update fail +/** + * \ingroup bmi2 + * \defgroup bmi2ApiGyroOC Gyro Offset Compensation + * @brief Gyro Offset Compensation operations of the sensor */ -int8_t bmi2_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev); /*! - * @brief This API reads the compensated gyroscope user-gain values. - * - * @param[out] gyr_usr_gain : Structure that stores gain values. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail - */ -int8_t bmi2_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, const struct bmi2_dev *dev); - -/*! - * @brief This API enables/disables gyroscope offset compensation. It adds the + * \ingroup bmi2ApiGyroOC + * \page bmi2_api_bmi2_set_gyro_offset_comp bmi2_set_gyro_offset_comp + * \code + * int8_t bmi2_set_gyro_offset_comp(uint8_t enable, struct bmi2_dev *dev); + * \endcode + * @details This API enables/disables gyroscope offset compensation. It adds the * offsets defined in the offset register with gyroscope data. * * @param[in] enable : Enables/Disables gyroscope offset compensation. * @param[in] dev : Structure instance of bmi2_dev. * + *@verbatim * enable | Description * -------------|--------------- * BMI2_ENABLE | Enables gyroscope offset compensation. * BMI2_DISABLE | Disables gyroscope offset compensation. + *@endverbatim * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ int8_t bmi2_set_gyro_offset_comp(uint8_t enable, struct bmi2_dev *dev); /*! - * @brief This API reads the gyroscope bias values for each axis which is used + * \ingroup bmi2ApiGyroOC + * \page bmi2_api_bmi2_read_gyro_offset_comp_axes bmi2_read_gyro_offset_comp_axes + * \code + * int8_t bmi2_read_gyro_offset_comp_axes(struct bmi2_sens_axes_data *gyr_off_comp_axes, const struct bmi2_dev *dev); + * \endcode + * @details This API reads the gyroscope bias values for each axis which is used * for gyroscope offset compensation. * * @param[out] gyr_off_comp_axes: Structure to store gyroscope offset @@ -1089,15 +1237,18 @@ int8_t bmi2_set_gyro_offset_comp(uint8_t enable, struct bmi2_dev *dev); * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ -int8_t bmi2_read_gyro_offset_comp_axes(struct bmi2_sens_axes_data *gyr_off_comp_axes, const struct bmi2_dev *dev); +int8_t bmi2_read_gyro_offset_comp_axes(struct bmi2_sens_axes_data *gyr_off_comp_axes, struct bmi2_dev *dev); /*! - * @brief This API writes the gyroscope bias values for each axis which is used + * \ingroup bmi2ApiGyroOC + * \page bmi2_api_bmi2_write_gyro_offset_comp_axes bmi2_write_gyro_offset_comp_axes + * \code + * int8_t bmi2_write_gyro_offset_comp_axes(const struct bmi2_sens_axes_data *gyr_off_comp_axes, struct bmi2_dev *dev); + * \endcode + * @details This API writes the gyroscope bias values for each axis which is used * for gyroscope offset compensation. * * @param[in] gyr_off_comp_axes : Structure to store gyroscope offset @@ -1105,84 +1256,53 @@ int8_t bmi2_read_gyro_offset_comp_axes(struct bmi2_sens_axes_data *gyr_off_comp_ * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ int8_t bmi2_write_gyro_offset_comp_axes(const struct bmi2_sens_axes_data *gyr_off_comp_axes, struct bmi2_dev *dev); -/*! - * @brief This internal API is used to parse the activity output from the - * FIFO in header mode. - * - * @param[out] act_recog : Pointer to buffer where the parsed activity data - * bytes are stored. - * @param[in] act_frm_len : Number of activity frames parsed. - * @param[in] fifo : Structure instance of bmi2_fifo_frame. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @verbatim - * bmi2_act_rec_output | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * time_stamp | time-stamp (expressed in 50Hz ticks) - * -------------------------|--------------------------------------------------- - * type | Type of activity - * -------------------------|--------------------------------------------------- - * stat | Activity status - * -------------------------|--------------------------------------------------- - * @endverbatim - * - * type | Activities - *----------|--------------------- - * 0 | UNKNOWN - * 1 | STILL - * 2 | WALK - * 3 | RUN - * 4 | BIKE - * 5 | VEHICLE - * 6 | TILTED - * - * - * stat | Activity status - *----------|--------------------- - * 1 | START - * 2 | END - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_W_FIFO_EMPTY - Warning : FIFO is empty - * @retval BMI2_W_PARTIAL_READ - Warning : There are more frames to be read +/** + * \ingroup bmi2 + * \defgroup bmi2ApiGyroCS Gyro cross sensitivity + * @brief Gyro Cross sensitivity operation */ -int8_t bmi2_get_act_recog_output(struct bmi2_act_recog_output *act_recog, - uint16_t *act_frm_len, - struct bmi2_fifo_frame *fifo, - const struct bmi2_dev *dev); /*! - * @brief This API updates the cross sensitivity coefficient between gyroscope's + * \ingroup bmi2ApiGyroCS + * \page bmi2_api_bmi2_get_gyro_cross_sense bmi2_get_gyro_cross_sense + * \code + * int8_t bmi2_get_gyro_cross_sense(struct bmi2_dev *dev); + * \endcode + * @details This API updates the cross sensitivity coefficient between gyroscope's * X and Z axes. * * @param[in, out] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ int8_t bmi2_get_gyro_cross_sense(struct bmi2_dev *dev); +/** + * \ingroup bmi2 + * \defgroup bmi2ApiInts Internal Status + * @brief Get Internal Status of the sensor + */ + /*! - * @brief This API gets error bits and message indicating internal status. + * \ingroup bmi2ApiInts + * \page bmi2_api_bmi2_get_internal_status bmi2_get_internal_status + * \code + * int8_t bmi2_get_internal_status(uint8_t *int_stat, const struct bmi2_dev *dev); + * \endcode + * @details This API gets error bits and message indicating internal status. * * @param[in] dev : Structure instance of bmi2_dev. * @param[out] int_stat : Pointer variable to store error bits and * message. * + *@verbatim * Internal status | *int_stat * ---------------------|--------------------- * BMI2_NOT_INIT | 0x00 @@ -1197,17 +1317,27 @@ int8_t bmi2_get_gyro_cross_sense(struct bmi2_dev *dev); * BMI2_AXES_MAP_ERROR | 0x20 * BMI2_ODR_50_HZ_ERROR | 0x40 * BMI2_ODR_HIGH_ERROR | 0x80 + *@endverbatim * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_get_internal_status(uint8_t *int_stat, struct bmi2_dev *dev); + +/** + * \ingroup bmi2 + * \defgroup bmi2ApiFOC FOC + * @brief FOC operations of the sensor */ -int8_t bmi2_get_internal_status(uint8_t *int_stat, const struct bmi2_dev *dev); /*! - * @brief This API performs Fast Offset Compensation for accelerometer. + * \ingroup bmi2ApiFOC + * \page bmi2_api_bmi2_perform_accel_foc bmi2_perform_accel_foc + * \code + * int8_t bmi2_perform_accel_foc(const struct bmi2_accel_foc_g_value *accel_g_value, struct bmi2_dev *dev); + * \endcode + * @details This API performs Fast Offset Compensation for accelerometer. * * @param[in] accel_g_value : This parameter selects the accel foc * axis to be performed @@ -1221,119 +1351,161 @@ int8_t bmi2_get_internal_status(uint8_t *int_stat, const struct bmi2_dev *dev); * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_SET_APS_FAIL - Error: Set Advance Power Save Fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ -int8_t bmi2_perform_accel_foc(const struct accel_foc_g_value *accel_g_value, struct bmi2_dev *dev); +int8_t bmi2_perform_accel_foc(const struct bmi2_accel_foc_g_value *accel_g_value, struct bmi2_dev *dev); /*! - * @brief This API performs Fast Offset Compensation for gyroscope. + * \ingroup bmi2ApiFOC + * \page bmi2_api_bmi2_perform_gyro_foc bmi2_perform_gyro_foc + * \code + * int8_t bmi2_perform_gyro_foc(struct bmi2_dev *dev); + * \endcode + * @details This API performs Fast Offset Compensation for gyroscope. * * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_INVALID_SENSOR - Error: Invalid sensor - * @retval BMI2_E_SET_APS_FAIL - Error: Set Advance Power Save Fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ int8_t bmi2_perform_gyro_foc(struct bmi2_dev *dev); +/** + * \ingroup bmi2 + * \defgroup bmi2ApiCRT CRT + * @brief CRT operations of the sensor + */ + /*! - * @brief API performs Component Re-Trim calibration (CRT). + * \ingroup bmi2ApiCRT + * \page bmi2_api_bmi2_do_crt bmi2_do_crt + * \code + * int8_t bmi2_do_crt(struct bmi2_dev *dev); + * \endcode + * @details API performs Component Re-Trim calibration (CRT). * * * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_CRT_ERROR - Error in CRT download + * @retval 0 -> Success + * @retval < 0 -> Fail * * @note CRT calibration takes approximately 500ms & maximum time out configured as 2 seconds */ int8_t bmi2_do_crt(struct bmi2_dev *dev); +/** + * \ingroup bmi2 + * \defgroup bmi2ApiCRTSt CRT and self test + * @brief Enable / Abort CRT and self test operations of gyroscope + */ + /*! - * @brief This api is used to abort ongoing crt or gyro self test. + * \ingroup bmi2ApiCRTSt + * \page bmi2_api_bmi2_set_gyro_self_test_crt bmi2_set_gyro_self_test_crt + * \code + * int8_t bmi2_set_gyro_self_test_crt + * \endcode + * @details This api is used to enable the gyro self-test and crt. + * *gyro_self_test_crt -> 0 then gyro self test enable + * *gyro_self_test_crt -> 1 then CRT enable + * + * @param[in] gyro_self_test_crt : enable the gyro self test or crt. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_set_gyro_self_test_crt(uint8_t *gyro_self_test_crt, struct bmi2_dev *dev); + +/*! + * \ingroup bmi2ApiCRTSt + * \page bmi2_api_bmi2_abort_crt_gyro_st bmi2_abort_crt_gyro_st + * \code + * int8_t bmi2_abort_crt_gyro_st(struct bmi2_dev *dev); + * \endcode + * @details This api is used to abort ongoing crt or gyro self test. * * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ - int8_t bmi2_abort_crt_gyro_st(struct bmi2_dev *dev); /*! - * @brief this api is used to perform gyroscope self test. + * \ingroup bmi2ApiASelftest + * \page bmi2_api_bmi2_do_gyro_st bmi2_do_gyro_st + * \code + * int8_t bmi2_do_gyro_st + * \endcode + * @details this api is used to perform gyroscope self test. * * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval < 0 - Fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ int8_t bmi2_do_gyro_st(struct bmi2_dev *dev); -/*! @brief This api is used for programming the non volatile memory(nvm) +/** + * \ingroup bmi2 + * \defgroup bmi2ApiNVM NVM + * @brief NVM operations of the sensor + */ + +/*! + * \ingroup bmi2ApiNVM + * \page bmi2_api_bmi2_nvm_prog bmi2_nvm_prog + * \code + * int8_t bmi2_nvm_prog + * \endcode + * @details This api is used for programming the non volatile memory(nvm) * * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval < 0 - Fail + * @retval 0 -> Success + * @retval < 0 -> Fail */ int8_t bmi2_nvm_prog(struct bmi2_dev *dev); -/*! @brief This api is used for retrieving the following activity recognition settings currently set. - * enable/disable post processing(0/1) by default -> 1(enable), - * Setting the min & max Gini's diversity index (GDI) threshold. min_GDI_tres(0-0XFFFF) by default ->(0x06e1) - * max_GDI_tres(0-0xFFFF) by default ->(0x0A66) - * buffer size for post processing. range (1-0x0A) default -> (0x0A) - * min segment confidence. range (1-0x0A) default -> (0x0A) +/*! + * @brief This API extracts the input feature configuration + * details like page and start address from the look-up table. * - * @param[in] sett : Structure instance of act_recg_sett. - * @param[in] dev : Structure instance of bmi2_dev. + * @param[out] feat_config : Structure that stores feature configurations. + * @param[in] type : Type of feature or sensor. + * @param[in] dev : Structure instance of bmi2_dev. * - * @return Result of API execution status. + * @return Returns the feature found flag. * - * @retval BMI2_OK - Success. - * @retval < 0 - Fail + * @retval BMI2_FALSE : Feature not found + * BMI2_TRUE : Feature found */ -int8_t bmi2_get_act_recg_sett(struct act_recg_sett *sett, struct bmi2_dev *dev); +uint8_t bmi2_extract_input_feat_config(struct bmi2_feature_config *feat_config, uint8_t type, + const struct bmi2_dev *dev); -/*! @brief This api is used for setting the following activity recognition settings - * enable/disable post processing(0/1) by default -> 1(enable), - * Setting the min & max Gini's diversity index (GDI) threshold. min_GDI_tres(0-0XFFFF) by default ->(0x06e1) - * max_GDI_tres(0-0xFFFF) by default ->(0x0A66) - * buffer size for post processing. range (1-0x0A) default -> (0x0A) - * min segment confidence. range (1-0x0A) default -> (0x0A) +/*! + * @brief This API is used to get the feature configuration from the + * selected page. * - * @param[in] sett : Structure instance of act_recg_sett. - * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] sw_page : Switches to the desired page. + * @param[out] feat_config : Pointer to the feature configuration. + * @param[in] dev : Structure instance of bmi2_dev. * - * @return Result of API execution status. - * - * @retval BMI2_OK - Success. - * @retval < 0 - Fail + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail */ -int8_t bmi2_set_act_recg_sett(const struct act_recg_sett *sett, struct bmi2_dev *dev); +int8_t bmi2_get_feat_config(uint8_t sw_page, uint8_t *feat_config, struct bmi2_dev *dev); -/******************************************************************************/ -/*! @name C++ Guard Macros */ -/******************************************************************************/ #ifdef __cplusplus } #endif /* End of CPP guard */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.c b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.c index 3ada58a86..124a984b8 100644 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.c +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.c @@ -30,11 +30,13 @@ * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * -* @file bmi270.c -* @date 2020-01-10 -* @version v2.46.1 +* @file bmi270.c +* @date 2020-11-04 +* @version v2.63.1 * -*//***************************************************************************/ +*/ + +/***************************************************************************/ /*! Header files ****************************************************************************/ @@ -483,6 +485,7 @@ const uint8_t bmi270_config_file[] = { /*! @name Global array that stores the feature input configuration of BMI270 */ const struct bmi2_feature_config bmi270_feat_in[BMI270_MAX_FEAT_IN] = { + { .type = BMI2_CONFIG_ID, .page = BMI2_PAGE_1, .start_addr = BMI270_CONFIG_ID_STRT_ADDR }, { .type = BMI2_MAX_BURST_LEN, .page = BMI2_PAGE_1, .start_addr = BMI270_MAX_BURST_LEN_STRT_ADDR }, { .type = BMI2_CRT_GYRO_SELF_TEST, .page = BMI2_PAGE_1, .start_addr = BMI270_CRT_GYRO_SELF_TEST_STRT_ADDR }, { .type = BMI2_ABORT_CRT_GYRO_SELF_TEST, .page = BMI2_PAGE_1, .start_addr = BMI270_ABORT_STRT_ADDR }, @@ -512,6 +515,18 @@ const struct bmi2_feature_config bmi270_feat_out[BMI270_MAX_FEAT_OUT] = { { .type = BMI2_VFRM_STATUS, .page = BMI2_PAGE_0, .start_addr = BMI270_NVM_VFRM_OUT_STRT_ADDR } }; +/*! @name Global array that stores the feature interrupts of BMI270 */ +struct bmi2_map_int bmi270_map_int[BMI270_MAX_INT_MAP] = { + { .type = BMI2_SIG_MOTION, .sens_map_int = BMI270_INT_SIG_MOT_MASK }, + { .type = BMI2_STEP_COUNTER, .sens_map_int = BMI270_INT_STEP_COUNTER_MASK }, + { .type = BMI2_STEP_DETECTOR, .sens_map_int = BMI270_INT_STEP_DETECTOR_MASK }, + { .type = BMI2_STEP_ACTIVITY, .sens_map_int = BMI270_INT_STEP_ACT_MASK }, + { .type = BMI2_WRIST_GESTURE, .sens_map_int = BMI270_INT_WRIST_GEST_MASK }, + { .type = BMI2_WRIST_WEAR_WAKE_UP, .sens_map_int = BMI270_INT_WRIST_WEAR_WAKEUP_MASK }, + { .type = BMI2_ANY_MOTION, .sens_map_int = BMI270_INT_ANY_MOT_MASK }, + { .type = BMI2_NO_MOTION, .sens_map_int = BMI270_INT_NO_MOT_MASK }, +}; + /******************************************************************************/ /*! Local Function Prototypes @@ -524,10 +539,697 @@ const struct bmi2_feature_config bmi270_feat_out[BMI270_MAX_FEAT_OUT] = { * @param[in] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * @retval zero -> Success / +ve value -> Warning / -ve value -> Error + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t null_ptr_check(const struct bmi2_dev *dev); +/*! + * @brief This internal API enables the selected sensor/features. + * + * @param[in] sensor_sel : Selects the desired sensor. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t sensor_enable(uint64_t sensor_sel, struct bmi2_dev *dev); + +/*! + * @brief This internal API disables the selected sensor/features. + * + * @param[in] sensor_sel : Selects the desired sensor. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t sensor_disable(uint64_t sensor_sel, struct bmi2_dev *dev); + +/*! + * @brief This internal API selects the sensors/features to be enabled or + * disabled. + * + * @param[in] sens_list : Pointer to select the sensor. + * @param[in] n_sens : Number of sensors selected. + * @param[out] sensor_sel : Gets the selected sensor. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t select_sensor(const uint8_t *sens_list, uint8_t n_sens, uint64_t *sensor_sel); + +/*! + * @brief This internal API is used to enable/disable any-motion feature. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables any-motion. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables any-motion. + * BMI2_ENABLE | Enables any-motion. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_any_motion(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to enable/disable no-motion feature. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables no-motion. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables no-motion. + * BMI2_ENABLE | Enables no-motion. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_no_motion(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to enable/disable sig-motion feature. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables sig-motion. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables sig-motion. + * BMI2_ENABLE | Enables sig-motion. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_sig_motion(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to enable/disable step detector feature. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables step-detector. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables step detector + * BMI2_ENABLE | Enables step detector + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_step_detector(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to enable/disable step counter feature. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables step counter. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables step counter + * BMI2_ENABLE | Enables step counter + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_step_counter(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to enable/disable step activity detection. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables step activity. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables step activity + * BMI2_ENABLE | Enables step activity + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_step_activity(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API gives an option to enable offset correction + * feature of gyroscope, either internally or by the host. + * + * @param[in] enable : Enables/Disables self-offset correction. + * @param[in] dev : Structure instance of bmi2_dev. + * + * enable | Description + * -------------|--------------- + * BMI2_ENABLE | gyroscope offset correction values are set internally + * BMI2_DISABLE | gyroscope offset correction values has to be set by host + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_gyro_self_offset_corr(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to enable/disable gyroscope user gain + * feature. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables gyroscope user gain. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables gyroscope user gain + * BMI2_ENABLE | Enables gyroscope user gain + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_gyro_user_gain(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API enables the wrist gesture feature. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables wrist gesture. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables wrist gesture + * BMI2_ENABLE | Enables wrist gesture + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_wrist_gesture(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API enables the wrist wear wake up feature. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables wrist wear wake up. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables wrist wear wake up + * BMI2_ENABLE | Enables wrist wear wake up + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_wrist_wear_wake_up(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets any-motion configurations like axes select, + * duration, threshold and output-configuration. + * + * @param[in] config : Structure instance of bmi2_any_motion_config. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @verbatim + *---------------------------------------------------------------------------- + * bmi2_any_motion_config | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * | Defines the number of consecutive data points for + * | which the threshold condition must be respected, + * | for interrupt assertion. It is expressed in 50 Hz + * duration | samples (20 msec). + * | Range is 0 to 163sec. + * | Default value is 5 = 100ms. + * -------------------------|--------------------------------------------------- + * | Slope threshold value for in 5.11g format. + * threshold | Range is 0 to 1g. + * | Default value is 0xAA = 83mg. + * -------------------------|--------------------------------------------------- + * x_sel, y_sel, z_sel | Selects the feature on a per-axis basis + * -------------------------|--------------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_any_motion_config(const struct bmi2_any_motion_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets no-motion configurations like axes select, + * duration, threshold and output-configuration. + * + * @param[in] config : Structure instance of bmi2_no_motion_config. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @verbatim + *---------------------------------------------------------------------------- + * bmi2_no_motion_config | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * | Defines the number of consecutive data points for + * | which the threshold condition must be respected, + * | for interrupt assertion. It is expressed in 50 Hz + * duration | samples (20 msec). + * | Range is 0 to 163sec. + * | Default value is 5 = 100ms. + * -------------------------|--------------------------------------------------- + * | Slope threshold value for in 5.11g format. + * threshold | Range is 0 to 1g. + * | Default value is 0xAA = 83mg. + * -------------------------|--------------------------------------------------- + * x_sel, y_sel, z_sel | Selects the feature on a per-axis basis + * -------------------------|--------------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_no_motion_config(const struct bmi2_no_motion_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets sig-motion configurations like block-size, + * output-configuration and other parameters. + * + * @param[in] config : Structure instance of bmi2_sig_motion_config. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + *---------------------------------------------------------------------------- + * bmi2_sig_motion_config | + * Structure parameters | Description + * -------------------------|--------------------------------------------------- + * | Defines the duration after which the significant + * block_size | motion interrupt is triggered. It is expressed in + * | 50 Hz samples (20 ms). Default value is 0xFA=5sec. + *--------------------------|--------------------------------------------------- + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_sig_motion_config(const struct bmi2_sig_motion_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets step counter parameter configurations. + * + * @param[in] step_count_params : Array that stores parameters 1 to 25. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_step_count_params_config(const uint16_t *step_count_params, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets step counter/detector/activity configurations. + * + * @param[in] config : Structure instance of bmi2_step_config. + * @param[in] dev : Structure instance of bmi2_dev. + * + *--------------------------------------------------------------------------- + * bmi2_step_config | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * | The Step-counter will trigger output every time + * | the number of steps are counted. Holds implicitly + * water-mark level | a 20x factor, so the range is 0 to 10230, + * | with resolution of 20 steps. + * -------------------------|--------------------------------------------------- + * reset counter | Flag to reset the counted steps. + * -------------------------|--------------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_step_config(const struct bmi2_step_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets wrist gesture configurations like wearable-arm, + * and output-configuration. + * + * @param[in] config : Structure instance of bmi2_wrist_gest_config. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @verbatim + *----------------------------------------------------------------------------- + * bmi2_wrist_gest_config | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * | Device in left (0) or right (1) arm. By default, + * wear_arm | the wearable device is assumed to be in left arm + * | i.e. default value is 0. + * -------------------------|--------------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_wrist_gest_config(const struct bmi2_wrist_gest_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets wrist wear wake-up configurations like + * output-configuration. + * + * @param[in] config : Structure instance of + * bmi2_wrist_wear_wake_up_config. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @verbatim + *----------------------------------------------------------------------------- + * bmi2_wrist_wear_wake_up_config | + * Structure parameters | Description + *----------------------------------|------------------------------------------- + * | To set the wrist wear wake-up parameters like + * | min_angle_focus, min_angle_nonfocus, + * wrist_wear_wakeup_params | angle_landscape_left, angle_landscape_right, + * | angle_potrait_up and down. + * ---------------------------------|------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_wrist_wear_wake_up_config(const struct bmi2_wrist_wear_wake_up_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets any-motion configurations like axes select, + * duration, threshold and output-configuration. + * + * @param[out] config : Structure instance of bmi2_any_motion_config. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @verbatim + *---------------------------------------------------------------------------- + * bmi2_any_motion_config | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * | Defines the number of consecutive data points for + * | which the threshold condition must be respected, + * | for interrupt assertion. It is expressed in 50 Hz + * duration | samples (20 msec). + * | Range is 0 to 163sec. + * | Default value is 5 = 100ms. + * -------------------------|--------------------------------------------------- + * | Slope threshold value for in 5.11g format. + * threshold | Range is 0 to 1g. + * | Default value is 0xAA = 83mg. + * -------------------------|--------------------------------------------------- + * x_sel, y_sel, z_sel | Selects the feature on a per-axis basis + * -------------------------|--------------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_any_motion_config(struct bmi2_any_motion_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets no-motion configurations like axes select, + * duration, threshold and output-configuration. + * + * @param[out] config : Structure instance of bmi2_no_motion_config. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @verbatim + *---------------------------------------------------------------------------- + * bmi2_no_motion_config | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * | Defines the number of consecutive data points for + * | which the threshold condition must be respected, + * | for interrupt assertion. It is expressed in 50 Hz + * duration | samples (20 msec). + * | Range is 0 to 163sec. + * | Default value is 5 = 100ms. + * -------------------------|--------------------------------------------------- + * | Slope threshold value for in 5.11g format. + * threshold | Range is 0 to 1g. + * | Default value is 0xAA = 83mg. + * -------------------------|--------------------------------------------------- + * x_sel, y_sel, z_sel | Selects the feature on a per-axis basis + * -------------------------|--------------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_no_motion_config(struct bmi2_no_motion_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets sig-motion configurations like block-size, + * output-configuration and other parameters. + * + * @param[out] config : Structure instance of bmi2_sig_motion_config. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + *---------------------------------------------------------------------------- + * bmi2_sig_motion_config | + * Structure parameters | Description + * -------------------------|--------------------------------------------------- + * | Defines the duration after which the significant + * block_size | motion interrupt is triggered. It is expressed in + * | 50 Hz samples (20 ms). Default value is 0xFA=5sec. + *--------------------------|--------------------------------------------------- + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_sig_motion_config(struct bmi2_sig_motion_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets step counter parameter configurations. + * + * @param[in] step_count_params : Array that stores parameters 1 to 25. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_step_count_params_config(uint16_t *step_count_params, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets step counter/detector/activity configurations. + * + * @param[out] config : Structure instance of bmi2_step_config. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @verbatim + *---------------------------------------------------------------------------- + * bmi2_step_config | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * | The Step-counter will trigger output every time + * | the number of steps are counted. Holds implicitly + * water-mark level | a 20x factor, so the range is 0 to 10230, + * | with resolution of 20 steps. + * -------------------------|--------------------------------------------------- + * reset counter | Flag to reset the counted steps. + * -------------------------|--------------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_step_config(struct bmi2_step_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets wrist gesture configurations like wearable-arm, + * and output-configuration. + * + * @param[out] config : Structure instance of bmi2_wrist_gest_config. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @verbatim + *----------------------------------------------------------------------------- + * bmi2_wrist_gest_config | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * | Device in left (0) or right (1) arm. By default, + * wear_arm | the wearable device is assumed to be in left arm + * | i.e. default value is 0. + * -------------------------|--------------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_wrist_gest_config(struct bmi2_wrist_gest_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets wrist wear wake-up configurations like + * output-configuration. + * + * @param[out] config : Structure instance of + * bmi2_wrist_wear_wake_up_config. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @verbatim + *------------------------------------|--------------------------------------- + * bmi2_wrist_wear_wake_up_wh_config | + * Structure parameters | Description + *------------------------------------|------------------------------------------- + * | To get the wrist wear wake-up parameters like + * | min_angle_focus, min_angle_nonfocus, + * wrist_wear_wake_params | angle_landscape_left, angle_landscape_right, + * | angle_potrait_up and down. + * -----------------------------------|------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_wrist_wear_wake_up_config(struct bmi2_wrist_wear_wake_up_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets the output values of wrist gesture. + * + * @param[out] wrist_gest : Pointer to the stored wrist gesture. + * @param[in] dev : Structure instance of bmi2_dev. + * + * *wrist_gest | Output + * -------------|------------ + * 0x00 | UNKNOWN + * 0x01 | PUSH_ARM_DOWN + * 0x02 | PIVOT_UP + * 0x03 | WRIST_SHAKE_JIGGLE + * 0x04 | FLICK_IN + * 0x05 | FLICK_OUT + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_wrist_gest_status(uint8_t *wrist_gest, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets the output values of step activity. + * + * @param[out] step_act : Pointer to the stored step activity data. + * @param[in] dev : Structure instance of bmi2_dev. + * + * *step_act | Output + * -----------|------------ + * 0x00 | STILL + * 0x01 | WALKING + * 0x02 | RUNNING + * 0x03 | UNKNOWN + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fails + */ +static int8_t get_step_activity_output(uint8_t *step_act, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets the output values of step counter. + * + * @param[out] step_count : Pointer to the stored step counter data. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_step_counter_output(uint32_t *step_count, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets the error status related to NVM. + * + * @param[out] nvm_err_stat : Stores the NVM error status. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_nvm_error_status(struct bmi2_nvm_err_status *nvm_err_stat, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets the error status related to virtual frames. + * + * @param[out] vfrm_err_stat : Stores the VFRM related error status. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_vfrm_error_status(struct bmi2_vfrm_err_status *vfrm_err_stat, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to get enable status of gyroscope user gain + * update. + * + * @param[out] status : Stores status of gyroscope user gain update. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_user_gain_upd_status(uint8_t *status, struct bmi2_dev *dev); + +/*! + * @brief This internal API enables/disables compensation of the gain defined + * in the GAIN register. + * + * @param[in] enable : Enables/Disables gain compensation + * @param[in] dev : Structure instance of bmi2_dev. + * + * enable | Description + * -------------|--------------- + * BMI2_ENABLE | Enable gain compensation. + * BMI2_DISABLE | Disable gain compensation. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t enable_gyro_gain(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to extract the output feature configuration + * details like page and start address from the look-up table. + * + * @param[out] feat_output : Structure that stores output feature + * configurations. + * @param[in] type : Type of feature or sensor. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Returns the feature found flag. + * + * @retval BMI2_FALSE : Feature not found + * BMI2_TRUE : Feature found + */ +static uint8_t extract_output_feat_config(struct bmi2_feature_config *feat_output, + uint8_t type, + const struct bmi2_dev *dev); + /***************************************************************************/ /*! User Interface Definitions @@ -560,7 +1262,7 @@ int8_t bmi270_init(struct bmi2_dev *dev) dev->variant_feature = BMI2_GYRO_CROSS_SENS_ENABLE | BMI2_CRT_RTOSK_ENABLE; /* An extra dummy byte is read during SPI read */ - if (dev->intf == BMI2_SPI_INTERFACE) + if (dev->intf == BMI2_SPI_INTF) { dev->dummy_byte = 1; } @@ -607,15 +1309,601 @@ int8_t bmi270_init(struct bmi2_dev *dev) */ dev->out_sens = BMI270_MAX_FEAT_OUT; + /* Assign the offsets of the feature interrupt + * to the device structure + */ + dev->map_int = bmi270_map_int; + + /* Assign maximum number of feature interrupts + * to device structure + */ + dev->sens_int_map = BMI270_MAX_INT_MAP; + /* Get the gyroscope cross axis sensitivity */ rslt = bmi2_get_gyro_cross_sense(dev); - } } return rslt; } +/*! + * @brief This API selects the sensors/features to be enabled. + */ +int8_t bmi270_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to select sensor */ + uint64_t sensor_sel = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_list != NULL)) + { + /* Get the selected sensors */ + rslt = select_sensor(sens_list, n_sens, &sensor_sel); + if (rslt == BMI2_OK) + { + /* Enable the selected sensors */ + rslt = sensor_enable(sensor_sel, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API selects the sensors/features to be disabled. + */ +int8_t bmi270_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to select sensor */ + uint64_t sensor_sel = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_list != NULL)) + { + /* Get the selected sensors */ + rslt = select_sensor(sens_list, n_sens, &sensor_sel); + if (rslt == BMI2_OK) + { + /* Disable the selected sensors */ + rslt = sensor_disable(sensor_sel, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API sets the sensor/feature configuration. + */ +int8_t bmi270_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_cfg != NULL)) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + + for (loop = 0; loop < n_sens; loop++) + { + if ((sens_cfg[loop].type == BMI2_ACCEL) || (sens_cfg[loop].type == BMI2_GYRO) || + (sens_cfg[loop].type == BMI2_AUX) || (sens_cfg[loop].type == BMI2_GYRO_GAIN_UPDATE)) + { + rslt = bmi2_set_sensor_config(&sens_cfg[loop], 1, dev); + } + else + { + /* Disable Advance power save if enabled for auxiliary + * and feature configurations + */ + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if + * enabled + */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + switch (sens_cfg[loop].type) + { + /* Set any motion configuration */ + case BMI2_ANY_MOTION: + rslt = set_any_motion_config(&sens_cfg[loop].cfg.any_motion, dev); + break; + + /* Set no motion configuration */ + case BMI2_NO_MOTION: + rslt = set_no_motion_config(&sens_cfg[loop].cfg.no_motion, dev); + break; + + /* Set sig-motion configuration */ + case BMI2_SIG_MOTION: + rslt = set_sig_motion_config(&sens_cfg[loop].cfg.sig_motion, dev); + break; + + /* Set the step counter parameters */ + case BMI2_STEP_COUNTER_PARAMS: + rslt = set_step_count_params_config(sens_cfg[loop].cfg.step_counter_params, dev); + break; + + /* Set step counter/detector/activity configuration */ + case BMI2_STEP_DETECTOR: + case BMI2_STEP_COUNTER: + case BMI2_STEP_ACTIVITY: + rslt = set_step_config(&sens_cfg[loop].cfg.step_counter, dev); + break; + + /* Set the wrist gesture configuration */ + case BMI2_WRIST_GESTURE: + rslt = set_wrist_gest_config(&sens_cfg[loop].cfg.wrist_gest, dev); + break; + + /* Set the wrist wear wake-up configuration */ + case BMI2_WRIST_WEAR_WAKE_UP: + rslt = set_wrist_wear_wake_up_config(&sens_cfg[loop].cfg.wrist_wear_wake_up, dev); + break; + + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + } + + /* Return error if any of the set configurations fail */ + if (rslt != BMI2_OK) + { + break; + } + } + } + + /* Enable Advance power save if disabled while configuring and + * not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API gets the sensor/feature configuration. + */ +int8_t bmi270_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_cfg != NULL)) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + for (loop = 0; loop < n_sens; loop++) + { + if ((sens_cfg[loop].type == BMI2_ACCEL) || (sens_cfg[loop].type == BMI2_GYRO) || + (sens_cfg[loop].type == BMI2_AUX) || (sens_cfg[loop].type == BMI2_GYRO_GAIN_UPDATE)) + { + rslt = bmi2_get_sensor_config(&sens_cfg[loop], 1, dev); + } + else + { + /* Disable Advance power save if enabled for auxiliary + * and feature configurations + */ + if ((sens_cfg[loop].type >= BMI2_MAIN_SENS_MAX_NUM) || (sens_cfg[loop].type == BMI2_AUX)) + { + + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if + * enabled + */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + } + + if (rslt == BMI2_OK) + { + switch (sens_cfg[loop].type) + { + /* Get sig-motion configuration */ + case BMI2_SIG_MOTION: + rslt = get_sig_motion_config(&sens_cfg[loop].cfg.sig_motion, dev); + break; + + /* Get any motion configuration */ + case BMI2_ANY_MOTION: + rslt = get_any_motion_config(&sens_cfg[loop].cfg.any_motion, dev); + break; + + /* Get no motion configuration */ + case BMI2_NO_MOTION: + rslt = get_no_motion_config(&sens_cfg[loop].cfg.no_motion, dev); + break; + + /* Set the step counter parameters */ + case BMI2_STEP_COUNTER_PARAMS: + rslt = get_step_count_params_config(sens_cfg[loop].cfg.step_counter_params, dev); + break; + + /* Get step counter/detector/activity configuration */ + case BMI2_STEP_DETECTOR: + case BMI2_STEP_COUNTER: + case BMI2_STEP_ACTIVITY: + rslt = get_step_config(&sens_cfg[loop].cfg.step_counter, dev); + break; + + /* Get the wrist gesture configuration */ + case BMI2_WRIST_GESTURE: + rslt = get_wrist_gest_config(&sens_cfg[loop].cfg.wrist_gest, dev); + break; + + /* Get the wrist wear wake-up configuration */ + case BMI2_WRIST_WEAR_WAKE_UP: + rslt = get_wrist_wear_wake_up_config(&sens_cfg[loop].cfg.wrist_wear_wake_up, dev); + break; + + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + } + + /* Return error if any of the get configurations fail */ + if (rslt != BMI2_OK) + { + break; + } + } + } + + /* Enable Advance power save if disabled while configuring and + * not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API gets the sensor/feature data for accelerometer, gyroscope, + * auxiliary sensor, step counter, high-g, gyroscope user-gain update, + * orientation, gyroscope cross sensitivity and error status for NVM and VFRM. + */ +int8_t bmi270_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sensor_data != NULL)) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + for (loop = 0; loop < n_sens; loop++) + { + if ((sensor_data[loop].type == BMI2_ACCEL) || (sensor_data[loop].type == BMI2_GYRO) || + (sensor_data[loop].type == BMI2_AUX) || (sensor_data[loop].type == BMI2_GYRO_GAIN_UPDATE) || + (sensor_data[loop].type == BMI2_GYRO_CROSS_SENSE)) + { + rslt = bmi2_get_sensor_data(&sensor_data[loop], 1, dev); + } + else + { + /* Disable Advance power save if enabled for feature + * configurations + */ + if (sensor_data[loop].type >= BMI2_MAIN_SENS_MAX_NUM) + { + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if + * enabled + */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + } + + if (rslt == BMI2_OK) + { + switch (sensor_data[loop].type) + { + case BMI2_STEP_COUNTER: + + /* Get step counter output */ + rslt = get_step_counter_output(&sensor_data[loop].sens_data.step_counter_output, dev); + break; + case BMI2_STEP_ACTIVITY: + + /* Get step activity output */ + rslt = get_step_activity_output(&sensor_data[loop].sens_data.activity_output, dev); + break; + case BMI2_NVM_STATUS: + + /* Get NVM error status */ + rslt = get_nvm_error_status(&sensor_data[loop].sens_data.nvm_status, dev); + break; + case BMI2_VFRM_STATUS: + + /* Get VFRM error status */ + rslt = get_vfrm_error_status(&sensor_data[loop].sens_data.vfrm_status, dev); + break; + case BMI2_WRIST_GESTURE: + + /* Get wrist gesture status */ + rslt = get_wrist_gest_status(&sensor_data[loop].sens_data.wrist_gesture_output, dev); + break; + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + + /* Return error if any of the get sensor data fails */ + if (rslt != BMI2_OK) + { + break; + } + } + } + + /* Enable Advance power save if disabled while + * configuring and not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API updates the gyroscope user-gain. + */ +int8_t bmi270_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to select sensor */ + uint8_t sens_sel[2] = { BMI2_GYRO, BMI2_GYRO_GAIN_UPDATE }; + + /* Structure to define sensor configurations */ + struct bmi2_sens_config sens_cfg; + + /* Variable to store status of user-gain update module */ + uint8_t status = 0; + + /* Variable to define count */ + uint8_t count = 100; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (user_gain != NULL)) + { + /* Select type of feature */ + sens_cfg.type = BMI2_GYRO_GAIN_UPDATE; + + /* Get the user gain configurations */ + rslt = bmi270_get_sensor_config(&sens_cfg, 1, dev); + if (rslt == BMI2_OK) + { + /* Get the user-defined ratio */ + sens_cfg.cfg.gyro_gain_update = *user_gain; + + /* Set rate ratio for all axes */ + rslt = bmi270_set_sensor_config(&sens_cfg, 1, dev); + } + + /* Disable gyroscope */ + if (rslt == BMI2_OK) + { + rslt = bmi270_sensor_disable(&sens_sel[0], 1, dev); + } + + /* Enable gyroscope user-gain update module */ + if (rslt == BMI2_OK) + { + rslt = bmi270_sensor_enable(&sens_sel[1], 1, dev); + } + + /* Set the command to trigger the computation */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_command_register(BMI2_USR_GAIN_CMD, dev); + } + + if (rslt == BMI2_OK) + { + /* Poll until enable bit of user-gain update is 0 */ + while (count--) + { + rslt = get_user_gain_upd_status(&status, dev); + if ((rslt == BMI2_OK) && (status == 0)) + { + /* Enable compensation of gain defined + * in the GAIN register + */ + rslt = enable_gyro_gain(BMI2_ENABLE, dev); + + /* Enable gyroscope */ + if (rslt == BMI2_OK) + { + rslt = bmi270_sensor_enable(&sens_sel[0], 1, dev); + } + + break; + } + + dev->delay_us(10000, dev->intf_ptr); + } + + /* Return error if user-gain update is failed */ + if ((rslt == BMI2_OK) && (status != 0)) + { + rslt = BMI2_E_GYR_USER_GAIN_UPD_FAIL; + } + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API reads the compensated gyroscope user-gain values. + */ +int8_t bmi270_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define register data */ + uint8_t reg_data[3] = { 0 }; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (gyr_usr_gain != NULL)) + { + /* Get the gyroscope compensated gain values */ + rslt = bmi2_get_regs(BMI2_GYR_USR_GAIN_0_ADDR, reg_data, 3, dev); + if (rslt == BMI2_OK) + { + /* Gyroscope user gain correction X-axis */ + gyr_usr_gain->x = (int8_t)BMI2_GET_BIT_POS0(reg_data[0], BMI2_GYR_USR_GAIN_X); + + /* Gyroscope user gain correction Y-axis */ + gyr_usr_gain->y = (int8_t)BMI2_GET_BIT_POS0(reg_data[1], BMI2_GYR_USR_GAIN_Y); + + /* Gyroscope user gain correction z-axis */ + gyr_usr_gain->z = (int8_t)BMI2_GET_BIT_POS0(reg_data[2], BMI2_GYR_USR_GAIN_Z); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API maps/unmaps feature interrupts to that of interrupt pins. + */ +int8_t bmi270_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_int != NULL)) + { + for (loop = 0; loop < n_sens; loop++) + { + switch (sens_int[loop].type) + { + case BMI2_SIG_MOTION: + case BMI2_WRIST_GESTURE: + case BMI2_ANY_MOTION: + case BMI2_NO_MOTION: + case BMI2_STEP_COUNTER: + case BMI2_STEP_DETECTOR: + case BMI2_STEP_ACTIVITY: + case BMI2_WRIST_WEAR_WAKE_UP: + + rslt = bmi2_map_feat_int(sens_int[loop].type, sens_int[loop].hw_int_pin, dev); + break; + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + + /* Return error if interrupt mapping fails */ + if (rslt != BMI2_OK) + { + break; + } + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + /***************************************************************************/ /*! Local Function Definitions @@ -638,3 +1926,2526 @@ static int8_t null_ptr_check(const struct bmi2_dev *dev) return rslt; } + +/*! + * @brief This internal API selects the sensor/features to be enabled or + * disabled. + */ +static int8_t select_sensor(const uint8_t *sens_list, uint8_t n_sens, uint64_t *sensor_sel) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Variable to define loop */ + uint8_t count; + + for (count = 0; count < n_sens; count++) + { + switch (sens_list[count]) + { + case BMI2_ACCEL: + *sensor_sel |= BMI2_ACCEL_SENS_SEL; + break; + case BMI2_GYRO: + *sensor_sel |= BMI2_GYRO_SENS_SEL; + break; + case BMI2_AUX: + *sensor_sel |= BMI2_AUX_SENS_SEL; + break; + case BMI2_TEMP: + *sensor_sel |= BMI2_TEMP_SENS_SEL; + break; + case BMI2_SIG_MOTION: + *sensor_sel |= BMI2_SIG_MOTION_SEL; + break; + case BMI2_ANY_MOTION: + *sensor_sel |= BMI2_ANY_MOT_SEL; + break; + case BMI2_NO_MOTION: + *sensor_sel |= BMI2_NO_MOT_SEL; + break; + case BMI2_STEP_DETECTOR: + *sensor_sel |= BMI2_STEP_DETECT_SEL; + break; + case BMI2_STEP_COUNTER: + *sensor_sel |= BMI2_STEP_COUNT_SEL; + break; + case BMI2_STEP_ACTIVITY: + *sensor_sel |= BMI2_STEP_ACT_SEL; + break; + case BMI2_GYRO_GAIN_UPDATE: + *sensor_sel |= BMI2_GYRO_GAIN_UPDATE_SEL; + break; + case BMI2_GYRO_SELF_OFF: + *sensor_sel |= BMI2_GYRO_SELF_OFF_SEL; + break; + case BMI2_WRIST_GESTURE: + *sensor_sel |= BMI2_WRIST_GEST_SEL; + break; + case BMI2_WRIST_WEAR_WAKE_UP: + *sensor_sel |= BMI2_WRIST_WEAR_WAKE_UP_SEL; + break; + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + } + + return rslt; +} + +/*! + * @brief This internal API enables the selected sensor/features. + */ +static int8_t sensor_enable(uint64_t sensor_sel, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store register values */ + uint8_t reg_data = 0; + + /* Variable to define loop */ + uint8_t loop = 1; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + /* Enable accelerometer */ + if (sensor_sel & BMI2_ACCEL_SENS_SEL) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_ACC_EN, BMI2_ENABLE); + } + + /* Enable gyroscope */ + if (sensor_sel & BMI2_GYRO_SENS_SEL) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_EN, BMI2_ENABLE); + } + + /* Enable auxiliary sensor */ + if (sensor_sel & BMI2_AUX_SENS_SEL) + { + reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_AUX_EN, BMI2_ENABLE); + } + + /* Enable temperature sensor */ + if (sensor_sel & BMI2_TEMP_SENS_SEL) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_TEMP_EN, BMI2_ENABLE); + } + + /* Enable the sensors that are set in the power control register */ + if (sensor_sel & BMI2_MAIN_SENSORS) + { + rslt = bmi2_set_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + } + } + + if ((rslt == BMI2_OK) && (sensor_sel & ~(BMI2_MAIN_SENSORS))) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if enabled */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + while (loop--) + { + /* Enable sig-motion feature */ + if (sensor_sel & BMI2_SIG_MOTION_SEL) + { + rslt = set_sig_motion(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_SIG_MOTION_SEL; + } + else + { + break; + } + } + + /* Enable any motion feature */ + if (sensor_sel & BMI2_ANY_MOT_SEL) + { + rslt = set_any_motion(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_ANY_MOT_SEL; + } + else + { + break; + } + } + + /* Enable no motion feature */ + if (sensor_sel & BMI2_NO_MOT_SEL) + { + rslt = set_no_motion(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_NO_MOT_SEL; + } + else + { + break; + } + } + + /* Enable step detector feature */ + if (sensor_sel & BMI2_STEP_DETECT_SEL) + { + rslt = set_step_detector(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_STEP_DETECT_SEL; + } + else + { + break; + } + } + + /* Enable step counter feature */ + if (sensor_sel & BMI2_STEP_COUNT_SEL) + { + rslt = set_step_counter(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_STEP_COUNT_SEL; + } + else + { + break; + } + } + + /* Enable step activity feature */ + if (sensor_sel & BMI2_STEP_ACT_SEL) + { + rslt = set_step_activity(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_STEP_ACT_SEL; + } + else + { + break; + } + } + + /* Enable gyroscope user gain */ + if (sensor_sel & BMI2_GYRO_GAIN_UPDATE_SEL) + { + rslt = set_gyro_user_gain(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_GYRO_GAIN_UPDATE_SEL; + } + else + { + break; + } + } + + /* Enable gyroscope self-offset correction feature */ + if (sensor_sel & BMI2_GYRO_SELF_OFF_SEL) + { + rslt = set_gyro_self_offset_corr(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_GYRO_SELF_OFF_SEL; + } + else + { + break; + } + } + + /* Enable wrist gesture feature for wearable variant */ + if (sensor_sel & BMI2_WRIST_GEST_SEL) + { + rslt = set_wrist_gesture(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_WRIST_GEST_SEL; + } + else + { + break; + } + } + + /* Enable wrist wear wake-up feature */ + if (sensor_sel & BMI2_WRIST_WEAR_WAKE_UP_SEL) + { + rslt = set_wrist_wear_wake_up(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_WRIST_WEAR_WAKE_UP_SEL; + } + else + { + break; + } + } + } + + /* Enable Advance power save if disabled while + * configuring and not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + } + + return rslt; +} + +/*! + * @brief This internal API disables the selected sensors/features. + */ +static int8_t sensor_disable(uint64_t sensor_sel, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store register values */ + uint8_t reg_data = 0; + + /* Variable to define loop */ + uint8_t loop = 1; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + /* Disable accelerometer */ + if (sensor_sel & BMI2_ACCEL_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_ACC_EN); + } + + /* Disable gyroscope */ + if (sensor_sel & BMI2_GYRO_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_GYR_EN); + } + + /* Disable auxiliary sensor */ + if (sensor_sel & BMI2_AUX_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_AUX_EN); + } + + /* Disable temperature sensor */ + if (sensor_sel & BMI2_TEMP_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_TEMP_EN); + } + + /* Disable the sensors that are set in the power control register */ + if (sensor_sel & BMI2_MAIN_SENSORS) + { + rslt = bmi2_set_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + } + } + + if ((rslt == BMI2_OK) && (sensor_sel & ~(BMI2_MAIN_SENSORS))) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if enabled */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + while (loop--) + { + /* Disable sig-motion feature */ + if (sensor_sel & BMI2_SIG_MOTION_SEL) + { + rslt = set_sig_motion(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_SIG_MOTION_SEL; + } + else + { + break; + } + } + + /* Disable any-motion feature */ + if (sensor_sel & BMI2_ANY_MOT_SEL) + { + rslt = set_any_motion(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_ANY_MOT_SEL; + } + else + { + break; + } + } + + /* Disable no-motion feature */ + if (sensor_sel & BMI2_NO_MOT_SEL) + { + rslt = set_no_motion(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_NO_MOT_SEL; + } + else + { + break; + } + } + + /* Disable step detector feature */ + if (sensor_sel & BMI2_STEP_DETECT_SEL) + { + rslt = set_step_detector(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_STEP_DETECT_SEL; + } + else + { + break; + } + } + + /* Disable step counter feature */ + if (sensor_sel & BMI2_STEP_COUNT_SEL) + { + rslt = set_step_counter(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_STEP_COUNT_SEL; + } + else + { + break; + } + } + + /* Disable step activity feature */ + if (sensor_sel & BMI2_STEP_ACT_SEL) + { + rslt = set_step_activity(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_STEP_ACT_SEL; + } + else + { + break; + } + } + + /* Disable gyroscope user gain */ + if (sensor_sel & BMI2_GYRO_GAIN_UPDATE_SEL) + { + rslt = set_gyro_user_gain(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_GYRO_GAIN_UPDATE_SEL; + } + else + { + break; + } + } + + /* Disable gyroscope self-offset correction feature */ + if (sensor_sel & BMI2_GYRO_SELF_OFF_SEL) + { + rslt = set_gyro_self_offset_corr(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_GYRO_SELF_OFF_SEL; + } + else + { + break; + } + } + + /* Disable wrist gesture feature for wearable variant*/ + if (sensor_sel & BMI2_WRIST_GEST_SEL) + { + rslt = set_wrist_gesture(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_WRIST_GEST_SEL; + } + else + { + break; + } + } + + /* Enable wrist wear wake-up feature */ + if (sensor_sel & BMI2_WRIST_WEAR_WAKE_UP_SEL) + { + rslt = set_wrist_wear_wake_up(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_WRIST_WEAR_WAKE_UP_SEL; + } + else + { + break; + } + } + + /* Enable Advance power save if disabled while + * configuring and not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + } + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable any motion feature. + */ +static int8_t set_any_motion(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for any-motion */ + struct bmi2_feature_config any_mot_config = { 0, 0, 0 }; + + /* Search for any-motion feature and extract its configurations details */ + feat_found = bmi2_extract_input_feat_config(&any_mot_config, BMI2_ANY_MOTION, dev); + if (feat_found) + { + /* Get the configuration from the page where any-motion feature resides */ + rslt = bmi2_get_feat_config(any_mot_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of any-motion axes */ + idx = any_mot_config.start_addr + BMI2_ANY_MOT_FEAT_EN_OFFSET; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_ANY_NO_MOT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable no-motion feature. + */ +static int8_t set_no_motion(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for no-motion */ + struct bmi2_feature_config no_mot_config = { 0, 0, 0 }; + + /* Search for no-motion feature and extract its configurations details */ + feat_found = bmi2_extract_input_feat_config(&no_mot_config, BMI2_NO_MOTION, dev); + if (feat_found) + { + /* Get the configuration from the page where any/no-motion feature resides */ + rslt = bmi2_get_feat_config(no_mot_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of no-motion axes */ + idx = no_mot_config.start_addr + BMI2_NO_MOT_FEAT_EN_OFFSET; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_ANY_NO_MOT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable step detector feature. + */ +static int8_t set_step_detector(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step detector */ + struct bmi2_feature_config step_det_config = { 0, 0, 0 }; + + /* Search for step detector feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_det_config, BMI2_STEP_DETECTOR, dev); + if (feat_found) + { + /* Get the configuration from the page where step detector feature resides */ + rslt = bmi2_get_feat_config(step_det_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of step detector */ + idx = step_det_config.start_addr + BMI2_STEP_COUNT_FEAT_EN_OFFSET; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_STEP_DET_FEAT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable step counter feature. + */ +static int8_t set_step_counter(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step counter */ + struct bmi2_feature_config step_count_config = { 0, 0, 0 }; + + /* Search for step counter feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); + if (feat_found) + { + /* Get the configuration from the page where step-counter feature resides */ + rslt = bmi2_get_feat_config(step_count_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of step counter */ + idx = step_count_config.start_addr + BMI2_STEP_COUNT_FEAT_EN_OFFSET; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_STEP_COUNT_FEAT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable sig-motion feature. + */ +static int8_t set_sig_motion(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for sig-motion */ + struct bmi2_feature_config sig_mot_config = { 0, 0, 0 }; + + /* Search for sig-motion feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&sig_mot_config, BMI2_SIG_MOTION, dev); + if (feat_found) + { + /* Get the configuration from the page where sig-motion feature resides */ + rslt = bmi2_get_feat_config(sig_mot_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of sig-motion */ + idx = sig_mot_config.start_addr + BMI2_SIG_MOT_FEAT_EN_OFFSET; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], BMI2_SIG_MOT_FEAT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable step activity detection. + */ +static int8_t set_step_activity(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step activity */ + struct bmi2_feature_config step_act_config = { 0, 0, 0 }; + + /* Search for step activity feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_act_config, BMI2_STEP_ACTIVITY, dev); + if (feat_found) + { + /* Get the configuration from the page where step-activity + * feature resides + */ + rslt = bmi2_get_feat_config(step_act_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of step activity */ + idx = step_act_config.start_addr + BMI2_STEP_COUNT_FEAT_EN_OFFSET; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_STEP_ACT_FEAT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gives an option to enable self-offset correction + * feature of gyroscope, either internally or by the host. + */ +static int8_t set_gyro_self_offset_corr(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for self-offset correction */ + struct bmi2_feature_config self_off_corr_cfg = { 0, 0, 0 }; + + /* Search for self-offset correction and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&self_off_corr_cfg, BMI2_GYRO_SELF_OFF, dev); + if (feat_found) + { + /* Get the configuration from the page where self-offset + * correction feature resides + */ + rslt = bmi2_get_feat_config(self_off_corr_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of self-offset correction */ + idx = self_off_corr_cfg.start_addr; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_GYR_SELF_OFF_CORR_FEAT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API enables the wrist gesture feature. + */ +static int8_t set_wrist_gesture(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for wrist gesture */ + struct bmi2_feature_config wrist_gest_cfg = { 0, 0, 0 }; + + /* Search for wrist gesture and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&wrist_gest_cfg, BMI2_WRIST_GESTURE, dev); + if (feat_found) + { + /* Get the configuration from the page where wrist gesture feature resides */ + rslt = bmi2_get_feat_config(wrist_gest_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of wrist gesture */ + idx = wrist_gest_cfg.start_addr; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_WRIST_GEST_FEAT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API enables the wrist wear wake up feature. + */ +static int8_t set_wrist_wear_wake_up(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for wrist wear wake up */ + struct bmi2_feature_config wrist_wake_up_cfg = { 0, 0, 0 }; + + /* Search for wrist wear wake up and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&wrist_wake_up_cfg, BMI2_WRIST_WEAR_WAKE_UP, dev); + if (feat_found) + { + /* Get the configuration from the page where wrist wear wake up + * feature resides + */ + rslt = bmi2_get_feat_config(wrist_wake_up_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of wrist wear wake up */ + idx = wrist_wake_up_cfg.start_addr; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_WRIST_WEAR_WAKE_UP_FEAT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable gyroscope user gain + * feature. + */ +static int8_t set_gyro_user_gain(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for gyroscope user gain */ + struct bmi2_feature_config gyr_user_gain_cfg = { 0, 0, 0 }; + + /* Search for user gain feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&gyr_user_gain_cfg, BMI2_GYRO_GAIN_UPDATE, dev); + if (feat_found) + { + /* Get the configuration from the page where user gain feature resides */ + rslt = bmi2_get_feat_config(gyr_user_gain_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of user gain */ + idx = gyr_user_gain_cfg.start_addr + BMI2_GYR_USER_GAIN_FEAT_EN_OFFSET; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_GYR_USER_GAIN_FEAT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API sets any-motion configurations like axes select, + * duration, threshold and output-configuration. + */ +static int8_t set_any_motion_config(const struct bmi2_any_motion_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define count */ + uint8_t index = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for any motion */ + struct bmi2_feature_config any_mot_config = { 0, 0, 0 }; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for any-motion feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&any_mot_config, BMI2_ANY_MOTION, dev); + if (feat_found) + { + /* Get the configuration from the page where any-motion feature resides */ + rslt = bmi2_get_feat_config(any_mot_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for any-motion select */ + idx = any_mot_config.start_addr; + + /* Get offset in words since all the features are set in words length */ + idx = idx / 2; + + /* Set duration */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_ANY_NO_MOT_DUR, config->duration); + + /* Set x-select */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_ANY_NO_MOT_X_SEL, config->select_x); + + /* Set y-select */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_ANY_NO_MOT_Y_SEL, config->select_y); + + /* Set z-select */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_ANY_NO_MOT_Z_SEL, config->select_z); + + /* Increment offset by 1 word to set threshold and output configuration */ + idx++; + + /* Set threshold */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_ANY_NO_MOT_THRES, config->threshold); + + /* Increment offset by 1 more word to get the total length in words */ + idx++; + + /* Get total length in bytes to copy from local pointer to the array */ + idx = (uint8_t)(idx * 2) - any_mot_config.start_addr; + + /* Copy the bytes to be set back to the array */ + for (index = 0; index < idx; index++) + { + feat_config[any_mot_config.start_addr + + index] = *((uint8_t *) data_p + any_mot_config.start_addr + index); + } + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API sets no-motion configurations like axes select, + * duration, threshold and output-configuration. + */ +static int8_t set_no_motion_config(const struct bmi2_no_motion_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define count */ + uint8_t index = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for no-motion */ + struct bmi2_feature_config no_mot_config = { 0, 0, 0 }; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for no-motion feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&no_mot_config, BMI2_NO_MOTION, dev); + if (feat_found) + { + /* Get the configuration from the page where no-motion feature resides */ + rslt = bmi2_get_feat_config(no_mot_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for no-motion select */ + idx = no_mot_config.start_addr; + + /* Get offset in words since all the features are set in words length */ + idx = idx / 2; + + /* Set duration */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_ANY_NO_MOT_DUR, config->duration); + + /* Set x-select */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_ANY_NO_MOT_X_SEL, config->select_x); + + /* Set y-select */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_ANY_NO_MOT_Y_SEL, config->select_y); + + /* Set z-select */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_ANY_NO_MOT_Z_SEL, config->select_z); + + /* Increment offset by 1 word to set threshold and output configuration */ + idx++; + + /* Set threshold */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_ANY_NO_MOT_THRES, config->threshold); + + /* Increment offset by 1 more word to get the total length in words */ + idx++; + + /* Get total length in bytes to copy from local pointer to the array */ + idx = (uint8_t)(idx * 2) - no_mot_config.start_addr; + + /* Copy the bytes to be set back to the array */ + for (index = 0; index < idx; index++) + { + feat_config[no_mot_config.start_addr + + index] = *((uint8_t *) data_p + no_mot_config.start_addr + index); + } + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API sets sig-motion configurations like block-size, + * output-configuration and other parameters. + */ +static int8_t set_sig_motion_config(const struct bmi2_sig_motion_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define index */ + uint8_t index = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for sig-motion */ + struct bmi2_feature_config sig_mot_config = { 0, 0, 0 }; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for sig-motion feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&sig_mot_config, BMI2_SIG_MOTION, dev); + if (feat_found) + { + /* Get the configuration from the page where sig-motion feature resides */ + rslt = bmi2_get_feat_config(sig_mot_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for sig-motion select */ + idx = sig_mot_config.start_addr; + + /* Get offset in words since all the features are set in words length */ + idx = idx / 2; + + /* Set parameter 1 */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_SIG_MOT_PARAM_1, config->block_size); + + /* Increment offset by 1 word to set parameter 2 */ + idx++; + + /* Set parameter 2 */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_SIG_MOT_PARAM_2, config->param_2); + + /* Increment offset by 1 word to set parameter 3 */ + idx++; + + /* Set parameter 3 */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_SIG_MOT_PARAM_3, config->param_3); + + /* Increment offset by 1 word to set parameter 4 */ + idx++; + + /* Set parameter 4 */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_SIG_MOT_PARAM_4, config->param_4); + + /* Increment offset by 1 word to set parameter 5 */ + idx++; + + /* Set parameter 5 */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_SIG_MOT_PARAM_5, config->param_5); + + /* Increment offset by 1 word to set output- configuration */ + idx++; + + /* Get total length in bytes to copy from local pointer to the array */ + idx = (uint8_t)(idx * 2) - sig_mot_config.start_addr; + + /* Copy the bytes to be set back to the array */ + for (index = 0; index < idx; index++) + { + feat_config[sig_mot_config.start_addr + + index] = *((uint8_t *) data_p + sig_mot_config.start_addr + index); + } + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API sets step counter parameter configurations. + */ +static int8_t set_step_count_params_config(const uint16_t *step_count_params, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define index */ + uint8_t index = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step counter parameters */ + struct bmi2_feature_config step_params_config = { 0, 0, 0 }; + + /* Variable to index the page number */ + uint8_t page_idx; + + /* Variable to define the start page */ + uint8_t start_page; + + /* Variable to define start address of the parameters */ + uint8_t start_addr; + + /* Variable to define number of bytes */ + uint8_t n_bytes = (BMI2_STEP_CNT_N_PARAMS * 2); + + /* Variable to store number of pages */ + uint8_t n_pages = (n_bytes / 16); + + /* Variable to define the end page */ + uint8_t end_page; + + /* Variable to define the remaining bytes to be read */ + uint8_t remain_len; + + /* Variable to define the maximum words(16 bytes or 8 words) to be read in a page */ + uint8_t max_len = 8; + + /* Variable index bytes in a page */ + uint8_t page_byte_idx; + + /* Variable to index the parameters */ + uint8_t param_idx = 0; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for step counter parameter feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_params_config, BMI2_STEP_COUNTER_PARAMS, dev); + if (feat_found) + { + /* Get the start page for the step counter parameters */ + start_page = step_params_config.page; + + /* Get the end page for the step counter parameters */ + end_page = start_page + n_pages; + + /* Get the start address for the step counter parameters */ + start_addr = step_params_config.start_addr; + + /* Get the remaining length of bytes to be read */ + remain_len = (uint8_t)((n_bytes - (n_pages * 16)) + start_addr); + for (page_idx = start_page; page_idx <= end_page; page_idx++) + { + /* Get the configuration from the respective page */ + rslt = bmi2_get_feat_config(page_idx, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Start from address 0x00 when switched to next page */ + if (page_idx > start_page) + { + start_addr = 0; + } + + /* Remaining number of words to be read in the page */ + if (page_idx == end_page) + { + max_len = (remain_len / 2); + } + + /* Get offset in words since all the features are set in words length */ + page_byte_idx = start_addr / 2; + for (; page_byte_idx < max_len;) + { + /* Set parameters 1 to 25 */ + *(data_p + page_byte_idx) = BMI2_SET_BIT_POS0(*(data_p + page_byte_idx), + BMI2_STEP_COUNT_PARAMS, + step_count_params[param_idx]); + + /* Increment offset by 1 word to set to the next parameter */ + page_byte_idx++; + + /* Increment to next parameter */ + param_idx++; + } + + /* Get total length in bytes to copy from local pointer to the array */ + page_byte_idx = (uint8_t)(page_byte_idx * 2) - step_params_config.start_addr; + + /* Copy the bytes to be set back to the array */ + for (index = 0; index < page_byte_idx; index++) + { + feat_config[step_params_config.start_addr + + index] = *((uint8_t *) data_p + step_params_config.start_addr + index); + } + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/* @brief This internal API sets step counter configurations like water-mark + * level, reset-counter and output-configuration step detector and activity. + */ +static int8_t set_step_config(const struct bmi2_step_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define index */ + uint8_t index = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step counter 4 */ + struct bmi2_feature_config step_count_config = { 0, 0, 0 }; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for step counter feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); + if (feat_found) + { + /* Get the configuration from the page where step counter resides */ + rslt = bmi2_get_feat_config(step_count_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes */ + idx = step_count_config.start_addr; + + /* Get offset in words since all the features are set in words length */ + idx = idx / 2; + + /* Set water-mark level */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_STEP_COUNT_WM_LEVEL, config->watermark_level); + + /* Set reset-counter */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_STEP_COUNT_RST_CNT, config->reset_counter); + + /* Increment offset by 1 word to set output + * configuration of step detector and step activity + */ + idx++; + + /* Set step buffer size */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_STEP_BUFFER_SIZE, config->step_buffer_size); + + /* Increment offset by 1 more word to get the total length in words */ + idx++; + + /* Get total length in bytes to copy from local pointer to the array */ + idx = (uint8_t)(idx * 2) - step_count_config.start_addr; + + /* Copy the bytes to be set back to the array */ + for (index = 0; index < idx; index++) + { + feat_config[step_count_config.start_addr + + index] = *((uint8_t *) data_p + step_count_config.start_addr + index); + } + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API sets wrist gesture configurations like wearable-arm, + * and output-configuration. + */ +static int8_t set_wrist_gest_config(const struct bmi2_wrist_gest_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define index */ + uint8_t index = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for wrist gesture */ + struct bmi2_feature_config wrist_gest_config = { 0, 0, 0 }; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for wrist gesture feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&wrist_gest_config, BMI2_WRIST_GESTURE, dev); + if (feat_found) + { + /* Get the configuration from the page where wrist gesture feature resides */ + rslt = bmi2_get_feat_config(wrist_gest_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for gesture select */ + idx = wrist_gest_config.start_addr; + + /* Get offset in words since all the features are set in words length */ + idx = idx / 2; + + /* Set wearable arm */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_WRIST_GEST_WEAR_ARM, config->wearable_arm); + + /* Increment offset by 1 more word to set minimum tilt angle (min_flick_peak) */ + idx++; + *(data_p + idx) = config->min_flick_peak; + + /* Increment offset by 1 more word to set min_flick_samples */ + idx++; + *(data_p + idx) = config->min_flick_samples; + + /* Increment offset by 1 more word to set max time within gesture moment has to be completed */ + idx++; + *(data_p + idx) = config->max_duration; + + /* Increment offset by 1 more word to get the total length in words */ + idx++; + + /* Get total length in bytes to copy from local pointer to the array */ + idx = (uint8_t)(idx * 2) - wrist_gest_config.start_addr; + + /* Copy the bytes to be set back to the array */ + for (index = 0; index < idx; index++) + { + feat_config[wrist_gest_config.start_addr + + index] = *((uint8_t *) data_p + wrist_gest_config.start_addr + index); + } + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API sets wrist wear wake-up configurations like + * output-configuration. + */ +static int8_t set_wrist_wear_wake_up_config(const struct bmi2_wrist_wear_wake_up_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define index */ + uint8_t index = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for wrist wear wake-up */ + struct bmi2_feature_config wrist_wake_up_config = { 0, 0, 0 }; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for wrist wear wake-up feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&wrist_wake_up_config, BMI2_WRIST_WEAR_WAKE_UP, dev); + if (feat_found) + { + /* Get the configuration from the page where wrist wear wake-up feature resides */ + rslt = bmi2_get_feat_config(wrist_wake_up_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for wrist wear wake-up select */ + idx = wrist_wake_up_config.start_addr; + + /* Get offset in words since all the features are set in words length */ + idx = idx / 2; + + *(data_p + idx) = config->min_angle_focus; + + /* Increment offset by 1 more word to set min_angle_nonfocus */ + idx++; + *(data_p + idx) = config->min_angle_nonfocus; + + /* Increment offset by 1 more word to set max_tilt_lr */ + idx++; + *(data_p + idx) = config->max_tilt_lr; + + /* Increment offset by 1 more word to set max_tilt_ll */ + idx++; + *(data_p + idx) = config->max_tilt_ll; + + /* Increment offset by 1 more word to set max_tilt_pd */ + idx++; + *(data_p + idx) = config->max_tilt_pd; + + /* Increment offset by 1 more word to set max_tilt_pu */ + idx++; + *(data_p + idx) = config->max_tilt_pu; + + /* Increment offset by 1 more word to get the total length in words */ + idx++; + + /* Get total length in bytes to copy from local pointer to the array */ + idx = (uint8_t)(idx * 2) - wrist_wake_up_config.start_addr; + + /* Copy the bytes to be set back to the array */ + for (index = 0; index < idx; index++) + { + feat_config[wrist_wake_up_config.start_addr + + index] = *((uint8_t *) data_p + wrist_wake_up_config.start_addr + index); + } + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets any-motion configurations like axes select, + * duration, threshold and output-configuration. + */ +static int8_t get_any_motion_config(struct bmi2_any_motion_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define LSB */ + uint16_t lsb; + + /* Variable to define MSB */ + uint16_t msb; + + /* Variable to define a word */ + uint16_t lsb_msb; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for any-motion */ + struct bmi2_feature_config any_mot_config = { 0, 0, 0 }; + + /* Search for any-motion feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&any_mot_config, BMI2_ANY_MOTION, dev); + if (feat_found) + { + /* Get the configuration from the page where any-motion feature resides */ + rslt = bmi2_get_feat_config(any_mot_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for feature enable for any-motion */ + idx = any_mot_config.start_addr; + + /* Get word to calculate duration, x, y and z select */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get duration */ + config->duration = lsb_msb & BMI2_ANY_NO_MOT_DUR_MASK; + + /* Get x-select */ + config->select_x = (lsb_msb & BMI2_ANY_NO_MOT_X_SEL_MASK) >> BMI2_ANY_NO_MOT_X_SEL_POS; + + /* Get y-select */ + config->select_y = (lsb_msb & BMI2_ANY_NO_MOT_Y_SEL_MASK) >> BMI2_ANY_NO_MOT_Y_SEL_POS; + + /* Get z-select */ + config->select_z = (lsb_msb & BMI2_ANY_NO_MOT_Z_SEL_MASK) >> BMI2_ANY_NO_MOT_Z_SEL_POS; + + /* Get word to calculate threshold, output configuration from the same word */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get threshold */ + config->threshold = lsb_msb & BMI2_ANY_NO_MOT_THRES_MASK; + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets no-motion configurations like axes select, + * duration, threshold and output-configuration. + */ +static int8_t get_no_motion_config(struct bmi2_no_motion_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define LSB */ + uint16_t lsb = 0; + + /* Variable to define MSB */ + uint16_t msb = 0; + + /* Variable to define a word */ + uint16_t lsb_msb = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for no-motion */ + struct bmi2_feature_config no_mot_config = { 0, 0, 0 }; + + /* Search for no-motion feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&no_mot_config, BMI2_NO_MOTION, dev); + if (feat_found) + { + /* Get the configuration from the page where no-motion feature resides */ + rslt = bmi2_get_feat_config(no_mot_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for feature enable for no-motion */ + idx = no_mot_config.start_addr; + + /* Get word to calculate duration, x, y and z select */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get duration */ + config->duration = lsb_msb & BMI2_ANY_NO_MOT_DUR_MASK; + + /* Get x-select */ + config->select_x = (lsb_msb & BMI2_ANY_NO_MOT_X_SEL_MASK) >> BMI2_ANY_NO_MOT_X_SEL_POS; + + /* Get y-select */ + config->select_y = (lsb_msb & BMI2_ANY_NO_MOT_Y_SEL_MASK) >> BMI2_ANY_NO_MOT_Y_SEL_POS; + + /* Get z-select */ + config->select_z = (lsb_msb & BMI2_ANY_NO_MOT_Z_SEL_MASK) >> BMI2_ANY_NO_MOT_Z_SEL_POS; + + /* Get word to calculate threshold, output configuration from the same word */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get threshold */ + config->threshold = lsb_msb & BMI2_ANY_NO_MOT_THRES_MASK; + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets sig-motion configurations like block-size, + * output-configuration and other parameters. + */ +static int8_t get_sig_motion_config(struct bmi2_sig_motion_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define LSB */ + uint16_t lsb = 0; + + /* Variable to define MSB */ + uint16_t msb = 0; + + /* Variable to define a word */ + uint16_t lsb_msb = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration sig-motion */ + struct bmi2_feature_config sig_mot_config = { 0, 0, 0 }; + + /* Search for sig-motion feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&sig_mot_config, BMI2_SIG_MOTION, dev); + if (feat_found) + { + /* Get the configuration from the page where sig-motion feature resides */ + rslt = bmi2_get_feat_config(sig_mot_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for feature enable for sig-motion */ + idx = sig_mot_config.start_addr; + + /* Get word to calculate parameter 1 */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get parameter 1 */ + config->block_size = lsb_msb & BMI2_SIG_MOT_PARAM_1_MASK; + + /* Get word to calculate parameter 2 */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get parameter 2 */ + config->param_2 = lsb_msb & BMI2_SIG_MOT_PARAM_2_MASK; + + /* Get word to calculate parameter 3 */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get parameter 3 */ + config->param_3 = lsb_msb & BMI2_SIG_MOT_PARAM_3_MASK; + + /* Get word to calculate parameter 4 */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get parameter 4 */ + config->param_4 = lsb_msb & BMI2_SIG_MOT_PARAM_4_MASK; + + /* Get word to calculate parameter 5 */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get parameter 5 */ + config->param_5 = lsb_msb & BMI2_SIG_MOT_PARAM_5_MASK; + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets step counter parameter configurations. + */ +static int8_t get_step_count_params_config(uint16_t *step_count_params, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Variable to define LSB */ + uint16_t lsb = 0; + + /* Variable to define MSB */ + uint16_t msb = 0; + + /* Variable to define a word */ + uint16_t lsb_msb = 0; + + /* Initialize feature configuration for step counter 1 */ + struct bmi2_feature_config step_params_config = { 0, 0, 0 }; + + /* Variable to index the page number */ + uint8_t page_idx; + + /* Variable to define the start page */ + uint8_t start_page; + + /* Variable to define start address of the parameters */ + uint8_t start_addr; + + /* Variable to define number of bytes */ + uint8_t n_bytes = (BMI2_STEP_CNT_N_PARAMS * 2); + + /* Variable to store number of pages */ + uint8_t n_pages = (n_bytes / 16); + + /* Variable to define the end page */ + uint8_t end_page; + + /* Variable to define the remaining bytes to be read */ + uint8_t remain_len; + + /* Variable to define the maximum words to be read in a page */ + uint8_t max_len = BMI2_FEAT_SIZE_IN_BYTES; + + /* Variable index bytes in a page */ + uint8_t page_byte_idx; + + /* Variable to index the parameters */ + uint8_t param_idx = 0; + + /* Search for step counter parameter feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_params_config, BMI2_STEP_COUNTER_PARAMS, dev); + if (feat_found) + { + /* Get the start page for the step counter parameters */ + start_page = step_params_config.page; + + /* Get the end page for the step counter parameters */ + end_page = start_page + n_pages; + + /* Get the start address for the step counter parameters */ + start_addr = step_params_config.start_addr; + + /* Get the remaining length of bytes to be read */ + remain_len = (uint8_t)((n_bytes - (n_pages * 16)) + start_addr); + for (page_idx = start_page; page_idx <= end_page; page_idx++) + { + /* Get the configuration from the respective page */ + rslt = bmi2_get_feat_config(page_idx, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Start from address 0x00 when switched to next page */ + if (page_idx > start_page) + { + start_addr = 0; + } + + /* Remaining number of bytes to be read in the page */ + if (page_idx == end_page) + { + max_len = remain_len; + } + + /* Get the offset */ + page_byte_idx = start_addr; + while (page_byte_idx < max_len) + { + /* Get word to calculate the parameter*/ + lsb = (uint16_t) feat_config[page_byte_idx++]; + if (page_byte_idx < max_len) + { + msb = ((uint16_t) feat_config[page_byte_idx++] << 8); + } + + lsb_msb = lsb | msb; + + /* Get parameters 1 to 25 */ + step_count_params[param_idx] = lsb_msb & BMI2_STEP_COUNT_PARAMS_MASK; + + /* Increment to next parameter */ + param_idx++; + } + } + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets step counter/detector/activity configurations. + */ +static int8_t get_step_config(struct bmi2_step_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define LSB */ + uint16_t lsb = 0; + + /* Variable to define MSB */ + uint16_t msb = 0; + + /* Variable to define a word */ + uint16_t lsb_msb = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step counter */ + struct bmi2_feature_config step_count_config = { 0, 0, 0 }; + + /* Search for step counter 4 feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); + if (feat_found) + { + /* Get the configuration from the page where step counter 4 parameter resides */ + rslt = bmi2_get_feat_config(step_count_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for feature enable for step counter/detector/activity */ + idx = step_count_config.start_addr; + + /* Get word to calculate water-mark level and reset counter */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get water-mark level */ + config->watermark_level = lsb_msb & BMI2_STEP_COUNT_WM_LEVEL_MASK; + + /* Get reset counter */ + config->reset_counter = (lsb_msb & BMI2_STEP_COUNT_RST_CNT_MASK) >> BMI2_STEP_COUNT_RST_CNT_POS; + + /* Get word to calculate output configuration of step detector and activity */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + config->step_buffer_size = (lsb_msb & BMI2_STEP_BUFFER_SIZE_MASK) >> BMI2_STEP_BUFFER_SIZE_POS; + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets wrist gesture configurations like wearable-arm, + * and output-configuration. + */ +static int8_t get_wrist_gest_config(struct bmi2_wrist_gest_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for wrist gesture */ + struct bmi2_feature_config wrist_gest_config = { 0, 0, 0 }; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for wrist gesture feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&wrist_gest_config, BMI2_WRIST_GESTURE, dev); + if (feat_found) + { + /* Get the configuration from the page where wrist gesture feature resides */ + rslt = bmi2_get_feat_config(wrist_gest_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for wrist gesture select */ + idx = wrist_gest_config.start_addr; + + /* Get offset in words since all the features are set in words length */ + idx = idx / 2; + + /* Get wearable arm */ + config->wearable_arm = (*(data_p + idx) & BMI2_WRIST_GEST_WEAR_ARM_MASK) >> BMI2_WRIST_GEST_WEAR_ARM_POS; + + /* Increment the offset by 1 word to get min_flick_peak */ + idx++; + config->min_flick_peak = *(data_p + idx); + + /* Increment the offset by 1 word to get min_flick_samples */ + idx++; + config->min_flick_samples = *(data_p + idx); + + /* Increment the offset by 1 word to get max_duration */ + idx++; + config->max_duration = *(data_p + idx); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets wrist wear wake-up configurations like + * output-configuration. + */ +static int8_t get_wrist_wear_wake_up_config(struct bmi2_wrist_wear_wake_up_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for wrist wear wake-up */ + struct bmi2_feature_config wrist_wake_up_config = { 0, 0, 0 }; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for wrist wear wake-up feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&wrist_wake_up_config, BMI2_WRIST_WEAR_WAKE_UP, dev); + if (feat_found) + { + /* Get the configuration from the page where wrist wear wake-up feature resides */ + rslt = bmi2_get_feat_config(wrist_wake_up_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for wrist wear wake-up select */ + idx = wrist_wake_up_config.start_addr; + + /* Get offset in words since all the features are set in words length */ + idx = idx / 2; + + config->min_angle_focus = *(data_p + idx); + + /* Increment the offset value by 1 word to get min_angle_nonfocus */ + idx++; + config->min_angle_nonfocus = *(data_p + idx); + + /* Increment the offset value by 1 word to get max_tilt_lr */ + idx++; + config->max_tilt_lr = *(data_p + idx); + + /* Increment the offset value by 1 word to get max_tilt_ll */ + idx++; + config->max_tilt_ll = *(data_p + idx); + + /* Increment the offset value by 1 word to get max_tilt_pd */ + idx++; + config->max_tilt_pd = *(data_p + idx); + + /* Increment the offset value by 1 word to get max_tilt_pu */ + idx++; + config->max_tilt_pu = *(data_p + idx); + + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets the output values of the wrist gesture. + */ +static int8_t get_wrist_gest_status(uint8_t *wrist_gest, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variables to define index */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature output for wrist gesture */ + struct bmi2_feature_config wrist_gest_out_config = { 0, 0, 0 }; + + /* Search for wrist gesture feature and extract its configuration details */ + feat_found = extract_output_feat_config(&wrist_gest_out_config, BMI2_WRIST_GESTURE, dev); + if (feat_found) + { + /* Get the feature output configuration for wrist gesture */ + rslt = bmi2_get_feat_config(wrist_gest_out_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for wrist gesture output */ + idx = wrist_gest_out_config.start_addr; + + /* Get the wrist gesture output */ + *wrist_gest = feat_config[idx]; + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets the output values of step counter. + */ +static int8_t get_step_counter_output(uint32_t *step_count, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variables to define index */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature output for step counter */ + struct bmi2_feature_config step_cnt_out_config = { 0, 0, 0 }; + + /* Search for step counter output feature and extract its configuration details */ + feat_found = extract_output_feat_config(&step_cnt_out_config, BMI2_STEP_COUNTER, dev); + if (feat_found) + { + /* Get the feature output configuration for step-counter */ + rslt = bmi2_get_feat_config(step_cnt_out_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for step counter output */ + idx = step_cnt_out_config.start_addr; + + /* Get the step counter output in 4 bytes */ + *step_count = (uint32_t) feat_config[idx++]; + *step_count |= ((uint32_t) feat_config[idx++] << 8); + *step_count |= ((uint32_t) feat_config[idx++] << 16); + *step_count |= ((uint32_t) feat_config[idx++] << 24); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets the error status related to NVM. + */ +static int8_t get_nvm_error_status(struct bmi2_nvm_err_status *nvm_err_stat, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variables to define index */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature output for NVM error status */ + struct bmi2_feature_config nvm_err_cfg = { 0, 0, 0 }; + + /* Search for NVM error status feature and extract its configuration details */ + feat_found = extract_output_feat_config(&nvm_err_cfg, BMI2_NVM_STATUS, dev); + if (feat_found) + { + /* Get the feature output configuration for NVM error status */ + rslt = bmi2_get_feat_config(nvm_err_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for NVM error status */ + idx = nvm_err_cfg.start_addr; + + /* Increment index to get the error status */ + idx++; + + /* Error when NVM load action fails */ + nvm_err_stat->load_error = BMI2_GET_BIT_POS0(feat_config[idx], BMI2_NVM_LOAD_ERR_STATUS); + + /* Error when NVM program action fails */ + nvm_err_stat->prog_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_PROG_ERR_STATUS); + + /* Error when NVM erase action fails */ + nvm_err_stat->erase_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_ERASE_ERR_STATUS); + + /* Error when NVM program limit is exceeded */ + nvm_err_stat->exceed_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_END_EXCEED_STATUS); + + /* Error when NVM privilege mode is not acquired */ + nvm_err_stat->privil_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_PRIV_ERR_STATUS); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to get enable status of gyroscope user gain + * update. + */ +static int8_t get_user_gain_upd_status(uint8_t *status, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Variable to check APS status */ + uint8_t aps_stat = 0; + + /* Initialize feature configuration for gyroscope user gain */ + struct bmi2_feature_config gyr_user_gain_cfg = { 0, 0, 0 }; + + /* Search for user gain feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&gyr_user_gain_cfg, BMI2_GYRO_GAIN_UPDATE, dev); + if (feat_found) + { + /* Disable advance power save */ + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + /* Get the configuration from the page where user gain feature resides */ + rslt = bmi2_get_feat_config(gyr_user_gain_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of user gain */ + idx = gyr_user_gain_cfg.start_addr + BMI2_GYR_USER_GAIN_FEAT_EN_OFFSET; + + /* Set the feature enable status */ + *status = BMI2_GET_BITS(feat_config[idx], BMI2_GYR_USER_GAIN_FEAT_EN); + } + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + /* Enable Advance power save if disabled while configuring and not when already disabled */ + if ((rslt == BMI2_OK) && (aps_stat == BMI2_ENABLE)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + + return rslt; +} + +/*! + * @brief This internal API gets the output values of step activity. + */ +static int8_t get_step_activity_output(uint8_t *step_act, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variables to define index */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature output for step activity */ + struct bmi2_feature_config step_act_out_config = { 0, 0, 0 }; + + /* Search for step activity output feature and extract its configuration details */ + feat_found = extract_output_feat_config(&step_act_out_config, BMI2_STEP_ACTIVITY, dev); + if (feat_found) + { + /* Get the feature output configuration for step-activity */ + rslt = bmi2_get_feat_config(step_act_out_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for step activity output */ + idx = step_act_out_config.start_addr; + + /* Get the step activity output */ + *step_act = feat_config[idx]; + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets the error status related to virtual frames. + */ +static int8_t get_vfrm_error_status(struct bmi2_vfrm_err_status *vfrm_err_stat, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variables to define index */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature output for VFRM error status */ + struct bmi2_feature_config vfrm_err_cfg = { 0, 0, 0 }; + + /* Search for VFRM error status feature and extract its configuration details */ + feat_found = extract_output_feat_config(&vfrm_err_cfg, BMI2_VFRM_STATUS, dev); + if (feat_found) + { + /* Get the feature output configuration for VFRM error status */ + rslt = bmi2_get_feat_config(vfrm_err_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for VFRM error status */ + idx = vfrm_err_cfg.start_addr; + + /* Increment index to get the error status */ + idx++; + + /* Internal error while acquiring lock for FIFO */ + vfrm_err_stat->lock_error = BMI2_GET_BITS(feat_config[idx], BMI2_VFRM_LOCK_ERR_STATUS); + + /* Internal error while writing byte into FIFO */ + vfrm_err_stat->write_error = BMI2_GET_BITS(feat_config[idx], BMI2_VFRM_WRITE_ERR_STATUS); + + /* Internal error while writing into FIFO */ + vfrm_err_stat->fatal_error = BMI2_GET_BITS(feat_config[idx], BMI2_VFRM_FATAL_ERR_STATUS); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API enables/disables compensation of the gain defined + * in the GAIN register. + */ +static int8_t enable_gyro_gain(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define register data */ + uint8_t reg_data = 0; + + rslt = bmi2_get_regs(BMI2_GYR_OFF_COMP_6_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_GAIN_EN, enable); + rslt = bmi2_set_regs(BMI2_GYR_OFF_COMP_6_ADDR, ®_data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This internal API is used to extract the output feature configuration + * details from the look-up table. + */ +static uint8_t extract_output_feat_config(struct bmi2_feature_config *feat_output, + uint8_t type, + const struct bmi2_dev *dev) +{ + /* Variable to define loop */ + uint8_t loop = 0; + + /* Variable to set flag */ + uint8_t feat_found = BMI2_FALSE; + + /* Search for the output feature from the output configuration array */ + while (loop < dev->out_sens) + { + if (dev->feat_output[loop].type == type) + { + *feat_output = dev->feat_output[loop]; + feat_found = BMI2_TRUE; + break; + } + + loop++; + } + + /* Return flag */ + return feat_found; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.h b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.h index 7179ac8ed..f42676d65 100644 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.h +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.h @@ -30,11 +30,19 @@ * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * -* @file bmi270.h -* @date 2020-01-10 -* @version v2.46.1 +* @file bmi270.h +* @date 2020-11-04 +* @version v2.63.1 * -*/#ifndef BMI270_H_ +*/ + +/** + * \ingroup bmi2xy + * \defgroup bmi270 BMI270 + * @brief Sensor driver for BMI270 sensor + */ + +#ifndef BMI270_H_ #define BMI270_H_ /*! CPP guard */ @@ -54,57 +62,82 @@ extern "C" { ****************************************************************************/ /*! @name BMI270 Chip identifier */ -#define BMI270_CHIP_ID UINT8_C(0x24) +#define BMI270_CHIP_ID UINT8_C(0x24) /*! @name BMI270 feature input start addresses */ -#define BMI270_MAX_BURST_LEN_STRT_ADDR UINT8_C(0x02) -#define BMI270_CRT_GYRO_SELF_TEST_STRT_ADDR UINT8_C(0x03) -#define BMI270_ABORT_STRT_ADDR UINT8_C(0x03) -#define BMI270_AXIS_MAP_STRT_ADDR UINT8_C(0x04) -#define BMI270_GYRO_SELF_OFF_STRT_ADDR UINT8_C(0x05) -#define BMI270_NVM_PROG_PREP_STRT_ADDR UINT8_C(0x05) -#define BMI270_GYRO_GAIN_UPDATE_STRT_ADDR UINT8_C(0x06) -#define BMI270_ANY_MOT_STRT_ADDR UINT8_C(0x0C) -#define BMI270_NO_MOT_STRT_ADDR UINT8_C(0x00) -#define BMI270_SIG_MOT_STRT_ADDR UINT8_C(0x04) -#define BMI270_STEP_CNT_1_STRT_ADDR UINT8_C(0x00) -#define BMI270_STEP_CNT_4_STRT_ADDR UINT8_C(0x02) -#define BMI270_WRIST_GEST_STRT_ADDR UINT8_C(0x06) -#define BMI270_WRIST_WEAR_WAKE_UP_STRT_ADDR UINT8_C(0x00) +#define BMI270_CONFIG_ID_STRT_ADDR UINT8_C(0x00) +#define BMI270_MAX_BURST_LEN_STRT_ADDR UINT8_C(0x02) +#define BMI270_CRT_GYRO_SELF_TEST_STRT_ADDR UINT8_C(0x03) +#define BMI270_ABORT_STRT_ADDR UINT8_C(0x03) +#define BMI270_AXIS_MAP_STRT_ADDR UINT8_C(0x04) +#define BMI270_GYRO_SELF_OFF_STRT_ADDR UINT8_C(0x05) +#define BMI270_NVM_PROG_PREP_STRT_ADDR UINT8_C(0x05) +#define BMI270_GYRO_GAIN_UPDATE_STRT_ADDR UINT8_C(0x06) +#define BMI270_ANY_MOT_STRT_ADDR UINT8_C(0x0C) +#define BMI270_NO_MOT_STRT_ADDR UINT8_C(0x00) +#define BMI270_SIG_MOT_STRT_ADDR UINT8_C(0x04) +#define BMI270_STEP_CNT_1_STRT_ADDR UINT8_C(0x00) +#define BMI270_STEP_CNT_4_STRT_ADDR UINT8_C(0x02) +#define BMI270_WRIST_GEST_STRT_ADDR UINT8_C(0x06) +#define BMI270_WRIST_WEAR_WAKE_UP_STRT_ADDR UINT8_C(0x00) /*! @name BMI270 feature output start addresses */ -#define BMI270_STEP_CNT_OUT_STRT_ADDR UINT8_C(0x00) -#define BMI270_STEP_ACT_OUT_STRT_ADDR UINT8_C(0x04) -#define BMI270_WRIST_GEST_OUT_STRT_ADDR UINT8_C(0x06) -#define BMI270_GYR_USER_GAIN_OUT_STRT_ADDR UINT8_C(0x08) -#define BMI270_GYRO_CROSS_SENSE_STRT_ADDR UINT8_C(0x0C) -#define BMI270_NVM_VFRM_OUT_STRT_ADDR UINT8_C(0x0E) +#define BMI270_STEP_CNT_OUT_STRT_ADDR UINT8_C(0x00) +#define BMI270_STEP_ACT_OUT_STRT_ADDR UINT8_C(0x04) +#define BMI270_WRIST_GEST_OUT_STRT_ADDR UINT8_C(0x06) +#define BMI270_GYR_USER_GAIN_OUT_STRT_ADDR UINT8_C(0x08) +#define BMI270_GYRO_CROSS_SENSE_STRT_ADDR UINT8_C(0x0C) +#define BMI270_NVM_VFRM_OUT_STRT_ADDR UINT8_C(0x0E) /*! @name Defines maximum number of pages */ -#define BMI270_MAX_PAGE_NUM UINT8_C(8) +#define BMI270_MAX_PAGE_NUM UINT8_C(8) /*! @name Defines maximum number of feature input configurations */ -#define BMI270_MAX_FEAT_IN UINT8_C(16) +#define BMI270_MAX_FEAT_IN UINT8_C(17) /*! @name Defines maximum number of feature outputs */ -#define BMI270_MAX_FEAT_OUT UINT8_C(7) +#define BMI270_MAX_FEAT_OUT UINT8_C(7) /*! @name Mask definitions for feature interrupt status bits */ -#define BMI270_SIG_MOT_STATUS_MASK UINT8_C(0x01) -#define BMI270_STEP_CNT_STATUS_MASK UINT8_C(0x02) -#define BMI270_STEP_ACT_STATUS_MASK UINT8_C(0x04) -#define BMI270_WRIST_WAKE_UP_STATUS_MASK UINT8_C(0x08) -#define BMI270_WRIST_GEST_STATUS_MASK UINT8_C(0x10) -#define BMI270_NO_MOT_STATUS_MASK UINT8_C(0x20) -#define BMI270_ANY_MOT_STATUS_MASK UINT8_C(0x40) +#define BMI270_SIG_MOT_STATUS_MASK UINT8_C(0x01) +#define BMI270_STEP_CNT_STATUS_MASK UINT8_C(0x02) +#define BMI270_STEP_ACT_STATUS_MASK UINT8_C(0x04) +#define BMI270_WRIST_WAKE_UP_STATUS_MASK UINT8_C(0x08) +#define BMI270_WRIST_GEST_STATUS_MASK UINT8_C(0x10) +#define BMI270_NO_MOT_STATUS_MASK UINT8_C(0x20) +#define BMI270_ANY_MOT_STATUS_MASK UINT8_C(0x40) + +/*! @name Mask definitions for feature interrupt mapping bits */ +#define BMI270_INT_SIG_MOT_MASK UINT8_C(0x01) +#define BMI270_INT_STEP_COUNTER_MASK UINT8_C(0x02) +#define BMI270_INT_STEP_DETECTOR_MASK UINT8_C(0x02) +#define BMI270_INT_STEP_ACT_MASK UINT8_C(0x04) +#define BMI270_INT_WRIST_WEAR_WAKEUP_MASK UINT8_C(0x08) +#define BMI270_INT_WRIST_GEST_MASK UINT8_C(0x10) +#define BMI270_INT_NO_MOT_MASK UINT8_C(0x20) +#define BMI270_INT_ANY_MOT_MASK UINT8_C(0x40) + +/*! @name Defines maximum number of feature interrupts */ +#define BMI270_MAX_INT_MAP UINT8_C(8) /***************************************************************************/ /*! BMI270 User Interface function prototypes ****************************************************************************/ +/** + * \ingroup bmi270 + * \defgroup bmi270ApiInit Initialization + * @brief Initialize the sensor and device structure + */ + /*! - * @brief This API: + * \ingroup bmi270ApiInit + * \page bmi270_api_bmi270_init bmi270_init + * \code + * int8_t bmi270_init(struct bmi2_dev *dev); + * \endcode + * @details This API: * 1) updates the device structure with address of the configuration file. * 2) Initializes BMI270 sensor. * 3) Writes the configuration file. @@ -114,14 +147,271 @@ extern "C" { * @param[in, out] dev : Structure instance of bmi2_dev. * * @return Result of API execution status - * - * @retval BMI2_OK - Success - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_DEV_NOT_FOUND - Invalid device + * @retval 0 -> Success + * @retval < 0 -> Fail */ int8_t bmi270_init(struct bmi2_dev *dev); +/** + * \ingroup bmi270 + * \defgroup bmi270ApiSensor Feature Set + * @brief Enable / Disable features of the sensor + */ + +/*! + * \ingroup bmi270ApiSensor + * \page bmi270_api_bmi270_sensor_enable bmi270_sensor_enable + * \code + * int8_t bmi270_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API selects the sensors/features to be enabled. + * + * @param[in] sens_list : Pointer to select the sensor/feature. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features that can be enabled. + * + *@verbatim + * sens_list | Values + * ----------------------------|----------- + * BMI2_ACCEL | 0 + * BMI2_GYRO | 1 + * BMI2_AUX | 2 + * BMI2_SIG_MOTION | 3 + * BMI2_ANY_MOTION | 4 + * BMI2_NO_MOTION | 5 + * BMI2_STEP_DETECTOR | 6 + * BMI2_STEP_COUNTER | 7 + * BMI2_STEP_ACTIVITY | 8 + * BMI2_GYRO_GAIN_UPDATE | 9 + * BMI2_WRIST_GESTURE | 19 + * BMI2_WRIST_WEAR_WAKE_UP | 20 + * BMI2_GYRO_SELF_OFF | 33 + *@endverbatim + * + * @note : + * example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO}; + * uint8_t n_sens = 2; + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + +/*! + * \ingroup bmi270ApiSensor + * \page bmi270_api_bmi270_sensor_disable bmi270_sensor_disable + * \code + * int8_t bmi270_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API selects the sensors/features to be disabled. + * + * @param[in] sens_list : Pointer to select the sensor/feature. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features that can be disabled. + * + *@verbatim + * sens_list | Values + * ----------------------------|----------- + * BMI2_ACCEL | 0 + * BMI2_GYRO | 1 + * BMI2_AUX | 2 + * BMI2_SIG_MOTION | 3 + * BMI2_ANY_MOTION | 4 + * BMI2_NO_MOTION | 5 + * BMI2_STEP_DETECTOR | 6 + * BMI2_STEP_COUNTER | 7 + * BMI2_STEP_ACTIVITY | 8 + * BMI2_GYRO_GAIN_UPDATE | 9 + * BMI2_WRIST_GESTURE | 19 + * BMI2_WRIST_WEAR_WAKE_UP | 20 + * BMI2_GYRO_SELF_OFF | 33 + *@endverbatim + * + * @note : + * example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO}; + * uint8_t n_sens = 2; + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + +/** + * \ingroup bmi270 + * \defgroup bmi270ApiSensorC Sensor Configuration + * @brief Enable / Disable feature configuration of the sensor + */ + +/*! + * \ingroup bmi270ApiSensorC + * \page bmi270_api_bmi270_set_sensor_config bmi270_set_sensor_config + * \code + * int8_t bmi270_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API sets the sensor/feature configuration. + * + * @param[in] sens_cfg : Structure instance of bmi2_sens_config. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features that can be configured + * + *@verbatim + * sens_list | Values + * ----------------------------|----------- + * BMI2_SIG_MOTION | 3 + * BMI2_ANY_MOTION | 4 + * BMI2_NO_MOTION | 5 + * BMI2_STEP_DETECTOR | 6 + * BMI2_STEP_COUNTER | 7 + * BMI2_STEP_ACTIVITY | 8 + * BMI2_WRIST_GESTURE | 19 + * BMI2_WRIST_WEAR_WAKE_UP | 20 + * BMI2_STEP_COUNTER_PARAMS | 28 + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + +/*! + * \ingroup bmi270ApiSensorC + * \page bmi270_api_bmi270_get_sensor_config bmi270_get_sensor_config + * \code + * int8_t bmi270_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API gets the sensor/feature configuration. + * + * @param[in] sens_cfg : Structure instance of bmi2_sens_config. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features whose configurations can be read. + * + *@verbatim + * sens_list | Values + * ----------------------------|----------- + * BMI2_SIG_MOTION | 3 + * BMI2_ANY_MOTION | 4 + * BMI2_NO_MOTION | 5 + * BMI2_STEP_DETECTOR | 6 + * BMI2_STEP_COUNTER | 7 + * BMI2_STEP_ACTIVITY | 8 + * BMI2_WRIST_GESTURE | 19 + * BMI2_WRIST_WEAR_WAKE_UP | 20 + * BMI2_STEP_COUNTER_PARAMS | 28 + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + +/** + * \ingroup bmi270 + * \defgroup bmi270ApiSensorD Sensor Data + * @brief Get sensor data + */ + +/*! + * \ingroup bmi270ApiSensorD + * \page bmi270_api_bmi270_get_sensor_data bmi270_get_sensor_data + * \code + * int8_t bmi270_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API gets the sensor/feature data for accelerometer, gyroscope, + * auxiliary sensor, step counter, high-g, gyroscope user-gain update, + * orientation, gyroscope cross sensitivity and error status for NVM and VFRM. + * + * @param[out] sensor_data : Structure instance of bmi2_sensor_data. + * @param[in] n_sens : Number of sensors selected. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features whose data can be read + * + *@verbatim + * sens_list | Values + * ----------------------------|----------- + * BMI2_STEP_COUNTER | 7 + * BMI2_STEP_ACTIVITY | 8 + * BMI2_WRIST_GESTURE | 19 + * BMI2_NVM_STATUS | 38 + * BMI2_VFRM_STATUS | 39 + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev); + +/** + * \ingroup bmi270 + * \defgroup bmi270ApiGyroUG Gyro User Gain + * @brief Set / Get Gyro User Gain of the sensor + */ + +/*! + * \ingroup bmi270ApiGyroUG + * \page bmi270_api_bmi270_update_gyro_user_gain bmi270_update_gyro_user_gain + * \code + * int8_t bmi270_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev); + * \endcode + * @details This API updates the gyroscope user-gain. + * + * @param[in] user_gain : Structure that stores user-gain configurations. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev); + +/*! + * \ingroup bmi270ApiGyroUG + * \page bmi270_api_bmi270_read_gyro_user_gain bmi270_read_gyro_user_gain + * \code + * int8_t bmi270_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, const struct bmi2_dev *dev); + * \endcode + * @details This API reads the compensated gyroscope user-gain values. + * + * @param[out] gyr_usr_gain : Structure that stores gain values. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, struct bmi2_dev *dev); + +/*! + * \ingroup bmi270ApiInt + * \page bmi270_api_bmi270_map_feat_int bmi270_map_feat_int + * \code + * int8_t bmi270_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev) + * \endcode + * @details This API maps/unmaps feature interrupts to that of interrupt pins. + * + * @param[in] sens_int : Structure instance of bmi2_sens_int_config. + * @param[in] n_sens : Number of interrupts to be mapped. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev); + /******************************************************************************/ /*! @name C++ Guard Macros */ /******************************************************************************/ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_context.c b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_context.c new file mode 100644 index 000000000..4f6e30035 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_context.c @@ -0,0 +1,3122 @@ +/** +* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. +* +* BSD-3-Clause +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* 3. Neither the name of the copyright holder nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING +* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* @file bmi270_context.c +* @date 2020-11-04 +* @version v2.63.1 +* +*/ + +/***************************************************************************/ + +/*! Header files + ****************************************************************************/ +#include "bmi270_context.h" + +/***************************************************************************/ + +/*! Global Variable + ****************************************************************************/ + +/*! @name Global array that stores the configuration file of BMI270_CONTEXT */ +const uint8_t bmi270_context_config_file[] = { + 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x00, 0xb0, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0xc9, + 0x01, 0x80, 0x2e, 0xe2, 0x00, 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x77, 0xb0, 0x50, 0x30, 0x21, 0x2e, 0x59, 0xf5, + 0x10, 0x30, 0x21, 0x2e, 0x6a, 0xf5, 0x80, 0x2e, 0xaf, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0x09, 0x01, 0x00, 0x22, + 0x00, 0x76, 0x00, 0x00, 0x10, 0x00, 0x10, 0xd1, 0x00, 0xcb, 0xa7, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0xfd, 0x2d, 0x2c, 0x56, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x02, 0x08, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x04, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1a, 0x24, 0x22, 0x00, 0x80, 0x2e, 0x48, 0x02, 0x01, 0x2e, 0x49, + 0xf1, 0x0b, 0xbc, 0x10, 0x50, 0x0f, 0xb8, 0x00, 0x90, 0xfb, 0x7f, 0x07, 0x2f, 0x03, 0x2e, 0x21, 0xf2, 0x02, 0x31, + 0x4a, 0x0a, 0x23, 0x2e, 0x21, 0xf2, 0x09, 0x2c, 0x00, 0x30, 0x98, 0x2e, 0x0e, 0xc7, 0x03, 0x2e, 0x21, 0xf2, 0xf2, + 0x3e, 0x4a, 0x08, 0x23, 0x2e, 0x21, 0xf2, 0xfb, 0x6f, 0xf0, 0x5f, 0xb8, 0x2e, 0x13, 0x52, 0x00, 0x2e, 0x60, 0x40, + 0x41, 0x40, 0x0d, 0xbc, 0x98, 0xbc, 0xc0, 0x2e, 0x01, 0x0a, 0x0f, 0xb8, 0x43, 0x86, 0x25, 0x40, 0x04, 0x40, 0xd8, + 0xbe, 0x2c, 0x0b, 0x22, 0x11, 0x54, 0x42, 0x03, 0x80, 0x4b, 0x0e, 0xf6, 0x2f, 0xb8, 0x2e, 0x20, 0x50, 0xe7, 0x7f, + 0xf6, 0x7f, 0x46, 0x30, 0x0f, 0x2e, 0xa4, 0xf1, 0xbe, 0x09, 0x80, 0xb3, 0x06, 0x2f, 0x0d, 0x2e, 0x84, 0x00, 0x84, + 0xaf, 0x02, 0x2f, 0x16, 0x30, 0x2d, 0x2e, 0x7b, 0x00, 0x86, 0x30, 0x2d, 0x2e, 0x60, 0xf5, 0xf6, 0x6f, 0xe7, 0x6f, + 0xe0, 0x5f, 0xc8, 0x2e, 0x80, 0x2e, 0xfb, 0x00, 0x00, 0x30, 0xc0, 0x2e, 0x21, 0x2e, 0x8d, 0x00, 0x44, 0x47, 0x99, + 0x00, 0xff, 0x3f, 0x00, 0x0c, 0xff, 0x0f, 0x00, 0x04, 0xc0, 0x00, 0x5b, 0xf5, 0x90, 0x00, 0x1e, 0xf2, 0xfd, 0xf5, + 0x8e, 0x00, 0x96, 0x00, 0x96, 0x00, 0xe0, 0x00, 0x19, 0xf4, 0x66, 0xf5, 0x00, 0x18, 0x64, 0xf5, 0x9d, 0x00, 0x7f, + 0x00, 0x81, 0x00, 0xae, 0x00, 0xff, 0xfb, 0x21, 0x02, 0x00, 0x10, 0x00, 0x40, 0xff, 0x00, 0x00, 0x80, 0xff, 0x7f, + 0x54, 0x0f, 0xeb, 0x00, 0x7f, 0xff, 0xc2, 0xf5, 0x68, 0xf7, 0xb3, 0xf1, 0x4e, 0x0f, 0x42, 0x0f, 0x48, 0x0f, 0x80, + 0x00, 0x67, 0x0f, 0x58, 0xf7, 0x5b, 0xf7, 0x6a, 0x0f, 0x86, 0x00, 0x59, 0x0f, 0x6c, 0x0f, 0xc6, 0xf1, 0x66, 0x0f, + 0x6c, 0xf7, 0x00, 0xe0, 0x00, 0xff, 0xd1, 0xf5, 0x6e, 0x0f, 0x71, 0x0f, 0xff, 0x03, 0x00, 0xfc, 0xf0, 0x3f, 0xb9, + 0x00, 0x2d, 0xf5, 0xca, 0xf5, 0x8a, 0x00, 0x00, 0x08, 0x71, 0x7d, 0xfe, 0xc0, 0x03, 0x3f, 0x05, 0x3e, 0x49, 0x01, + 0x92, 0x02, 0xf5, 0xd6, 0xe8, 0x63, 0xd3, 0xf8, 0x2e, 0x07, 0x5c, 0xce, 0xa5, 0x67, 0x28, 0x02, 0x4e, 0x01, 0x00, + 0xf0, 0x33, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x15, 0x50, 0x10, + 0x50, 0x17, 0x52, 0x05, 0x2e, 0x7d, 0x00, 0xfb, 0x7f, 0x00, 0x2e, 0x13, 0x40, 0x93, 0x42, 0x41, 0x0e, 0xfb, 0x2f, + 0x98, 0x2e, 0x91, 0x03, 0x98, 0x2e, 0x87, 0xcf, 0x01, 0x2e, 0x89, 0x00, 0x00, 0xb2, 0x08, 0x2f, 0x01, 0x2e, 0x69, + 0xf7, 0xb1, 0x3f, 0x01, 0x08, 0x01, 0x30, 0x23, 0x2e, 0x89, 0x00, 0x21, 0x2e, 0x69, 0xf7, 0xfb, 0x6f, 0xf0, 0x5f, + 0xb8, 0x2e, 0xa0, 0x50, 0x80, 0x7f, 0xe7, 0x7f, 0xd5, 0x7f, 0xc4, 0x7f, 0xb3, 0x7f, 0xa2, 0x7f, 0x91, 0x7f, 0xf6, + 0x7f, 0x7b, 0x7f, 0x00, 0x2e, 0x01, 0x2e, 0x60, 0xf5, 0x60, 0x7f, 0x98, 0x2e, 0xce, 0x00, 0x62, 0x6f, 0x01, 0x32, + 0x91, 0x08, 0x80, 0xb2, 0x11, 0x2f, 0x00, 0xb2, 0x03, 0x2f, 0x05, 0x2e, 0x18, 0x00, 0x80, 0x90, 0x09, 0x2f, 0x60, + 0x7f, 0x98, 0x2e, 0xf9, 0x00, 0x23, 0x50, 0x01, 0x32, 0x01, 0x42, 0x02, 0x86, 0x60, 0x6f, 0x02, 0x30, 0xc2, 0x42, + 0x23, 0x2e, 0x60, 0xf5, 0x00, 0x90, 0x00, 0x30, 0x01, 0x2f, 0x21, 0x2e, 0x7a, 0x00, 0xf6, 0x6f, 0x91, 0x6f, 0xa2, + 0x6f, 0xb3, 0x6f, 0xc4, 0x6f, 0xd5, 0x6f, 0xe7, 0x6f, 0x7b, 0x6f, 0x80, 0x6f, 0x60, 0x5f, 0xc8, 0x2e, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x2d, 0x01, 0xd4, 0x7b, 0x3b, + 0x01, 0xdb, 0x7a, 0x04, 0x00, 0x3f, 0x7b, 0xcd, 0x6c, 0xc3, 0x04, 0x85, 0x09, 0xc3, 0x04, 0xec, 0xe6, 0x0c, 0x46, + 0x01, 0x00, 0x27, 0x00, 0x19, 0x00, 0x96, 0x00, 0xa0, 0x00, 0x01, 0x00, 0x0c, 0x00, 0xf0, 0x3c, 0x00, 0x01, 0x01, + 0x00, 0x03, 0x00, 0x01, 0x00, 0x0e, 0x00, 0x00, 0x00, 0x01, 0x00, 0x07, 0x09, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x01, 0x00, 0xe1, 0x06, 0x66, 0x0a, 0x0a, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x50, 0x98, 0x2e, 0xd7, 0x0e, 0x50, 0x32, 0x98, 0x2e, + 0x48, 0x03, 0x10, 0x30, 0x21, 0x2e, 0x21, 0xf2, 0x00, 0x30, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x00, 0x2e, 0x01, + 0x80, 0x06, 0xa2, 0xfb, 0x2f, 0x01, 0x2e, 0x9c, 0x00, 0x00, 0xb2, 0x10, 0x2f, 0x01, 0x2e, 0x18, 0x00, 0x00, 0xb2, + 0x0c, 0x2f, 0x01, 0x54, 0x03, 0x52, 0x01, 0x50, 0x98, 0x2e, 0xc2, 0xc0, 0x98, 0x2e, 0xf5, 0xb0, 0x01, 0x50, 0x98, + 0x2e, 0xd5, 0xb6, 0x10, 0x30, 0x21, 0x2e, 0x19, 0x00, 0x01, 0x2e, 0x84, 0x00, 0x04, 0xae, 0x0b, 0x2f, 0x01, 0x2e, + 0x9c, 0x00, 0x00, 0xb2, 0x07, 0x2f, 0x01, 0x52, 0x98, 0x2e, 0x8e, 0x0e, 0x00, 0xb2, 0x02, 0x2f, 0x10, 0x30, 0x21, + 0x2e, 0x79, 0x00, 0x01, 0x2e, 0x79, 0x00, 0x00, 0x90, 0x90, 0x2e, 0x14, 0x03, 0x01, 0x2e, 0x87, 0x00, 0x00, 0xb2, + 0x04, 0x2f, 0x98, 0x2e, 0x2f, 0x0e, 0x00, 0x30, 0x21, 0x2e, 0x7b, 0x00, 0x01, 0x2e, 0x7b, 0x00, 0x00, 0xb2, 0x12, + 0x2f, 0x01, 0x2e, 0x84, 0x00, 0x00, 0x90, 0x02, 0x2f, 0x98, 0x2e, 0x1f, 0x0e, 0x09, 0x2d, 0x98, 0x2e, 0x81, 0x0d, + 0x01, 0x2e, 0x84, 0x00, 0x04, 0x90, 0x02, 0x2f, 0x50, 0x32, 0x98, 0x2e, 0x48, 0x03, 0x00, 0x30, 0x21, 0x2e, 0x7b, + 0x00, 0x01, 0x2e, 0x78, 0x00, 0x00, 0xb2, 0x90, 0x2e, 0x2c, 0x03, 0x01, 0x2e, 0x78, 0x00, 0x81, 0x30, 0x01, 0x08, + 0x00, 0xb2, 0x61, 0x2f, 0x03, 0x2e, 0x24, 0x02, 0x01, 0x2e, 0x84, 0x00, 0x98, 0xbc, 0x98, 0xb8, 0x05, 0xb2, 0x0d, + 0x58, 0x23, 0x2f, 0x07, 0x90, 0x07, 0x54, 0x00, 0x30, 0x37, 0x2f, 0x15, 0x41, 0x04, 0x41, 0xdc, 0xbe, 0x44, 0xbe, + 0xdc, 0xba, 0x2c, 0x01, 0x61, 0x00, 0x0d, 0x56, 0x4a, 0x0f, 0x0c, 0x2f, 0xd1, 0x42, 0x94, 0xb8, 0xc1, 0x42, 0x11, + 0x30, 0x05, 0x2e, 0x6a, 0xf7, 0x2c, 0xbd, 0x2f, 0xb9, 0x80, 0xb2, 0x08, 0x22, 0x98, 0x2e, 0xaf, 0x03, 0x21, 0x2d, + 0x61, 0x30, 0x23, 0x2e, 0x84, 0x00, 0x98, 0x2e, 0xaf, 0x03, 0x00, 0x30, 0x21, 0x2e, 0x5a, 0xf5, 0x18, 0x2d, 0xf1, + 0x7f, 0x50, 0x30, 0x98, 0x2e, 0x48, 0x03, 0x0d, 0x52, 0x05, 0x50, 0x50, 0x42, 0x70, 0x30, 0x0b, 0x54, 0x42, 0x42, + 0x7e, 0x82, 0xf2, 0x6f, 0x80, 0xb2, 0x42, 0x42, 0x05, 0x2f, 0x21, 0x2e, 0x84, 0x00, 0x10, 0x30, 0x98, 0x2e, 0xaf, + 0x03, 0x03, 0x2d, 0x60, 0x30, 0x21, 0x2e, 0x84, 0x00, 0x01, 0x2e, 0x84, 0x00, 0x06, 0x90, 0x18, 0x2f, 0x01, 0x2e, + 0x77, 0x00, 0x09, 0x54, 0x05, 0x52, 0xf0, 0x7f, 0x98, 0x2e, 0x7a, 0xc1, 0xf1, 0x6f, 0x08, 0x1a, 0x40, 0x30, 0x08, + 0x2f, 0x21, 0x2e, 0x84, 0x00, 0x20, 0x30, 0x98, 0x2e, 0x9b, 0x03, 0x50, 0x32, 0x98, 0x2e, 0x48, 0x03, 0x05, 0x2d, + 0x98, 0x2e, 0x38, 0x0e, 0x00, 0x30, 0x21, 0x2e, 0x84, 0x00, 0x00, 0x30, 0x21, 0x2e, 0x78, 0x00, 0x18, 0x2d, 0x01, + 0x2e, 0x84, 0x00, 0x03, 0xaa, 0x01, 0x2f, 0x98, 0x2e, 0x45, 0x0e, 0x01, 0x2e, 0x84, 0x00, 0x3f, 0x80, 0x03, 0xa2, + 0x01, 0x2f, 0x00, 0x2e, 0x02, 0x2d, 0x98, 0x2e, 0x5b, 0x0e, 0x30, 0x30, 0x98, 0x2e, 0xba, 0x03, 0x00, 0x30, 0x21, + 0x2e, 0x79, 0x00, 0x50, 0x32, 0x98, 0x2e, 0x48, 0x03, 0x01, 0x2e, 0x19, 0x00, 0x00, 0xb2, 0x10, 0x2f, 0x01, 0x2e, + 0x85, 0x00, 0x21, 0x2e, 0x90, 0x00, 0x0f, 0x52, 0x7e, 0x82, 0x11, 0x50, 0x41, 0x40, 0x18, 0xb9, 0x11, 0x42, 0x02, + 0x42, 0x02, 0x80, 0x00, 0x2e, 0x01, 0x40, 0x01, 0x42, 0x98, 0x2e, 0xaa, 0x01, 0x00, 0x30, 0x21, 0x2e, 0x19, 0x00, + 0x21, 0x2e, 0x9c, 0x00, 0x80, 0x2e, 0x52, 0x02, 0x21, 0x2e, 0x59, 0xf5, 0x10, 0x30, 0xc0, 0x2e, 0x21, 0x2e, 0x4a, + 0xf1, 0x80, 0x2e, 0x00, 0xc1, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x9a, 0x01, + 0x34, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x03, 0x2e, 0x7d, 0x00, 0x16, 0xb8, 0x02, 0x34, 0x4a, 0x0c, 0x21, 0x2e, 0x2d, 0xf5, 0xc0, 0x2e, 0x23, + 0x2e, 0x7d, 0x00, 0x03, 0xbc, 0x21, 0x2e, 0x85, 0x00, 0x03, 0x2e, 0x85, 0x00, 0x40, 0xb2, 0x10, 0x30, 0x21, 0x2e, + 0x19, 0x00, 0x01, 0x30, 0x05, 0x2f, 0x05, 0x2e, 0x88, 0x00, 0x80, 0x90, 0x01, 0x2f, 0x23, 0x2e, 0x6f, 0xf5, 0xc0, + 0x2e, 0x21, 0x2e, 0x89, 0x00, 0x11, 0x30, 0x81, 0x08, 0x01, 0x2e, 0x6a, 0xf7, 0x71, 0x3f, 0x23, 0xbd, 0x01, 0x08, + 0x02, 0x0a, 0xc0, 0x2e, 0x21, 0x2e, 0x6a, 0xf7, 0x30, 0x25, 0x00, 0x30, 0x21, 0x2e, 0x5a, 0xf5, 0x10, 0x50, 0x21, + 0x2e, 0x7b, 0x00, 0x21, 0x2e, 0x78, 0x00, 0xfb, 0x7f, 0x98, 0x2e, 0xaf, 0x03, 0x40, 0x30, 0x21, 0x2e, 0x84, 0x00, + 0xfb, 0x6f, 0xf0, 0x5f, 0x03, 0x25, 0x80, 0x2e, 0x9b, 0x03, 0x0b, 0x00, 0x94, 0x02, 0x14, 0x24, 0x80, 0x00, 0x04, + 0x00, 0x04, 0x30, 0x08, 0xb8, 0x94, 0x02, 0xc0, 0x2e, 0x28, 0xbd, 0x02, 0x0a, 0x0d, 0x82, 0x02, 0x30, 0x12, 0x42, + 0x41, 0x0e, 0xfc, 0x2f, 0xb8, 0x2e, 0x95, 0x50, 0xc0, 0x2e, 0x21, 0x2e, 0xa9, 0x01, 0x02, 0x30, 0x02, 0x2c, 0x41, + 0x00, 0x12, 0x42, 0x41, 0x0e, 0xfc, 0x2f, 0xb8, 0x2e, 0x13, 0x82, 0x02, 0x30, 0x12, 0x42, 0x41, 0x0e, 0xfc, 0x2f, + 0x3f, 0x80, 0xa1, 0x30, 0xc0, 0x2e, 0x01, 0x42, 0x00, 0x2e, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0xc0, 0x50, 0xe7, 0x7f, + 0xf6, 0x7f, 0x26, 0x30, 0x0f, 0x2e, 0x61, 0xf5, 0x2f, 0x2e, 0x78, 0x00, 0x0f, 0x2e, 0x78, 0x00, 0xbe, 0x09, 0xa2, + 0x7f, 0x80, 0x7f, 0x80, 0xb3, 0xd5, 0x7f, 0xc4, 0x7f, 0xb3, 0x7f, 0x91, 0x7f, 0x7b, 0x7f, 0x0b, 0x2f, 0x19, 0x50, + 0x1a, 0x25, 0x12, 0x40, 0x42, 0x7f, 0x74, 0x82, 0x12, 0x40, 0x52, 0x7f, 0x00, 0x2e, 0x00, 0x40, 0x60, 0x7f, 0x98, + 0x2e, 0x6a, 0xd6, 0x81, 0x30, 0x01, 0x2e, 0x78, 0x00, 0x01, 0x08, 0x00, 0xb2, 0x42, 0x2f, 0x03, 0x2e, 0x24, 0x02, + 0x01, 0x2e, 0x24, 0x02, 0x97, 0xbc, 0x06, 0xbc, 0x9f, 0xb8, 0x0f, 0xb8, 0x00, 0x90, 0x23, 0x2e, 0x88, 0x00, 0x10, + 0x30, 0x01, 0x30, 0x2a, 0x2f, 0x03, 0x2e, 0x84, 0x00, 0x44, 0xb2, 0x05, 0x2f, 0x47, 0xb2, 0x00, 0x30, 0x2d, 0x2f, + 0x21, 0x2e, 0x78, 0x00, 0x2b, 0x2d, 0x03, 0x2e, 0xfd, 0xf5, 0x9e, 0xbc, 0x9f, 0xb8, 0x40, 0x90, 0x14, 0x2f, 0x03, + 0x2e, 0xfc, 0xf5, 0x99, 0xbc, 0x9f, 0xb8, 0x40, 0x90, 0x0e, 0x2f, 0x03, 0x2e, 0x49, 0xf1, 0x1b, 0x54, 0x4a, 0x08, + 0x40, 0x90, 0x08, 0x2f, 0x98, 0x2e, 0xce, 0x00, 0x00, 0xb2, 0x10, 0x30, 0x03, 0x2f, 0x50, 0x30, 0x21, 0x2e, 0x84, + 0x00, 0x10, 0x2d, 0x98, 0x2e, 0x9b, 0x03, 0x00, 0x30, 0x21, 0x2e, 0x78, 0x00, 0x0a, 0x2d, 0x05, 0x2e, 0x69, 0xf7, + 0x2d, 0xbd, 0x2f, 0xb9, 0x80, 0xb2, 0x01, 0x2f, 0x21, 0x2e, 0x79, 0x00, 0x23, 0x2e, 0x78, 0x00, 0xe0, 0x31, 0x21, + 0x2e, 0x61, 0xf5, 0xf6, 0x6f, 0xe7, 0x6f, 0x80, 0x6f, 0xa2, 0x6f, 0xb3, 0x6f, 0xc4, 0x6f, 0xd5, 0x6f, 0x7b, 0x6f, + 0x91, 0x6f, 0x40, 0x5f, 0xc8, 0x2e, 0x90, 0x50, 0xf7, 0x7f, 0xe6, 0x7f, 0xd5, 0x7f, 0xc4, 0x7f, 0xb3, 0x7f, 0xa1, + 0x7f, 0x90, 0x7f, 0x82, 0x7f, 0x7b, 0x7f, 0x98, 0x2e, 0xce, 0x00, 0x00, 0xb2, 0x10, 0x30, 0x49, 0x2f, 0x05, 0x2e, + 0x21, 0x02, 0x03, 0x2e, 0x2d, 0x02, 0x21, 0x56, 0x08, 0x08, 0x93, 0x08, 0x90, 0x0a, 0x25, 0x2e, 0x18, 0x00, 0x05, + 0x2e, 0xc1, 0xf5, 0x2e, 0xbc, 0x05, 0x2e, 0x84, 0x00, 0x84, 0xa2, 0x0e, 0xb8, 0x31, 0x30, 0x88, 0x04, 0x03, 0x2f, + 0x01, 0x2e, 0x18, 0x00, 0x00, 0xb2, 0x0c, 0x2f, 0x1d, 0x50, 0x01, 0x52, 0x98, 0x2e, 0xd7, 0x00, 0x05, 0x2e, 0x7a, + 0x00, 0x80, 0x90, 0x02, 0x2f, 0x10, 0x30, 0x21, 0x2e, 0x7a, 0x00, 0x25, 0x2e, 0x9c, 0x00, 0x05, 0x2e, 0x18, 0x00, + 0x80, 0xb2, 0x20, 0x2f, 0x01, 0x2e, 0xc0, 0xf5, 0xf2, 0x30, 0x02, 0x08, 0x07, 0xaa, 0x73, 0x30, 0x03, 0x2e, 0x7c, + 0x00, 0x18, 0x22, 0x41, 0x1a, 0x05, 0x2f, 0x03, 0x2e, 0x66, 0xf5, 0x9f, 0xbc, 0x9f, 0xb8, 0x40, 0x90, 0x0c, 0x2f, + 0x1f, 0x52, 0x03, 0x30, 0x53, 0x42, 0x2b, 0x30, 0x90, 0x04, 0x5b, 0x42, 0x21, 0x2e, 0x7c, 0x00, 0x24, 0xbd, 0x7e, + 0x80, 0x81, 0x84, 0x43, 0x42, 0x02, 0x42, 0x02, 0x32, 0x25, 0x2e, 0x62, 0xf5, 0x05, 0x2e, 0x86, 0x00, 0x81, 0x84, + 0x25, 0x2e, 0x86, 0x00, 0x02, 0x31, 0x25, 0x2e, 0x60, 0xf5, 0x05, 0x2e, 0x25, 0x02, 0x10, 0x30, 0x90, 0x08, 0x80, + 0xb2, 0x0b, 0x2f, 0x05, 0x2e, 0xca, 0xf5, 0xf0, 0x3e, 0x90, 0x08, 0x25, 0x2e, 0xca, 0xf5, 0x05, 0x2e, 0x59, 0xf5, + 0xe0, 0x3f, 0x90, 0x08, 0x25, 0x2e, 0x59, 0xf5, 0x90, 0x6f, 0xa1, 0x6f, 0xb3, 0x6f, 0xc4, 0x6f, 0xd5, 0x6f, 0xe6, + 0x6f, 0xf7, 0x6f, 0x7b, 0x6f, 0x82, 0x6f, 0x70, 0x5f, 0xc8, 0x2e, 0x2f, 0x52, 0x90, 0x50, 0x53, 0x40, 0x4a, 0x25, + 0x40, 0x40, 0x39, 0x8b, 0xfb, 0x7f, 0x0c, 0xbc, 0x21, 0x52, 0x37, 0x89, 0x0b, 0x30, 0x59, 0x08, 0x0c, 0xb8, 0xe0, + 0x7f, 0x8b, 0x7f, 0x4b, 0x43, 0x0b, 0x43, 0x40, 0xb2, 0xd1, 0x7f, 0x6e, 0x2f, 0x01, 0x2e, 0x83, 0x00, 0x00, 0xb2, + 0x0e, 0x2f, 0x25, 0x52, 0x01, 0x2e, 0x7e, 0x00, 0xc3, 0x7f, 0xb4, 0x7f, 0xa5, 0x7f, 0x98, 0x2e, 0xbb, 0xcc, 0x05, + 0x30, 0x2b, 0x2e, 0x83, 0x00, 0xc3, 0x6f, 0xd1, 0x6f, 0xb4, 0x6f, 0xa5, 0x6f, 0x36, 0xbc, 0x06, 0xb9, 0x35, 0xbc, + 0x0f, 0xb8, 0x94, 0xb0, 0xc6, 0x7f, 0x00, 0xb2, 0x0c, 0x2f, 0x27, 0x50, 0x29, 0x56, 0x0b, 0x30, 0x05, 0x2e, 0x21, + 0x02, 0x2d, 0x5c, 0x1b, 0x42, 0xdb, 0x42, 0x96, 0x08, 0x25, 0x2e, 0x21, 0x02, 0x0b, 0x42, 0xcb, 0x42, 0x00, 0x2e, + 0x31, 0x56, 0xcb, 0x08, 0x25, 0x52, 0x01, 0x2e, 0x7e, 0x00, 0x01, 0x54, 0x2b, 0x5c, 0x98, 0x2e, 0x06, 0xcd, 0xd2, + 0x6f, 0x27, 0x5a, 0x94, 0x6f, 0xa4, 0xbc, 0x53, 0x41, 0x00, 0xb3, 0x1f, 0xb8, 0x44, 0x41, 0x01, 0x30, 0xd5, 0x7f, + 0x05, 0x2f, 0x00, 0xb2, 0x03, 0x2f, 0x29, 0x5c, 0x11, 0x30, 0x93, 0x43, 0x84, 0x43, 0x23, 0xbd, 0x2f, 0xb9, 0x80, + 0xb2, 0x1c, 0x2f, 0x72, 0x6f, 0xda, 0x00, 0x82, 0x6f, 0x22, 0x03, 0x44, 0x43, 0x00, 0x90, 0x27, 0x2e, 0x7f, 0x00, + 0x29, 0x5a, 0x12, 0x2f, 0x29, 0x54, 0x00, 0x2e, 0x90, 0x40, 0x82, 0x40, 0x18, 0x04, 0xa2, 0x06, 0x80, 0xaa, 0x04, + 0x2f, 0x80, 0x90, 0x08, 0x2f, 0xc2, 0x6f, 0x50, 0x0f, 0x05, 0x2f, 0xc0, 0x6f, 0x00, 0xb2, 0x02, 0x2f, 0x53, 0x43, + 0x44, 0x43, 0x11, 0x30, 0xe0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, 0xd1, 0x6f, 0x15, 0x5a, 0x09, 0x2e, 0x7f, 0x00, 0x41, + 0x40, 0x54, 0x43, 0x08, 0x2c, 0x41, 0x43, 0x15, 0x30, 0x2b, 0x2e, 0x83, 0x00, 0x01, 0x30, 0xe0, 0x6f, 0x98, 0x2e, + 0x95, 0xcf, 0x00, 0x2e, 0xfb, 0x6f, 0x70, 0x5f, 0xb8, 0x2e, 0x50, 0x86, 0xcd, 0x88, 0x34, 0x85, 0xc5, 0x40, 0x91, + 0x40, 0x8c, 0x80, 0x06, 0x41, 0x13, 0x40, 0x50, 0x50, 0x6e, 0x01, 0x82, 0x40, 0x04, 0x40, 0x34, 0x8c, 0xfb, 0x7f, + 0x98, 0x2e, 0xce, 0x03, 0xe0, 0x7f, 0x00, 0x2e, 0x91, 0x41, 0x8c, 0x81, 0x82, 0x41, 0x13, 0x40, 0x04, 0x40, 0x34, + 0x8e, 0x98, 0x2e, 0xce, 0x03, 0xc0, 0x7f, 0xd5, 0x7f, 0x13, 0x24, 0xff, 0x00, 0xd6, 0x41, 0xcc, 0x83, 0xc2, 0x41, + 0x57, 0x40, 0x74, 0x80, 0x44, 0x40, 0x11, 0x40, 0x0c, 0x8a, 0xf7, 0x01, 0x94, 0x03, 0x12, 0x24, 0x80, 0x00, 0x3a, + 0x01, 0x02, 0x30, 0xb2, 0x03, 0xce, 0x17, 0xfb, 0x08, 0x23, 0x01, 0xb2, 0x02, 0x48, 0xbb, 0x28, 0xbd, 0xf2, 0x0b, + 0x53, 0x41, 0x02, 0x40, 0x44, 0x41, 0x74, 0x8d, 0xb7, 0x7f, 0x98, 0x2e, 0xce, 0x03, 0x50, 0x25, 0x91, 0x41, 0x8c, + 0x81, 0x82, 0x41, 0x13, 0x40, 0x04, 0x40, 0x34, 0x8e, 0x98, 0x2e, 0xce, 0x03, 0x60, 0x25, 0xd1, 0x41, 0xcc, 0x81, + 0xc2, 0x41, 0x13, 0x40, 0x04, 0x40, 0x98, 0x2e, 0xce, 0x03, 0x11, 0x24, 0xb3, 0x00, 0x71, 0x0e, 0xd3, 0x6f, 0xe1, + 0x6f, 0x33, 0x2f, 0x12, 0x24, 0xdd, 0x00, 0xda, 0x0f, 0x2b, 0x2f, 0x12, 0x24, 0x8c, 0x00, 0x5a, 0x0e, 0x09, 0x2f, + 0x10, 0x24, 0x83, 0x05, 0x48, 0x0e, 0x11, 0x24, 0x7f, 0x22, 0x10, 0x24, 0x18, 0x32, 0x08, 0x22, 0x80, 0x2e, 0xd7, + 0xb4, 0x13, 0x24, 0xf4, 0x00, 0x73, 0x0e, 0x0f, 0x2f, 0x10, 0x24, 0x11, 0x10, 0x68, 0x0e, 0x10, 0x24, 0xa2, 0x30, + 0x13, 0x24, 0x97, 0x23, 0x03, 0x22, 0x13, 0x24, 0x3b, 0x04, 0x4b, 0x0e, 0x11, 0x24, 0x0f, 0x30, 0x01, 0x22, 0x80, + 0x2e, 0xd7, 0xb4, 0x11, 0x24, 0x53, 0x02, 0x41, 0x0e, 0x11, 0x24, 0xe7, 0x31, 0x10, 0x24, 0xfc, 0x25, 0x08, 0x22, + 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0xe8, 0x40, 0x80, 0x2e, 0xd7, 0xb4, 0xf2, 0x37, 0x5a, 0x0e, 0x90, 0x2e, 0x50, + 0xb3, 0x12, 0x24, 0xea, 0x00, 0x4a, 0x0e, 0x90, 0x2e, 0xc7, 0xb2, 0xc2, 0x6f, 0x14, 0x24, 0x4c, 0x0b, 0x54, 0x0e, + 0x90, 0x2e, 0xab, 0xb2, 0x14, 0x24, 0x9b, 0x00, 0x5c, 0x0e, 0x90, 0x2e, 0xa1, 0xb2, 0x14, 0x24, 0x22, 0x01, 0x4c, + 0x0e, 0x70, 0x2f, 0x82, 0xa3, 0x5e, 0x2f, 0x11, 0x24, 0xba, 0x0b, 0x51, 0x0e, 0x35, 0x2f, 0x11, 0x24, 0x03, 0x08, + 0x69, 0x0e, 0x2d, 0x2f, 0xb1, 0x6f, 0x14, 0x24, 0x90, 0x00, 0x0c, 0x0e, 0x24, 0x2f, 0x11, 0x24, 0x31, 0x08, 0x69, + 0x0e, 0x16, 0x2f, 0x11, 0x24, 0x7d, 0x01, 0x59, 0x0e, 0x0e, 0x2f, 0x11, 0x24, 0xd7, 0x0c, 0x51, 0x0e, 0x11, 0x24, + 0x9f, 0x44, 0x13, 0x24, 0x41, 0x57, 0x4b, 0x22, 0x93, 0x35, 0x43, 0x0e, 0x10, 0x24, 0xbd, 0x42, 0x08, 0x22, 0x80, + 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0x1c, 0x42, 0x80, 0x2e, 0xd7, 0xb4, 0x11, 0x24, 0x47, 0x01, 0x59, 0x0e, 0x11, 0x24, + 0xa2, 0x45, 0x10, 0x24, 0x31, 0x51, 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0x80, 0x41, 0x80, 0x2e, 0xd7, + 0xb4, 0x10, 0x24, 0x67, 0x54, 0x80, 0x2e, 0xd7, 0xb4, 0x11, 0x24, 0x8c, 0x08, 0xe9, 0x0f, 0x10, 0x24, 0x0a, 0x48, + 0x90, 0x2e, 0xd7, 0xb4, 0xb1, 0x6f, 0x13, 0x24, 0xe8, 0x03, 0x8b, 0x0f, 0x10, 0x24, 0xcd, 0x57, 0x90, 0x2e, 0xd7, + 0xb4, 0x73, 0x35, 0x8b, 0x0f, 0x10, 0x24, 0x6f, 0x42, 0x90, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0xa0, 0xfe, 0x08, 0x0e, + 0x10, 0x24, 0x38, 0x54, 0x13, 0x24, 0xa3, 0x46, 0x03, 0x22, 0x13, 0x24, 0x45, 0xfd, 0x0b, 0x0e, 0x11, 0x24, 0x04, + 0x43, 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0xb1, 0x6f, 0x00, 0x3a, 0x08, 0x0e, 0x11, 0x24, 0x3d, 0x45, 0x10, 0x24, + 0x52, 0x54, 0x48, 0x22, 0x10, 0x24, 0x8f, 0x01, 0x58, 0x0e, 0x10, 0x24, 0x48, 0x44, 0x01, 0x22, 0x80, 0x2e, 0xd7, + 0xb4, 0xb1, 0x6f, 0x13, 0x24, 0xfa, 0x03, 0x0b, 0x0e, 0x11, 0x24, 0x85, 0x43, 0x13, 0x24, 0x35, 0x55, 0x4b, 0x22, + 0x11, 0xa2, 0x10, 0x24, 0xf6, 0x57, 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x11, 0x24, 0xa4, 0x0a, 0x69, 0x0e, 0x11, + 0x24, 0x7b, 0x5a, 0x10, 0x24, 0x5e, 0x20, 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x11, 0x24, 0x0f, 0x01, 0x59, 0x0e, + 0x0d, 0x2f, 0x18, 0xa2, 0x11, 0x24, 0x2b, 0x47, 0x10, 0x24, 0xf4, 0x55, 0x48, 0x22, 0x10, 0x24, 0x16, 0x0b, 0x50, + 0x0e, 0x10, 0x24, 0xc7, 0x51, 0x01, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x11, 0x24, 0x72, 0x0a, 0x51, 0x0e, 0x11, 0x24, + 0x85, 0x55, 0x10, 0x24, 0xb2, 0x47, 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0x83, 0x00, 0x48, 0x0e, 0x53, + 0x2f, 0x11, 0x24, 0xe1, 0x07, 0x69, 0x0e, 0x2d, 0x2f, 0x95, 0xaf, 0x27, 0x2f, 0x82, 0xaf, 0x21, 0x2f, 0x11, 0x24, + 0xd7, 0x00, 0x59, 0x0e, 0x19, 0x2f, 0xb1, 0x6f, 0x10, 0x24, 0xcc, 0x03, 0x88, 0x0f, 0x10, 0x2f, 0x10, 0x24, 0xe8, + 0xfe, 0x08, 0x0e, 0x11, 0x24, 0x7e, 0x56, 0x10, 0x24, 0x94, 0x45, 0x48, 0x22, 0xc0, 0x6f, 0x13, 0x24, 0x06, 0x0b, + 0x43, 0x0e, 0x10, 0x24, 0x2f, 0x51, 0x01, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0xde, 0x51, 0x80, 0x2e, 0xd7, + 0xb4, 0x10, 0x24, 0xe8, 0x54, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0xa4, 0x52, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, + 0xd0, 0x44, 0x80, 0x2e, 0xd7, 0xb4, 0x11, 0x24, 0xb8, 0x00, 0xd9, 0x0f, 0x19, 0x2f, 0xc1, 0x6f, 0x10, 0x24, 0xe7, + 0x0c, 0xc8, 0x0f, 0x10, 0x2f, 0x11, 0x24, 0xc7, 0x07, 0x69, 0x0e, 0x11, 0x24, 0xf6, 0x52, 0x10, 0x24, 0x7a, 0x12, + 0x48, 0x22, 0xb0, 0x6f, 0x13, 0x24, 0x5d, 0x02, 0x03, 0x0e, 0x10, 0x24, 0x7c, 0x54, 0x01, 0x22, 0x80, 0x2e, 0xd7, + 0xb4, 0x10, 0x24, 0x8d, 0x51, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0x28, 0x52, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, + 0xd2, 0x07, 0xe8, 0x0f, 0x28, 0x2f, 0x10, 0x24, 0xb0, 0x00, 0xd8, 0x0f, 0x20, 0x2f, 0x10, 0x24, 0xc6, 0x07, 0x68, + 0x0e, 0x18, 0x2f, 0x50, 0x35, 0x48, 0x0e, 0x11, 0x2f, 0xb1, 0x6f, 0x10, 0x24, 0xf4, 0x01, 0x08, 0x0e, 0x11, 0x24, + 0x35, 0x51, 0x10, 0x24, 0x22, 0x12, 0x48, 0x22, 0xc0, 0x6f, 0x13, 0x24, 0xe0, 0x0c, 0x43, 0x0e, 0x10, 0x24, 0x7b, + 0x50, 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0x81, 0x52, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0x3b, 0x53, + 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0x63, 0x51, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0x27, 0x51, 0x80, 0x2e, 0xd7, + 0xb4, 0x18, 0xa2, 0x90, 0x2e, 0xdb, 0xb3, 0x12, 0x24, 0x08, 0x02, 0x4a, 0x0e, 0x37, 0x2f, 0x12, 0x24, 0x2a, 0x09, + 0x6a, 0x0e, 0x1d, 0x2f, 0x13, 0x24, 0x8e, 0x00, 0x73, 0x0e, 0x09, 0x2f, 0x11, 0x24, 0xa5, 0x01, 0x41, 0x0e, 0x11, + 0x24, 0x76, 0x32, 0x10, 0x24, 0x12, 0x25, 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0xa9, 0x0d, 0x68, 0x0e, + 0x10, 0x24, 0x04, 0x27, 0x13, 0x24, 0x73, 0x20, 0x03, 0x22, 0x13, 0x24, 0x14, 0x04, 0x4b, 0x0e, 0x11, 0x24, 0x15, + 0x2c, 0x01, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x11, 0x24, 0xae, 0x08, 0x69, 0x0e, 0x08, 0x2f, 0xa1, 0x35, 0x71, 0x0e, + 0x11, 0x24, 0x8b, 0x2b, 0x10, 0x24, 0x07, 0x35, 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x91, 0x34, 0x59, 0x0e, 0x11, + 0x24, 0x7b, 0x19, 0x10, 0x24, 0x50, 0x59, 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x62, 0x32, 0x42, 0x0e, 0x22, 0x2f, + 0xa2, 0x32, 0x5a, 0x0e, 0x1b, 0x2f, 0x12, 0x24, 0x0b, 0x08, 0x6a, 0x0e, 0x0e, 0x2f, 0xa3, 0x34, 0x43, 0x0e, 0x10, + 0x24, 0x28, 0x2b, 0x13, 0x24, 0x20, 0x23, 0x03, 0x22, 0x13, 0x24, 0x8d, 0x01, 0x4b, 0x0e, 0x11, 0x24, 0x5c, 0x21, + 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x31, 0x36, 0x59, 0x0e, 0x11, 0x24, 0x43, 0x25, 0x10, 0x24, 0xfa, 0x49, 0x08, + 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0xc7, 0x2a, 0x80, 0x2e, 0xd7, 0xb4, 0x40, 0x36, 0x58, 0x0e, 0x09, 0x2f, + 0x11, 0x24, 0x9e, 0x08, 0x69, 0x0e, 0x11, 0x24, 0xe3, 0x54, 0x10, 0x24, 0x73, 0x22, 0x08, 0x22, 0x80, 0x2e, 0xd7, + 0xb4, 0x10, 0x24, 0x38, 0x01, 0xc8, 0x0f, 0x10, 0x2f, 0x11, 0x24, 0x11, 0x08, 0x69, 0x0e, 0x11, 0x24, 0x6e, 0x48, + 0x10, 0x24, 0x2b, 0x28, 0x48, 0x22, 0xc0, 0x6f, 0x13, 0x24, 0xc1, 0x0a, 0x43, 0x0e, 0x10, 0x24, 0x0f, 0x23, 0x08, + 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0xd0, 0x1a, 0x80, 0x2e, 0xd7, 0xb4, 0xe2, 0x33, 0x5a, 0x0e, 0x77, 0x2f, + 0x12, 0x24, 0x0c, 0x08, 0x6a, 0x0e, 0x2a, 0x2f, 0x12, 0x24, 0xc5, 0x00, 0x4a, 0x0e, 0x08, 0x2f, 0x11, 0x36, 0x59, + 0x0e, 0x11, 0x24, 0xfd, 0x18, 0x10, 0x24, 0x75, 0x58, 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0xc2, 0x34, 0x5a, 0x0e, + 0x0d, 0x2f, 0x11, 0x24, 0x36, 0x08, 0x69, 0x0e, 0x11, 0x24, 0x08, 0x58, 0x13, 0x24, 0x3b, 0x54, 0x4b, 0x22, 0x01, + 0xa2, 0x10, 0x24, 0xc6, 0x52, 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0xb3, 0x36, 0x4b, 0x0e, 0x11, 0x24, 0x0e, 0x24, + 0x13, 0x24, 0x7b, 0x50, 0x59, 0x22, 0x0e, 0xa2, 0x10, 0x24, 0xf7, 0x56, 0x01, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0xc2, + 0x35, 0x5a, 0x0e, 0x12, 0x2f, 0x01, 0xa2, 0x0c, 0x2f, 0x84, 0xa3, 0x10, 0x24, 0xd4, 0x58, 0x13, 0x24, 0x76, 0x56, + 0x03, 0x22, 0x73, 0x36, 0x4b, 0x0e, 0x11, 0x24, 0xeb, 0x52, 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0x87, + 0x16, 0x80, 0x2e, 0xd7, 0xb4, 0xb0, 0x6f, 0x13, 0x24, 0x02, 0xfd, 0x03, 0x0e, 0x29, 0x2f, 0x84, 0xa3, 0xc0, 0x6f, + 0x09, 0x2f, 0x11, 0x24, 0xe4, 0x0a, 0x41, 0x0e, 0x11, 0x24, 0x5d, 0x44, 0x10, 0x24, 0x2f, 0x5a, 0x08, 0x22, 0x80, + 0x2e, 0xd7, 0xb4, 0x13, 0x24, 0x96, 0x0c, 0x43, 0x0e, 0x0e, 0x2f, 0x40, 0x33, 0x48, 0x0e, 0x10, 0x24, 0xf2, 0x18, + 0x13, 0x24, 0x31, 0x49, 0x03, 0x22, 0x13, 0x24, 0x99, 0x00, 0x4b, 0x0e, 0x11, 0x24, 0xab, 0x18, 0x01, 0x22, 0x80, + 0x2e, 0xd7, 0xb4, 0x11, 0x24, 0xc6, 0x07, 0x69, 0x0e, 0x11, 0x24, 0xb0, 0x49, 0x10, 0x24, 0xbf, 0x17, 0x08, 0x22, + 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0x03, 0x15, 0x80, 0x2e, 0xd7, 0xb4, 0xb0, 0x32, 0x48, 0x0e, 0x57, 0x2f, 0xa0, + 0x37, 0x48, 0x0e, 0x13, 0x2f, 0x83, 0xa3, 0x08, 0x2f, 0x10, 0x24, 0xe0, 0x00, 0x48, 0x0e, 0x11, 0x24, 0xf6, 0x25, + 0x10, 0x24, 0x75, 0x17, 0x71, 0x2c, 0x08, 0x22, 0x10, 0x24, 0xa0, 0x00, 0x48, 0x0e, 0x11, 0x24, 0x7f, 0x18, 0x10, + 0x24, 0xa6, 0x13, 0x68, 0x2c, 0x08, 0x22, 0x11, 0x24, 0xf9, 0x07, 0x69, 0x0e, 0x0d, 0x2f, 0x11, 0x24, 0x10, 0x08, + 0x69, 0x0e, 0x11, 0x24, 0xb1, 0x14, 0x10, 0x24, 0x8e, 0x58, 0x48, 0x22, 0x90, 0x32, 0x58, 0x0e, 0x10, 0x24, 0x6d, + 0x14, 0x56, 0x2c, 0x01, 0x22, 0xc1, 0x6f, 0x10, 0x24, 0x68, 0x0c, 0x48, 0x0e, 0xb1, 0x6f, 0x0c, 0x2f, 0xcd, 0xa2, + 0x10, 0x24, 0x23, 0x14, 0x13, 0x24, 0x8d, 0x42, 0x03, 0x22, 0x13, 0x24, 0x2a, 0xfd, 0x0b, 0x0e, 0x11, 0x24, 0x53, + 0x12, 0x43, 0x2c, 0x08, 0x22, 0x10, 0x24, 0xcc, 0x07, 0x68, 0x0e, 0x0e, 0x2f, 0x10, 0x24, 0x08, 0xfd, 0x08, 0x0e, + 0x10, 0x24, 0x08, 0x16, 0x13, 0x24, 0x83, 0x45, 0x03, 0x22, 0x13, 0x24, 0xa1, 0xfd, 0x0b, 0x0e, 0x11, 0x24, 0xa6, + 0x14, 0x30, 0x2c, 0x01, 0x22, 0x10, 0x24, 0x5b, 0x01, 0x08, 0x0e, 0x11, 0x24, 0x2f, 0x12, 0x10, 0x24, 0xdd, 0x44, + 0x27, 0x2c, 0x08, 0x22, 0xdb, 0xa2, 0x0f, 0x2f, 0xc1, 0x6f, 0x10, 0x24, 0xb2, 0x0b, 0x48, 0x0e, 0x11, 0x24, 0x21, + 0x55, 0x10, 0x24, 0xc8, 0x14, 0x48, 0x22, 0x10, 0x24, 0x4c, 0x08, 0x68, 0x0e, 0x10, 0x24, 0xe4, 0x57, 0x15, 0x2c, + 0x01, 0x22, 0x44, 0xa2, 0x0f, 0x2f, 0xc1, 0x6f, 0x10, 0x24, 0xcb, 0x0b, 0x48, 0x0e, 0x11, 0x24, 0x09, 0x58, 0x10, + 0x24, 0xe4, 0x10, 0x48, 0x22, 0x10, 0x24, 0x4d, 0x08, 0x68, 0x0e, 0x10, 0x24, 0x1a, 0x12, 0x03, 0x2c, 0x01, 0x22, + 0x10, 0x24, 0x0c, 0x10, 0xfb, 0x6f, 0xb0, 0x5f, 0xb8, 0x2e, 0xa3, 0x32, 0xc3, 0x00, 0x60, 0x51, 0xc2, 0x40, 0x81, + 0x84, 0xd3, 0x7f, 0xd2, 0x42, 0xe0, 0x7f, 0x00, 0x30, 0xc4, 0x40, 0x20, 0x02, 0xc3, 0x7f, 0xd0, 0x42, 0x42, 0x3d, + 0xc0, 0x40, 0x01, 0x80, 0xc0, 0x42, 0xda, 0x00, 0x93, 0x7f, 0xb1, 0x7f, 0xab, 0x7f, 0x98, 0x2e, 0xb3, 0xc0, 0x91, + 0x6f, 0xf3, 0x32, 0x40, 0x42, 0x00, 0xac, 0x8b, 0x00, 0x02, 0x2f, 0xe1, 0x6f, 0x39, 0x56, 0x43, 0x42, 0xa1, 0x82, + 0x91, 0x7f, 0x33, 0x33, 0x4b, 0x00, 0x81, 0x7f, 0x13, 0x3c, 0x4b, 0x00, 0x80, 0x40, 0x53, 0x34, 0xb5, 0x6f, 0x8b, + 0x00, 0x0d, 0xb0, 0x43, 0x87, 0x76, 0x7f, 0xb2, 0x7f, 0x63, 0x7f, 0x65, 0x25, 0xb5, 0x6f, 0x92, 0x41, 0x63, 0x41, + 0x64, 0x41, 0x44, 0x81, 0x56, 0x7f, 0x41, 0x7f, 0x00, 0x2e, 0x26, 0x40, 0x27, 0x40, 0x45, 0x41, 0xf7, 0x7f, 0xb0, + 0x7f, 0x98, 0x2e, 0x67, 0xcc, 0x81, 0x6f, 0x0f, 0xa4, 0x43, 0x40, 0x72, 0x6f, 0x94, 0x6f, 0x05, 0x30, 0x01, 0x2f, + 0xc0, 0xa0, 0x03, 0x2f, 0x31, 0xac, 0x07, 0x2f, 0xc0, 0xa4, 0x05, 0x2f, 0xa2, 0x00, 0xeb, 0x04, 0x80, 0x40, 0x01, + 0x80, 0x43, 0x42, 0x80, 0x42, 0x41, 0x86, 0x56, 0x6f, 0x62, 0x6f, 0x41, 0x6f, 0x42, 0x82, 0x72, 0x0e, 0x83, 0x7f, + 0xd5, 0x2f, 0x53, 0x32, 0x8b, 0x00, 0xa1, 0x86, 0x56, 0x25, 0xf0, 0x82, 0x82, 0x40, 0x8d, 0xb0, 0x52, 0x40, 0xde, + 0x00, 0x91, 0x7f, 0xb3, 0x7f, 0x85, 0x7f, 0xb3, 0x30, 0x7b, 0x52, 0x98, 0x2e, 0x5a, 0xca, 0x1a, 0x25, 0x83, 0x6f, + 0x6d, 0x82, 0xfd, 0x88, 0x50, 0x7f, 0x71, 0x7f, 0x81, 0x7f, 0x05, 0x30, 0x83, 0x30, 0x00, 0x30, 0x11, 0x41, 0x52, + 0x6f, 0x25, 0x7f, 0x30, 0x7f, 0x44, 0x7f, 0x98, 0x2e, 0x0f, 0xca, 0x73, 0x6f, 0x20, 0x25, 0x90, 0x6f, 0x7d, 0x52, + 0xd2, 0x42, 0x73, 0x7f, 0x12, 0x7f, 0x98, 0x2e, 0x86, 0xb7, 0x93, 0x6f, 0x11, 0x6f, 0xd2, 0x40, 0x0a, 0x18, 0x31, + 0x6f, 0x0e, 0x00, 0x93, 0x7f, 0x83, 0x30, 0x44, 0x6f, 0x21, 0x6f, 0x62, 0x6f, 0x62, 0x0e, 0x4f, 0x03, 0xe1, 0x2f, + 0x33, 0x52, 0x01, 0x00, 0x01, 0x30, 0x69, 0x03, 0x3a, 0x25, 0xea, 0x82, 0x92, 0x6f, 0xf0, 0x86, 0xd1, 0xbe, 0x0f, + 0xb8, 0xbd, 0x84, 0x94, 0x7f, 0x05, 0x0a, 0x23, 0x7f, 0x52, 0x7f, 0x40, 0x7f, 0x31, 0x7f, 0x71, 0x7f, 0xd3, 0x30, + 0x84, 0x6f, 0x55, 0x6f, 0x10, 0x41, 0x52, 0x41, 0x41, 0x6f, 0x55, 0x7f, 0x10, 0x7f, 0x04, 0x7f, 0x98, 0x2e, 0x0f, + 0xca, 0x11, 0x6f, 0x20, 0x25, 0x98, 0x2e, 0xfe, 0xc9, 0x31, 0x6f, 0x04, 0x6f, 0x50, 0x42, 0x31, 0x7f, 0xd3, 0x30, + 0x21, 0x6f, 0x61, 0x0e, 0xea, 0x2f, 0xb1, 0x6f, 0x41, 0x84, 0x32, 0x25, 0x90, 0x40, 0x84, 0x40, 0x71, 0x6f, 0xb4, + 0x7f, 0x72, 0x7f, 0x40, 0x7f, 0x33, 0x7f, 0x98, 0x2e, 0xb3, 0xc0, 0x53, 0x6f, 0xb1, 0x32, 0x99, 0x00, 0x83, 0xb9, + 0x41, 0x6f, 0x4b, 0x00, 0xb0, 0x6f, 0x03, 0x30, 0xc3, 0x02, 0x84, 0x40, 0xb2, 0x7f, 0xa1, 0x84, 0x0d, 0xb1, 0x52, + 0x7f, 0x56, 0x01, 0x74, 0x6f, 0x30, 0x6f, 0x92, 0x6f, 0x43, 0x8b, 0x03, 0x43, 0x01, 0x42, 0x95, 0x7f, 0xbd, 0x86, + 0x51, 0x41, 0x41, 0x7f, 0x75, 0x7f, 0x00, 0x2e, 0xd1, 0x40, 0x42, 0x41, 0x32, 0x7f, 0x23, 0x7f, 0x98, 0x2e, 0x74, + 0xc0, 0x41, 0x6f, 0xc8, 0x00, 0x90, 0x6f, 0x01, 0x30, 0x75, 0x6f, 0x32, 0x6f, 0x03, 0x42, 0x91, 0x02, 0x23, 0x6f, + 0x61, 0x6f, 0x59, 0x0e, 0x62, 0x43, 0x95, 0x7f, 0xe7, 0x2f, 0xb2, 0x6f, 0x51, 0x6f, 0x82, 0x40, 0x8d, 0xb0, 0x8e, + 0x00, 0xfd, 0x8a, 0xb2, 0x7f, 0x02, 0x30, 0x79, 0x52, 0x05, 0x25, 0x03, 0x30, 0x54, 0x40, 0xec, 0x01, 0x16, 0x40, + 0x43, 0x89, 0xc7, 0x41, 0x37, 0x18, 0x3d, 0x8b, 0x96, 0x00, 0x44, 0x0e, 0xdf, 0x02, 0xf4, 0x2f, 0x09, 0x52, 0x51, + 0x00, 0x02, 0x30, 0x9a, 0x02, 0xb5, 0x6f, 0x45, 0x87, 0x1b, 0xba, 0x25, 0xbc, 0x51, 0x6f, 0x4d, 0x8b, 0x7a, 0x82, + 0xc6, 0x40, 0x20, 0x0a, 0x30, 0x00, 0xd0, 0x42, 0x2b, 0xb5, 0xc0, 0x40, 0x82, 0x02, 0x40, 0x34, 0x08, 0x00, 0xd2, + 0x42, 0xb0, 0x7f, 0x75, 0x7f, 0x93, 0x7f, 0x00, 0x2e, 0xb5, 0x6f, 0xe2, 0x6f, 0x63, 0x41, 0x64, 0x41, 0x44, 0x8f, + 0x82, 0x40, 0xe6, 0x41, 0xc0, 0x41, 0xc4, 0x8f, 0x45, 0x41, 0xf0, 0x7f, 0xb7, 0x7f, 0x61, 0x7f, 0x98, 0x2e, 0x67, + 0xcc, 0x00, 0x18, 0x09, 0x52, 0x71, 0x00, 0x03, 0x30, 0xbb, 0x02, 0x1b, 0xba, 0x93, 0x6f, 0x25, 0xbc, 0x61, 0x6f, + 0xc5, 0x40, 0x42, 0x82, 0x20, 0x0a, 0x28, 0x00, 0xd0, 0x42, 0x2b, 0xb9, 0xc0, 0x40, 0x82, 0x02, 0xd2, 0x42, 0x93, + 0x7f, 0x00, 0x2e, 0x72, 0x6f, 0x5a, 0x0e, 0xd9, 0x2f, 0xb1, 0x6f, 0xf3, 0x3c, 0xcb, 0x00, 0xda, 0x82, 0xc3, 0x40, + 0x41, 0x40, 0x59, 0x0e, 0x50, 0x2f, 0xe1, 0x6f, 0xe3, 0x32, 0xcb, 0x00, 0xb3, 0x7f, 0x22, 0x30, 0xc0, 0x40, 0x01, + 0x80, 0xc0, 0x42, 0x02, 0xa2, 0x30, 0x2f, 0xc2, 0x42, 0x98, 0x2e, 0x83, 0xb1, 0xe1, 0x6f, 0xb3, 0x35, 0xcb, 0x00, + 0x24, 0x3d, 0xc2, 0x40, 0xdc, 0x00, 0x84, 0x40, 0x00, 0x91, 0x93, 0x7f, 0x02, 0x2f, 0x00, 0x2e, 0x06, 0x2c, 0x0c, + 0xb8, 0x30, 0x25, 0x00, 0x33, 0x48, 0x00, 0x98, 0x2e, 0xf6, 0xb6, 0x91, 0x6f, 0x90, 0x7f, 0x00, 0x2e, 0x44, 0x40, + 0x20, 0x1a, 0x15, 0x2f, 0xd3, 0x6f, 0xc1, 0x6f, 0xc3, 0x40, 0x35, 0x5a, 0x42, 0x40, 0xd3, 0x7e, 0x08, 0xbc, 0x25, + 0x09, 0xe2, 0x7e, 0xc4, 0x0a, 0x42, 0x82, 0xf3, 0x7e, 0xd1, 0x7f, 0x34, 0x30, 0x83, 0x6f, 0x82, 0x30, 0x31, 0x30, + 0x98, 0x2e, 0xb3, 0x00, 0xd1, 0x6f, 0x93, 0x6f, 0x43, 0x42, 0xf0, 0x32, 0xb1, 0x6f, 0x41, 0x82, 0xe2, 0x6f, 0x43, + 0x40, 0xc1, 0x86, 0xc2, 0xa2, 0x43, 0x42, 0x03, 0x30, 0x02, 0x2f, 0x90, 0x00, 0x00, 0x2e, 0x83, 0x42, 0x61, 0x88, + 0x42, 0x40, 0x8d, 0xb0, 0x26, 0x00, 0x98, 0x2e, 0xd9, 0x03, 0x1c, 0x83, 0x00, 0x2e, 0x43, 0x42, 0x00, 0x2e, 0xab, + 0x6f, 0xa0, 0x5e, 0xb8, 0x2e, 0xb1, 0x35, 0x40, 0x51, 0x41, 0x01, 0x02, 0x30, 0x1a, 0x25, 0x13, 0x30, 0x40, 0x25, + 0x12, 0x42, 0x45, 0x0e, 0xfc, 0x2f, 0x65, 0x34, 0x28, 0x80, 0x25, 0x01, 0x13, 0x42, 0x44, 0x0e, 0xfc, 0x2f, 0x27, + 0x80, 0x65, 0x56, 0x03, 0x42, 0x15, 0x80, 0xa3, 0x30, 0x03, 0x42, 0x04, 0x80, 0x4d, 0x56, 0x7f, 0x58, 0x13, 0x42, + 0xd4, 0x7e, 0xc2, 0x7e, 0xf2, 0x7e, 0x6c, 0x8c, 0x81, 0x56, 0x83, 0x58, 0xe3, 0x7e, 0x04, 0x7f, 0x71, 0x8a, 0x97, + 0x41, 0x17, 0x42, 0x75, 0x0e, 0xfb, 0x2f, 0x85, 0x5c, 0x87, 0x5e, 0x16, 0x7f, 0x36, 0x7f, 0x27, 0x7f, 0x00, 0x2e, + 0x89, 0x5c, 0x8b, 0x5e, 0x46, 0x7f, 0x57, 0x7f, 0x76, 0x8c, 0x57, 0x41, 0x17, 0x42, 0x6e, 0x0e, 0xfb, 0x2f, 0x8d, + 0x5a, 0x8f, 0x5e, 0x65, 0x7f, 0x87, 0x7f, 0x72, 0x7f, 0x00, 0x2e, 0x91, 0x5a, 0x93, 0x5e, 0x95, 0x7f, 0xa7, 0x7f, + 0x7b, 0x8a, 0x97, 0x41, 0x17, 0x42, 0x75, 0x0e, 0xfb, 0x2f, 0x7f, 0x5c, 0xb2, 0x7f, 0xc6, 0x7f, 0xd3, 0x7f, 0xe2, + 0x7f, 0xf4, 0x7f, 0x40, 0x82, 0x52, 0x41, 0x12, 0x42, 0x69, 0x0e, 0xfb, 0x2f, 0xc0, 0x5e, 0xb8, 0x2e, 0x03, 0x2e, + 0x2d, 0x02, 0x9f, 0xbc, 0x9f, 0xb8, 0x20, 0x50, 0x40, 0xb2, 0x14, 0x2f, 0x10, 0x25, 0x01, 0x2e, 0x8d, 0x00, 0x00, + 0x90, 0x0b, 0x2f, 0x97, 0x50, 0xf1, 0x7f, 0xeb, 0x7f, 0x98, 0x2e, 0x83, 0xb6, 0x01, 0x2e, 0x8d, 0x00, 0x01, 0x80, + 0x21, 0x2e, 0x8d, 0x00, 0xf1, 0x6f, 0xeb, 0x6f, 0xe0, 0x5f, 0x97, 0x50, 0x80, 0x2e, 0xda, 0xb4, 0x00, 0x30, 0x21, + 0x2e, 0x8d, 0x00, 0xe0, 0x5f, 0xb8, 0x2e, 0x41, 0x25, 0x42, 0x8a, 0x50, 0x50, 0x99, 0x52, 0x81, 0x80, 0x99, 0x09, + 0xf5, 0x7f, 0x52, 0x25, 0x07, 0x52, 0x03, 0x8e, 0xd9, 0x08, 0x02, 0x40, 0x03, 0x81, 0x44, 0x83, 0x6c, 0xbb, 0xda, + 0x0e, 0xe7, 0x7f, 0xdb, 0x7f, 0x20, 0x2f, 0x02, 0x41, 0x32, 0x1a, 0x1d, 0x2f, 0x42, 0x85, 0x00, 0x2e, 0x82, 0x40, + 0xda, 0x0e, 0x03, 0x30, 0x05, 0x2f, 0xf1, 0x6f, 0x06, 0x30, 0x42, 0x40, 0x81, 0x84, 0x18, 0x2c, 0x42, 0x42, 0xbf, + 0x85, 0x82, 0x00, 0x41, 0x40, 0x86, 0x40, 0x81, 0x8d, 0x86, 0x42, 0x20, 0x25, 0x13, 0x30, 0x06, 0x30, 0x97, 0x40, + 0x81, 0x8d, 0xf9, 0x0f, 0x09, 0x2f, 0x85, 0xa3, 0xf9, 0x2f, 0x03, 0x30, 0x06, 0x2c, 0x06, 0x30, 0x9b, 0x52, 0xd9, + 0x0e, 0x13, 0x30, 0x01, 0x30, 0xd9, 0x22, 0xc0, 0xb2, 0x12, 0x83, 0xc1, 0x7f, 0x03, 0x30, 0xb4, 0x7f, 0x06, 0x2f, + 0x51, 0x30, 0x70, 0x25, 0x98, 0x2e, 0xe3, 0x03, 0xff, 0x81, 0x00, 0x2e, 0x03, 0x42, 0x43, 0x8b, 0xe0, 0x6f, 0xf1, + 0x6f, 0x00, 0x40, 0x41, 0x40, 0xc8, 0x0f, 0x37, 0x2f, 0x00, 0x41, 0x80, 0xa7, 0x3c, 0x2f, 0x01, 0x83, 0x47, 0x8e, + 0x42, 0x40, 0xfa, 0x01, 0x81, 0x84, 0x08, 0x89, 0x45, 0x41, 0x55, 0x0e, 0xc6, 0x43, 0x42, 0x42, 0xf4, 0x7f, 0x00, + 0x2f, 0x43, 0x42, 0x51, 0x82, 0x70, 0x1a, 0x41, 0x40, 0x06, 0x2f, 0xc3, 0x6f, 0x41, 0x82, 0xc1, 0x42, 0xcd, 0x0e, + 0x26, 0x2f, 0xc5, 0x42, 0x25, 0x2d, 0x7f, 0x82, 0x51, 0xbb, 0xa5, 0x00, 0xce, 0x0f, 0x12, 0x2f, 0x14, 0x30, 0x05, + 0x30, 0xf7, 0x6f, 0x06, 0x30, 0x05, 0x2c, 0xe0, 0x7f, 0xd0, 0x41, 0x05, 0x1a, 0x23, 0x22, 0xb0, 0x01, 0x7a, 0x0e, + 0xf9, 0x2f, 0x71, 0x0f, 0xe0, 0x6f, 0x28, 0x22, 0x41, 0x8b, 0x71, 0x22, 0x45, 0xa7, 0xee, 0x2f, 0xb3, 0x6f, 0xc2, + 0x6f, 0xc0, 0x42, 0x81, 0x42, 0x08, 0x2d, 0x04, 0x25, 0xc4, 0x6f, 0x98, 0x2e, 0xea, 0x03, 0x00, 0x2e, 0x40, 0x41, + 0x00, 0x43, 0x00, 0x30, 0xdb, 0x6f, 0xb0, 0x5f, 0xb8, 0x2e, 0x10, 0x50, 0x03, 0x40, 0x19, 0x18, 0x37, 0x56, 0x19, + 0x05, 0x36, 0x25, 0xf7, 0x7f, 0x4a, 0x17, 0x54, 0x18, 0xec, 0x18, 0x09, 0x17, 0x01, 0x30, 0x0c, 0x07, 0xe2, 0x18, + 0xde, 0x00, 0xf2, 0x6f, 0x97, 0x02, 0x33, 0x58, 0xdc, 0x00, 0x91, 0x02, 0xbf, 0xb8, 0x21, 0xbd, 0x8a, 0x0a, 0xc0, + 0x2e, 0x02, 0x42, 0xf0, 0x5f, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x01, 0x2e, 0x5d, 0xf7, 0x08, 0xbc, 0x80, 0xac, 0x0e, 0xbb, 0x02, 0x2f, + 0x00, 0x30, 0x41, 0x04, 0x82, 0x06, 0xc0, 0xa4, 0x00, 0x30, 0x11, 0x2f, 0x40, 0xa9, 0x03, 0x2f, 0x40, 0x91, 0x0d, + 0x2f, 0x00, 0xa7, 0x0b, 0x2f, 0x80, 0xb3, 0x33, 0x58, 0x02, 0x2f, 0x90, 0xa1, 0x26, 0x13, 0x20, 0x23, 0x80, 0x90, + 0x10, 0x30, 0x01, 0x2f, 0xcc, 0x0e, 0x00, 0x2f, 0x00, 0x30, 0xb8, 0x2e, 0x35, 0x50, 0x18, 0x08, 0x08, 0xbc, 0x88, + 0xb6, 0x0d, 0x17, 0xc6, 0xbd, 0x56, 0xbc, 0x37, 0x58, 0xda, 0xba, 0x04, 0x01, 0x1d, 0x0a, 0x10, 0x50, 0x05, 0x30, + 0x32, 0x25, 0x45, 0x03, 0xfb, 0x7f, 0xf6, 0x30, 0x21, 0x25, 0x98, 0x2e, 0x37, 0xca, 0x16, 0xb5, 0x9a, 0xbc, 0x06, + 0xb8, 0x80, 0xa8, 0x41, 0x0a, 0x0e, 0x2f, 0x80, 0x90, 0x02, 0x2f, 0x39, 0x50, 0x48, 0x0f, 0x09, 0x2f, 0xbf, 0xa0, + 0x04, 0x2f, 0xbf, 0x90, 0x06, 0x2f, 0x37, 0x54, 0xca, 0x0f, 0x03, 0x2f, 0x00, 0x2e, 0x02, 0x2c, 0x37, 0x52, 0x39, + 0x52, 0xf2, 0x33, 0x98, 0x2e, 0xd9, 0xc0, 0xfb, 0x6f, 0xf1, 0x37, 0xc0, 0x2e, 0x01, 0x08, 0xf0, 0x5f, 0x41, 0x56, + 0x3b, 0x54, 0xd0, 0x40, 0xc4, 0x40, 0x0b, 0x2e, 0xfd, 0xf3, 0x41, 0x52, 0x90, 0x42, 0x94, 0x42, 0x95, 0x42, 0x05, + 0x30, 0x43, 0x50, 0x0f, 0x88, 0x06, 0x40, 0x04, 0x41, 0x96, 0x42, 0xc5, 0x42, 0x48, 0xbe, 0x73, 0x30, 0x0d, 0x2e, + 0x88, 0x00, 0x4f, 0xba, 0x84, 0x42, 0x03, 0x42, 0x81, 0xb3, 0x02, 0x2f, 0x2b, 0x2e, 0x6f, 0xf5, 0x06, 0x2d, 0x05, + 0x2e, 0x77, 0xf7, 0x3f, 0x56, 0x93, 0x08, 0x25, 0x2e, 0x77, 0xf7, 0x3d, 0x54, 0x25, 0x2e, 0xc2, 0xf5, 0x07, 0x2e, + 0xfd, 0xf3, 0x42, 0x30, 0xb4, 0x33, 0xda, 0x0a, 0x4c, 0x00, 0x27, 0x2e, 0xfd, 0xf3, 0x43, 0x40, 0xd4, 0x3f, 0xdc, + 0x08, 0x43, 0x42, 0x00, 0x2e, 0x00, 0x2e, 0x43, 0x40, 0x24, 0x30, 0xdc, 0x0a, 0x43, 0x42, 0x04, 0x80, 0x03, 0x2e, + 0xfd, 0xf3, 0x4a, 0x0a, 0x23, 0x2e, 0xfd, 0xf3, 0x61, 0x34, 0xc0, 0x2e, 0x01, 0x42, 0x00, 0x2e, 0x60, 0x50, 0x1a, + 0x25, 0x7a, 0x86, 0xe0, 0x7f, 0xf3, 0x7f, 0x03, 0x25, 0x45, 0x52, 0x41, 0x84, 0xdb, 0x7f, 0x33, 0x30, 0x98, 0x2e, + 0x16, 0xc2, 0x1a, 0x25, 0x7d, 0x82, 0xf0, 0x6f, 0xe2, 0x6f, 0x32, 0x25, 0x16, 0x40, 0x94, 0x40, 0x26, 0x01, 0x85, + 0x40, 0x8e, 0x17, 0xc4, 0x42, 0x6e, 0x03, 0x95, 0x42, 0x41, 0x0e, 0xf4, 0x2f, 0xdb, 0x6f, 0xa0, 0x5f, 0xb8, 0x2e, + 0xb0, 0x51, 0xfb, 0x7f, 0x98, 0x2e, 0xe8, 0x0d, 0x5a, 0x25, 0x98, 0x2e, 0x0f, 0x0e, 0x4f, 0x58, 0x32, 0x87, 0xc4, + 0x7f, 0x65, 0x89, 0x6b, 0x8d, 0x47, 0x5a, 0x65, 0x7f, 0xe1, 0x7f, 0x83, 0x7f, 0xa6, 0x7f, 0x74, 0x7f, 0xd0, 0x7f, + 0xb6, 0x7f, 0x94, 0x7f, 0x17, 0x30, 0x49, 0x52, 0x4b, 0x54, 0x51, 0x7f, 0x00, 0x2e, 0x85, 0x6f, 0x42, 0x7f, 0x00, + 0x2e, 0x51, 0x41, 0x45, 0x81, 0x42, 0x41, 0x13, 0x40, 0x3b, 0x8a, 0x00, 0x40, 0x4b, 0x04, 0xd0, 0x06, 0xc0, 0xac, + 0x85, 0x7f, 0x02, 0x2f, 0x02, 0x30, 0x51, 0x04, 0xd3, 0x06, 0x41, 0x84, 0x05, 0x30, 0x5d, 0x02, 0xc9, 0x16, 0xdf, + 0x08, 0xd3, 0x00, 0x8d, 0x02, 0xaf, 0xbc, 0xb1, 0xb9, 0x59, 0x0a, 0x65, 0x6f, 0x11, 0x43, 0xa1, 0xb4, 0x52, 0x41, + 0x53, 0x41, 0x01, 0x43, 0x34, 0x7f, 0x65, 0x7f, 0x26, 0x31, 0xe5, 0x6f, 0xd4, 0x6f, 0x98, 0x2e, 0x37, 0xca, 0x32, + 0x6f, 0x75, 0x6f, 0x83, 0x40, 0x42, 0x41, 0x23, 0x7f, 0x12, 0x7f, 0xf6, 0x30, 0x40, 0x25, 0x51, 0x25, 0x98, 0x2e, + 0x37, 0xca, 0x14, 0x6f, 0x20, 0x05, 0x70, 0x6f, 0x25, 0x6f, 0x69, 0x07, 0xa2, 0x6f, 0x31, 0x6f, 0x0b, 0x30, 0x04, + 0x42, 0x9b, 0x42, 0x8b, 0x42, 0x55, 0x42, 0x32, 0x7f, 0x40, 0xa9, 0xc3, 0x6f, 0x71, 0x7f, 0x02, 0x30, 0xd0, 0x40, + 0xc3, 0x7f, 0x03, 0x2f, 0x40, 0x91, 0x15, 0x2f, 0x00, 0xa7, 0x13, 0x2f, 0x00, 0xa4, 0x11, 0x2f, 0x84, 0xbd, 0x98, + 0x2e, 0x79, 0xca, 0x55, 0x6f, 0x37, 0x54, 0x54, 0x41, 0x82, 0x00, 0xf3, 0x3f, 0x45, 0x41, 0xcb, 0x02, 0xf6, 0x30, + 0x98, 0x2e, 0x37, 0xca, 0x35, 0x6f, 0xa4, 0x6f, 0x41, 0x43, 0x03, 0x2c, 0x00, 0x43, 0xa4, 0x6f, 0x35, 0x6f, 0x17, + 0x30, 0x42, 0x6f, 0x51, 0x6f, 0x93, 0x40, 0x42, 0x82, 0x00, 0x41, 0xc3, 0x00, 0x03, 0x43, 0x51, 0x7f, 0x00, 0x2e, + 0x94, 0x40, 0x41, 0x41, 0x4c, 0x02, 0xc4, 0x6f, 0x55, 0x56, 0x63, 0x0e, 0x74, 0x6f, 0x51, 0x43, 0xa5, 0x7f, 0x8a, + 0x2f, 0x09, 0x2e, 0x88, 0x00, 0x01, 0xb3, 0x21, 0x2f, 0x4f, 0x58, 0x90, 0x6f, 0x13, 0x41, 0xb6, 0x6f, 0xe4, 0x7f, + 0x00, 0x2e, 0x91, 0x41, 0x14, 0x40, 0x92, 0x41, 0x15, 0x40, 0x17, 0x2e, 0x6f, 0xf5, 0xb6, 0x7f, 0xd0, 0x7f, 0xcb, + 0x7f, 0x98, 0x2e, 0x00, 0x0c, 0x07, 0x15, 0xc2, 0x6f, 0x14, 0x0b, 0x29, 0x2e, 0x6f, 0xf5, 0xc3, 0xa3, 0xc1, 0x8f, + 0xe4, 0x6f, 0xd0, 0x6f, 0xe6, 0x2f, 0x14, 0x30, 0x05, 0x2e, 0x6f, 0xf5, 0x14, 0x0b, 0x29, 0x2e, 0x6f, 0xf5, 0x18, + 0x2d, 0x51, 0x56, 0x04, 0x32, 0xb5, 0x6f, 0x1c, 0x01, 0x51, 0x41, 0x52, 0x41, 0xc3, 0x40, 0xb5, 0x7f, 0xe4, 0x7f, + 0x98, 0x2e, 0x1f, 0x0c, 0xe4, 0x6f, 0x21, 0x87, 0x00, 0x43, 0x04, 0x32, 0x53, 0x54, 0x5a, 0x0e, 0xef, 0x2f, 0x4d, + 0x54, 0x09, 0x2e, 0x77, 0xf7, 0x22, 0x0b, 0x29, 0x2e, 0x77, 0xf7, 0xfb, 0x6f, 0x50, 0x5e, 0xb8, 0x2e, 0x10, 0x50, + 0x01, 0x2e, 0x84, 0x00, 0x00, 0xb2, 0xfb, 0x7f, 0x51, 0x2f, 0x01, 0xb2, 0x48, 0x2f, 0x02, 0xb2, 0x42, 0x2f, 0x03, + 0x90, 0x56, 0x2f, 0x5b, 0x52, 0x79, 0x80, 0x42, 0x40, 0x81, 0x84, 0x00, 0x40, 0x42, 0x42, 0x98, 0x2e, 0x93, 0x0c, + 0x5d, 0x54, 0x5b, 0x50, 0xa1, 0x40, 0x98, 0xbd, 0x82, 0x40, 0x3e, 0x82, 0xda, 0x0a, 0x44, 0x40, 0x8b, 0x16, 0xe3, + 0x00, 0x53, 0x42, 0x00, 0x2e, 0x43, 0x40, 0x9a, 0x02, 0x52, 0x42, 0x00, 0x2e, 0x41, 0x40, 0x4d, 0x54, 0x4a, 0x0e, + 0x3a, 0x2f, 0x3a, 0x82, 0x00, 0x30, 0x41, 0x40, 0x21, 0x2e, 0x6c, 0x0f, 0x40, 0xb2, 0x0a, 0x2f, 0x98, 0x2e, 0xb1, + 0x0c, 0x98, 0x2e, 0x45, 0x0e, 0x98, 0x2e, 0x5b, 0x0e, 0xfb, 0x6f, 0xf0, 0x5f, 0x00, 0x30, 0x80, 0x2e, 0xba, 0x03, + 0x61, 0x52, 0x57, 0x54, 0x42, 0x42, 0x4f, 0x84, 0x73, 0x30, 0x5f, 0x52, 0x83, 0x42, 0x1b, 0x30, 0x6b, 0x42, 0x23, + 0x30, 0x27, 0x2e, 0x87, 0x00, 0x37, 0x2e, 0x84, 0x00, 0x21, 0x2e, 0x86, 0x00, 0x7a, 0x84, 0x17, 0x2c, 0x42, 0x42, + 0x30, 0x30, 0x21, 0x2e, 0x84, 0x00, 0x12, 0x2d, 0x21, 0x30, 0x00, 0x30, 0x23, 0x2e, 0x84, 0x00, 0x21, 0x2e, 0x7b, + 0xf7, 0x0b, 0x2d, 0x17, 0x30, 0x98, 0x2e, 0x51, 0x0c, 0x59, 0x50, 0x0c, 0x82, 0x72, 0x30, 0x2f, 0x2e, 0x84, 0x00, + 0x25, 0x2e, 0x7b, 0xf7, 0x40, 0x42, 0x00, 0x2e, 0xfb, 0x6f, 0xf0, 0x5f, 0xb8, 0x2e, 0x70, 0x50, 0x0a, 0x25, 0x39, + 0x86, 0xfb, 0x7f, 0xe1, 0x32, 0x62, 0x30, 0x98, 0x2e, 0xc2, 0xc4, 0x35, 0x56, 0xa5, 0x6f, 0xab, 0x08, 0x91, 0x6f, + 0x4b, 0x08, 0x63, 0x56, 0xc4, 0x6f, 0x23, 0x09, 0x4d, 0xba, 0x93, 0xbc, 0x8c, 0x0b, 0xd1, 0x6f, 0x0b, 0x09, 0x4f, + 0x52, 0x65, 0x5e, 0x56, 0x42, 0xaf, 0x09, 0x4d, 0xba, 0x23, 0xbd, 0x94, 0x0a, 0xe5, 0x6f, 0x68, 0xbb, 0xeb, 0x08, + 0xbd, 0xb9, 0x63, 0xbe, 0xfb, 0x6f, 0x52, 0x42, 0xe3, 0x0a, 0xc0, 0x2e, 0x43, 0x42, 0x90, 0x5f, 0x55, 0x50, 0x03, + 0x2e, 0x25, 0xf3, 0x13, 0x40, 0x00, 0x40, 0x9b, 0xbc, 0x9b, 0xb4, 0x08, 0xbd, 0xb8, 0xb9, 0x98, 0xbc, 0xda, 0x0a, + 0x08, 0xb6, 0x89, 0x16, 0xc0, 0x2e, 0x19, 0x00, 0x62, 0x02, 0x10, 0x50, 0xfb, 0x7f, 0x98, 0x2e, 0x81, 0x0d, 0x01, + 0x2e, 0x84, 0x00, 0x31, 0x30, 0x08, 0x04, 0xfb, 0x6f, 0x01, 0x30, 0xf0, 0x5f, 0x23, 0x2e, 0x86, 0x00, 0x21, 0x2e, + 0x87, 0x00, 0xb8, 0x2e, 0x01, 0x2e, 0x87, 0x00, 0x03, 0x2e, 0x86, 0x00, 0x48, 0x0e, 0x01, 0x2f, 0x80, 0x2e, 0x1f, + 0x0e, 0xb8, 0x2e, 0x67, 0x50, 0x21, 0x34, 0x01, 0x42, 0x82, 0x30, 0xc1, 0x32, 0x25, 0x2e, 0x62, 0xf5, 0x01, 0x00, + 0x22, 0x30, 0x01, 0x40, 0x4a, 0x0a, 0x01, 0x42, 0xb8, 0x2e, 0x67, 0x54, 0xf0, 0x3b, 0x83, 0x40, 0xd8, 0x08, 0x69, + 0x52, 0x83, 0x42, 0x00, 0x30, 0x83, 0x30, 0x50, 0x42, 0xc4, 0x32, 0x27, 0x2e, 0x64, 0xf5, 0x94, 0x00, 0x50, 0x42, + 0x40, 0x42, 0xd3, 0x3f, 0x84, 0x40, 0x7d, 0x82, 0xe3, 0x08, 0x40, 0x42, 0x83, 0x42, 0xb8, 0x2e, 0x61, 0x52, 0x00, + 0x30, 0x40, 0x42, 0x7c, 0x86, 0x3b, 0x52, 0x09, 0x2e, 0x57, 0x0f, 0x41, 0x54, 0xc4, 0x42, 0xd3, 0x86, 0x54, 0x40, + 0x55, 0x40, 0x94, 0x42, 0x85, 0x42, 0x21, 0x2e, 0x87, 0x00, 0x42, 0x40, 0x25, 0x2e, 0xfd, 0xf3, 0xc0, 0x42, 0x7e, + 0x82, 0x05, 0x2e, 0x79, 0x00, 0x80, 0xb2, 0x14, 0x2f, 0x05, 0x2e, 0x24, 0x02, 0x27, 0xbd, 0x2f, 0xb9, 0x80, 0x90, + 0x02, 0x2f, 0x21, 0x2e, 0x6f, 0xf5, 0x0c, 0x2d, 0x07, 0x2e, 0x58, 0x0f, 0x14, 0x30, 0x1c, 0x09, 0x05, 0x2e, 0x77, + 0xf7, 0x3f, 0x56, 0x47, 0xbe, 0x93, 0x08, 0x94, 0x0a, 0x25, 0x2e, 0x77, 0xf7, 0x6b, 0x54, 0x50, 0x42, 0x4a, 0x0e, + 0xfc, 0x2f, 0xb8, 0x2e, 0x50, 0x50, 0x02, 0x30, 0x43, 0x86, 0x69, 0x50, 0xfb, 0x7f, 0xe3, 0x7f, 0xd2, 0x7f, 0xc0, + 0x7f, 0xb1, 0x7f, 0x00, 0x2e, 0x41, 0x40, 0x00, 0x40, 0x48, 0x04, 0x98, 0x2e, 0x74, 0xc0, 0x1e, 0xaa, 0xd3, 0x6f, + 0x14, 0x30, 0xb1, 0x6f, 0xe3, 0x22, 0xc0, 0x6f, 0x52, 0x40, 0xe4, 0x6f, 0x4c, 0x0e, 0x12, 0x42, 0xd3, 0x7f, 0xeb, + 0x2f, 0x03, 0x2e, 0x6d, 0x0f, 0x40, 0x90, 0x11, 0x30, 0x03, 0x2f, 0x23, 0x2e, 0x6d, 0x0f, 0x02, 0x2c, 0x00, 0x30, + 0xd0, 0x6f, 0xfb, 0x6f, 0xb0, 0x5f, 0xb8, 0x2e, 0x40, 0x50, 0xf1, 0x7f, 0x0a, 0x25, 0x3c, 0x86, 0xeb, 0x7f, 0x41, + 0x33, 0x22, 0x30, 0x98, 0x2e, 0xc2, 0xc4, 0xd3, 0x6f, 0xf4, 0x30, 0xdc, 0x09, 0x6f, 0x58, 0xc2, 0x6f, 0x94, 0x09, + 0x71, 0x58, 0x6a, 0xbb, 0xdc, 0x08, 0xb4, 0xb9, 0xb1, 0xbd, 0x6d, 0x5a, 0x95, 0x08, 0x21, 0xbd, 0xf6, 0xbf, 0x77, + 0x0b, 0x51, 0xbe, 0xf1, 0x6f, 0xeb, 0x6f, 0x52, 0x42, 0x54, 0x42, 0xc0, 0x2e, 0x43, 0x42, 0xc0, 0x5f, 0x50, 0x50, + 0x75, 0x52, 0x93, 0x30, 0x53, 0x42, 0xfb, 0x7f, 0x7b, 0x30, 0x4b, 0x42, 0x13, 0x30, 0x42, 0x82, 0x20, 0x33, 0x43, + 0x42, 0xc8, 0x00, 0x01, 0x2e, 0x80, 0x03, 0x05, 0x2e, 0x7d, 0x00, 0x19, 0x52, 0xe2, 0x7f, 0xd0, 0x7f, 0xc3, 0x7f, + 0x98, 0x2e, 0xb6, 0x0e, 0xd1, 0x6f, 0x48, 0x0a, 0xd1, 0x7f, 0x3a, 0x25, 0xfb, 0x86, 0x01, 0x33, 0x12, 0x30, 0x98, + 0x2e, 0xc2, 0xc4, 0xd1, 0x6f, 0x48, 0x0a, 0x40, 0xb2, 0x0d, 0x2f, 0xe0, 0x6f, 0x03, 0x2e, 0x80, 0x03, 0x53, 0x30, + 0x07, 0x80, 0x27, 0x2e, 0x21, 0xf2, 0x98, 0xbc, 0x01, 0x42, 0x98, 0x2e, 0x91, 0x03, 0x00, 0x2e, 0x00, 0x2e, 0xd0, + 0x2e, 0xb1, 0x6f, 0x9b, 0xb8, 0x07, 0x2e, 0x1b, 0x00, 0x19, 0x1a, 0xb1, 0x7f, 0x71, 0x30, 0x04, 0x2f, 0x23, 0x2e, + 0x21, 0xf2, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x98, 0x2e, 0x6d, 0xc0, 0x98, 0x2e, 0x5d, 0xc0, 0x98, 0x2e, 0xdf, + 0x03, 0x20, 0x26, 0xc1, 0x6f, 0x02, 0x31, 0x52, 0x42, 0xab, 0x30, 0x4b, 0x42, 0x20, 0x33, 0x77, 0x56, 0xf1, 0x37, + 0xc4, 0x40, 0xa2, 0x0a, 0xc2, 0x42, 0xd8, 0x00, 0x01, 0x2e, 0x5e, 0xf7, 0x41, 0x08, 0x23, 0x2e, 0x94, 0x00, 0xe3, + 0x7f, 0x98, 0x2e, 0xaa, 0x01, 0xe1, 0x6f, 0x83, 0x30, 0x43, 0x42, 0x03, 0x30, 0xfb, 0x6f, 0x73, 0x50, 0x02, 0x30, + 0x00, 0x2e, 0x00, 0x2e, 0x81, 0x84, 0x50, 0x0e, 0xfa, 0x2f, 0x43, 0x42, 0x11, 0x30, 0xb0, 0x5f, 0x23, 0x2e, 0x21, + 0xf2, 0xb8, 0x2e, 0xc1, 0x4a, 0x00, 0x00, 0x6d, 0x57, 0x00, 0x00, 0x77, 0x8e, 0x00, 0x00, 0xe0, 0xff, 0xff, 0xff, + 0xd3, 0xff, 0xff, 0xff, 0xe5, 0xff, 0xff, 0xff, 0xee, 0xe1, 0xff, 0xff, 0x7c, 0x13, 0x00, 0x00, 0x46, 0xe6, 0xff, + 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0xfd, 0x2d +}; + +/*! @name Global array that stores the feature input configuration of + * BMI270_CONTEXT + */ +const struct bmi2_feature_config bmi270_context_feat_in[BMI270_CONTEXT_MAX_FEAT_IN] = { + { .type = BMI2_CONFIG_ID, .page = BMI2_PAGE_4, .start_addr = BMI270_CONTEXT_CONFIG_ID_STRT_ADDR }, + { .type = BMI2_STEP_COUNTER_PARAMS, .page = BMI2_PAGE_1, .start_addr = BMI270_CONTEXT_STEP_CNT_1_STRT_ADDR }, + { .type = BMI2_STEP_DETECTOR, .page = BMI2_PAGE_4, .start_addr = BMI270_CONTEXT_STEP_CNT_4_STRT_ADDR }, + { .type = BMI2_STEP_COUNTER, .page = BMI2_PAGE_4, .start_addr = BMI270_CONTEXT_STEP_CNT_4_STRT_ADDR }, + { .type = BMI2_NVM_PROG_PREP, .page = BMI2_PAGE_4, .start_addr = BMI270_CONTEXT_NVM_PROG_PREP_STRT_ADDR }, + { .type = BMI2_MAX_BURST_LEN, .page = BMI2_PAGE_4, .start_addr = BMI270_CONTEXT_MAX_BURST_LEN_STRT_ADDR }, + { .type = BMI2_CRT_GYRO_SELF_TEST, .page = BMI2_PAGE_4, .start_addr = BMI270_CONTEXT_CRT_GYRO_SELF_TEST_STRT_ADDR }, + { .type = BMI2_ABORT_CRT_GYRO_SELF_TEST, .page = BMI2_PAGE_4, .start_addr = BMI270_CONTEXT_ABORT_STRT_ADDR }, + { .type = BMI2_ACTIVITY_RECOGNITION_SETTINGS, .page = BMI2_PAGE_5, + .start_addr = BMI270_CONTEXT_ACT_RGN_SETT_STRT_ADDR }, + { .type = BMI2_ACTIVITY_RECOGNITION, .page = BMI2_PAGE_5, .start_addr = BMI270_CONTEXT_ACT_RGN_STRT_ADDR }, +}; + +/*! @name Global array that stores the feature output configuration */ +const struct bmi2_feature_config bmi270_context_feat_out[BMI270_CONTEXT_MAX_FEAT_OUT] = { + { .type = BMI2_STEP_COUNTER, .page = BMI2_PAGE_0, .start_addr = BMI270_CONTEXT_STEP_CNT_OUT_STRT_ADDR }, + { .type = BMI2_GYRO_CROSS_SENSE, .page = BMI2_PAGE_0, .start_addr = BMI270_CONTEXT_GYRO_CROSS_SENSE_STRT_ADDR }, + { .type = BMI2_GYRO_GAIN_UPDATE, .page = BMI2_PAGE_0, .start_addr = BMI270_CONTEXT_GYR_USER_GAIN_OUT_STRT_ADDR }, + { .type = BMI2_NVM_STATUS, .page = BMI2_PAGE_0, .start_addr = BMI270_CONTEXT_NVM_VFRM_OUT_STRT_ADDR }, + { .type = BMI2_VFRM_STATUS, .page = BMI2_PAGE_0, .start_addr = BMI270_CONTEXT_NVM_VFRM_OUT_STRT_ADDR } +}; + +/*! @name Global array that stores the feature interrupts of BMI270_CONTEXT */ +struct bmi2_map_int bmi270_c_map_int[BMI270_C_MAX_INT_MAP] = { + { .type = BMI2_STEP_COUNTER, .sens_map_int = BMI270_C_INT_STEP_COUNTER_MASK }, + { .type = BMI2_STEP_DETECTOR, .sens_map_int = BMI270_C_INT_STEP_DETECTOR_MASK }, +}; + +/******************************************************************************/ + +/*! Local Function Prototypes + ******************************************************************************/ + +/*! + * @brief This internal API is used to validate the device pointer for + * null conditions. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t null_ptr_check(const struct bmi2_dev *dev); + +/*! + * @brief This internal API enables the selected sensor/features. + * + * @param[in] sensor_sel : Selects the desired sensor. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t sensor_enable(uint64_t sensor_sel, struct bmi2_dev *dev); + +/*! + * @brief This internal API disables the selected sensor/features. + * + * @param[in] sensor_sel : Selects the desired sensor. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t sensor_disable(uint64_t sensor_sel, struct bmi2_dev *dev); + +/*! + * @brief This internal API selects the sensors/features to be enabled or + * disabled. + * + * @param[in] sens_list : Pointer to select the sensor. + * @param[in] n_sens : Number of sensors selected. + * @param[out] sensor_sel : Gets the selected sensor. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t select_sensor(const uint8_t *sens_list, uint8_t n_sens, uint64_t *sensor_sel); + +/*! + * @brief This internal API is used to enable/disable step detector feature. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables step-detector. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables step detector + * BMI2_ENABLE | Enables step detector + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_step_detector(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to enable/disable step counter feature. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables step counter. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables step counter + * BMI2_ENABLE | Enables step counter + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_step_counter(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to enable/disable gyroscope user gain + * feature. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables gyroscope user gain. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables gyroscope user gain + * BMI2_ENABLE | Enables gyroscope user gain + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_gyro_user_gain(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API enables/disables the activity recognition feature. + * + * @param[in] enable : Enables/Disables activity recognition. + * @param[in] dev : Structure instance of bmi2_dev. + * + * enable | Description + * -------------|--------------- + * BMI2_ENABLE | Enables activity recognition. + * BMI2_DISABLE | Disables activity recognition. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_act_recog(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets step counter parameter configurations. + * + * @param[in] step_count_params : Array that stores parameters 1 to 25. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_step_count_params_config(const uint16_t *step_count_params, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets step counter/detector/activity configurations. + * + * @param[in] config : Structure instance of bmi2_step_config. + * @param[in] dev : Structure instance of bmi2_dev. + * + *--------------------------------------------------------------------------- + * bmi2_step_config | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * | The Step-counter will trigger output every time + * | the number of steps are counted. Holds implicitly + * water-mark level | a 20x factor, so the range is 0 to 10230, + * | with resolution of 20 steps. + * -------------------------|--------------------------------------------------- + * reset counter | Flag to reset the counted steps. + * -------------------------|--------------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_step_config(const struct bmi2_step_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets step counter parameter configurations. + * + * @param[in] step_count_params : Array that stores parameters 1 to 25. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_step_count_params_config(uint16_t *step_count_params, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets step counter/detector/activity configurations. + * + * @param[out] config : Structure instance of bmi2_step_config. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @verbatim + *---------------------------------------------------------------------------- + * bmi2_step_config | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * | The Step-counter will trigger output every time + * | the number of steps are counted. Holds implicitly + * water-mark level | a 20x factor, so the range is 0 to 10230, + * | with resolution of 20 steps. + * -------------------------|--------------------------------------------------- + * reset counter | Flag to reset the counted steps. + * -------------------------|--------------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_step_config(struct bmi2_step_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to parse and store the activity recognition + * output from the FIFO data. + * + * @param[out] act_recog : Structure to retrieve output of activity + * recognition frame. + * @param[in] data_index : Index of the FIFO data which contains + * activity recognition frame. + * @param[out] fifo : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t unpack_act_recog_output(struct bmi2_act_recog_output *act_recog, + uint16_t *data_index, + const struct bmi2_fifo_frame *fifo); + +/*! + * @brief This internal API gets the output values of step counter. + * + * @param[out] step_count : Pointer to the stored step counter data. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_step_counter_output(uint32_t *step_count, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets the error status related to NVM. + * + * @param[out] nvm_err_stat : Stores the NVM error status. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_nvm_error_status(struct bmi2_nvm_err_status *nvm_err_stat, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets the error status related to virtual frames. + * + * @param[out] vfrm_err_stat : Stores the VFRM related error status. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_vfrm_error_status(struct bmi2_vfrm_err_status *vfrm_err_stat, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to get enable status of gyroscope user gain + * update. + * + * @param[out] status : Stores status of gyroscope user gain update. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_user_gain_upd_status(uint8_t *status, struct bmi2_dev *dev); + +/*! + * @brief This internal API skips S4S frame in the FIFO data while getting + * activity recognition output. + * + * @param[in, out] frame_header : FIFO frame header. + * @param[in, out] data_index : Index value of the FIFO data bytes + * from which S4S frame header is to be + * skipped. + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t move_if_s4s_frame(const uint8_t *frame_header, uint16_t *data_index, const struct bmi2_fifo_frame *fifo); + +/*! + * @brief This internal API enables/disables compensation of the gain defined + * in the GAIN register. + * + * @param[in] enable : Enables/Disables gain compensation + * @param[in] dev : Structure instance of bmi2_dev. + * + * enable | Description + * -------------|--------------- + * BMI2_ENABLE | Enable gain compensation. + * BMI2_DISABLE | Disable gain compensation. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t enable_gyro_gain(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to extract the output feature configuration + * details like page and start address from the look-up table. + * + * @param[out] feat_output : Structure that stores output feature + * configurations. + * @param[in] type : Type of feature or sensor. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Returns the feature found flag. + * + * @retval BMI2_FALSE : Feature not found + * BMI2_TRUE : Feature found + */ +static uint8_t extract_output_feat_config(struct bmi2_feature_config *feat_output, + uint8_t type, + const struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to move the data index ahead of the + * current frame length parameter when unnecessary FIFO data appears while + * extracting the user specified data. + * + * @param[in,out] data_index : Index of the FIFO data which is to be + * moved ahead of the current frame length + * @param[in] current_frame_length : Number of bytes in the current frame. + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t move_next_frame(uint16_t *data_index, uint8_t current_frame_length, const struct bmi2_fifo_frame *fifo); + +/***************************************************************************/ + +/*! User Interface Definitions + ****************************************************************************/ + +/*! + * @brief This API: + * 1) Updates the device structure with address of the configuration file. + * 2) Initializes BMI270_CONTEXT sensor. + * 3) Writes the configuration file. + * 4) Updates the feature offset parameters in the device structure. + * 5) Updates the maximum number of pages, in the device structure. + */ +int8_t bmi270_context_init(struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + /* Assign chip id of BMI270_CONTEXT */ + dev->chip_id = BMI270_CONTEXT_CHIP_ID; + + /* Get the size of config array */ + dev->config_size = sizeof(bmi270_context_config_file); + + /* Enable the variant specific features if any */ + dev->variant_feature = BMI2_CRT_RTOSK_ENABLE | BMI2_GYRO_CROSS_SENS_ENABLE; + + /* An extra dummy byte is read during SPI read */ + if (dev->intf == BMI2_SPI_INTF) + { + dev->dummy_byte = 1; + } + else + { + dev->dummy_byte = 0; + } + + /* If configuration file pointer is not assigned any address */ + if (!dev->config_file_ptr) + { + /* Give the address of the configuration file array to + * the device pointer + */ + dev->config_file_ptr = bmi270_context_config_file; + } + + /* Initialize BMI2 sensor */ + rslt = bmi2_sec_init(dev); + if (rslt == BMI2_OK) + { + /* Assign the offsets of the feature input + * configuration to the device structure + */ + dev->feat_config = bmi270_context_feat_in; + + /* Assign the offsets of the feature output to + * the device structure + */ + dev->feat_output = bmi270_context_feat_out; + + /* Assign the maximum number of pages to the + * device structure + */ + dev->page_max = BMI270_CONTEXT_MAX_PAGE_NUM; + + /* Assign maximum number of input sensors/ + * features to device structure + */ + dev->input_sens = BMI270_CONTEXT_MAX_FEAT_IN; + + /* Assign maximum number of output sensors/ + * features to device structure + */ + dev->out_sens = BMI270_CONTEXT_MAX_FEAT_OUT; + + /* Assign the offsets of the feature interrupt + * to the device structure + */ + dev->map_int = bmi270_c_map_int; + + /* Assign maximum number of feature interrupts + * to device structure + */ + dev->sens_int_map = BMI270_C_MAX_INT_MAP; + } + } + + return rslt; +} + +/*! + * @brief This API selects the sensors/features to be enabled. + */ +int8_t bmi270_context_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to select sensor */ + uint64_t sensor_sel = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_list != NULL)) + { + /* Get the selected sensors */ + rslt = select_sensor(sens_list, n_sens, &sensor_sel); + if (rslt == BMI2_OK) + { + /* Enable the selected sensors */ + rslt = sensor_enable(sensor_sel, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API selects the sensors/features to be disabled. + */ +int8_t bmi270_context_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to select sensor */ + uint64_t sensor_sel = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_list != NULL)) + { + /* Get the selected sensors */ + rslt = select_sensor(sens_list, n_sens, &sensor_sel); + if (rslt == BMI2_OK) + { + /* Disable the selected sensors */ + rslt = sensor_disable(sensor_sel, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API sets the sensor/feature configuration. + */ +int8_t bmi270_context_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_cfg != NULL)) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + + for (loop = 0; loop < n_sens; loop++) + { + if ((sens_cfg[loop].type == BMI2_ACCEL) || (sens_cfg[loop].type == BMI2_GYRO) || + (sens_cfg[loop].type == BMI2_AUX) || (sens_cfg[loop].type == BMI2_GYRO_GAIN_UPDATE)) + { + rslt = bmi2_set_sensor_config(&sens_cfg[loop], 1, dev); + } + else + { + /* Disable Advance power save if enabled for auxiliary + * and feature configurations + */ + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if + * enabled + */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + switch (sens_cfg[loop].type) + { + /* Set the step counter parameters */ + case BMI2_STEP_COUNTER_PARAMS: + rslt = set_step_count_params_config(sens_cfg[loop].cfg.step_counter_params, dev); + break; + + /* Set step counter/detector/activity configuration */ + case BMI2_STEP_DETECTOR: + case BMI2_STEP_COUNTER: + rslt = set_step_config(&sens_cfg[loop].cfg.step_counter, dev); + break; + + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + } + + /* Return error if any of the set configurations fail */ + if (rslt != BMI2_OK) + { + break; + } + } + } + + /* Enable Advance power save if disabled while configuring and + * not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API gets the sensor/feature configuration. + */ +int8_t bmi270_context_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_cfg != NULL)) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + for (loop = 0; loop < n_sens; loop++) + { + if ((sens_cfg[loop].type == BMI2_ACCEL) || (sens_cfg[loop].type == BMI2_GYRO) || + (sens_cfg[loop].type == BMI2_AUX) || (sens_cfg[loop].type == BMI2_GYRO_GAIN_UPDATE)) + { + rslt = bmi2_get_sensor_config(&sens_cfg[loop], 1, dev); + } + else + { + /* Disable Advance power save if enabled for auxiliary + * and feature configurations + */ + if ((sens_cfg[loop].type >= BMI2_MAIN_SENS_MAX_NUM) || (sens_cfg[loop].type == BMI2_AUX)) + { + + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if + * enabled + */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + } + + if (rslt == BMI2_OK) + { + switch (sens_cfg[loop].type) + { + /* Set the step counter parameters */ + case BMI2_STEP_COUNTER_PARAMS: + rslt = get_step_count_params_config(sens_cfg[loop].cfg.step_counter_params, dev); + break; + + /* Get step counter/detector/activity configuration */ + case BMI2_STEP_DETECTOR: + case BMI2_STEP_COUNTER: + rslt = get_step_config(&sens_cfg[loop].cfg.step_counter, dev); + break; + + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + } + + /* Return error if any of the get configurations fail */ + if (rslt != BMI2_OK) + { + break; + } + } + } + + /* Enable Advance power save if disabled while configuring and + * not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API gets the sensor/feature data for accelerometer, gyroscope, + * auxiliary sensor, step counter, high-g, gyroscope user-gain update, + * orientation, gyroscope cross sensitivity and error status for NVM and VFRM. + */ +int8_t bmi270_context_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sensor_data != NULL)) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + for (loop = 0; loop < n_sens; loop++) + { + if ((sensor_data[loop].type == BMI2_ACCEL) || (sensor_data[loop].type == BMI2_GYRO) || + (sensor_data[loop].type == BMI2_AUX) || (sensor_data[loop].type == BMI2_GYRO_GAIN_UPDATE) || + (sensor_data[loop].type == BMI2_GYRO_CROSS_SENSE)) + { + rslt = bmi2_get_sensor_data(&sensor_data[loop], 1, dev); + } + else + { + /* Disable Advance power save if enabled for feature + * configurations + */ + if (sensor_data[loop].type >= BMI2_MAIN_SENS_MAX_NUM) + { + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if + * enabled + */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + } + + if (rslt == BMI2_OK) + { + switch (sensor_data[loop].type) + { + case BMI2_STEP_COUNTER: + + /* Get step counter output */ + rslt = get_step_counter_output(&sensor_data[loop].sens_data.step_counter_output, dev); + break; + case BMI2_NVM_STATUS: + + /* Get NVM error status */ + rslt = get_nvm_error_status(&sensor_data[loop].sens_data.nvm_status, dev); + break; + case BMI2_VFRM_STATUS: + + /* Get VFRM error status */ + rslt = get_vfrm_error_status(&sensor_data[loop].sens_data.vfrm_status, dev); + break; + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + + /* Return error if any of the get sensor data fails */ + if (rslt != BMI2_OK) + { + break; + } + } + } + + /* Enable Advance power save if disabled while + * configuring and not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This api is used for retrieving the activity recognition settings currently set. + */ +int8_t bmi270_context_get_act_recg_sett(struct bmi2_act_recg_sett *sett, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat; + + /* Variable to set flag */ + uint8_t feat_found; + uint16_t msb_lsb; + uint8_t lsb; + uint8_t msb; + + /* Initialize feature configuration for activity recognition */ + struct bmi2_feature_config bmi2_act_recg_sett = { 0, 0, 0 }; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + + /* Search for bmi2 Abort feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&bmi2_act_recg_sett, BMI2_ACTIVITY_RECOGNITION_SETTINGS, dev); + if (feat_found) + { + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if enabled */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + /* Get the configuration from the page where activity recognition setting feature resides */ + if (rslt == BMI2_OK) + { + rslt = bmi2_get_feat_config(bmi2_act_recg_sett.page, feat_config, dev); + } + + if (rslt == BMI2_OK) + { + /* Define the offset in bytes */ + idx = bmi2_act_recg_sett.start_addr; + + /* get the status of enable/disable post processing */ + sett->act_rec_1 = BMI2_GET_BIT_POS0(feat_config[idx], BMI2_ACT_RECG_POST_PROS_EN_DIS); + + /* increment idx by 2 to point min gdi thres addres */ + idx = idx + 2; + lsb = feat_config[idx]; + idx++; + msb = feat_config[idx]; + msb_lsb = (uint16_t)(lsb | msb << 8); + sett->act_rec_2 = msb_lsb; + + /* increment idx by 1 to point max gdi thres addres */ + idx++; + lsb = feat_config[idx]; + idx++; + msb = feat_config[idx]; + msb_lsb = (uint16_t)(lsb | msb << 8); + sett->act_rec_3 = msb_lsb; + + /* increment idx by 1 to point buffer size */ + idx++; + sett->act_rec_4 = BMI2_GET_BIT_POS0(feat_config[idx], BMI2_ACT_RECG_BUFF_SIZE); + + /* increment idx by 2 to to point to min segment confidence */ + idx = idx + 2; + sett->act_rec_5 = BMI2_GET_BIT_POS0(feat_config[idx], BMI2_ACT_RECG_MIN_SEG_CONF); + } + + /* Enable Advance power save if disabled while + * configuring and not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + } + + return rslt; +} + +/*! + * @brief This api is used for setting the activity recognition settings. + */ +int8_t bmi270_context_set_act_recg_sett(const struct bmi2_act_recg_sett *sett, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for activity recognition */ + struct bmi2_feature_config bmi2_act_recg_sett = { 0, 0, 0 }; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + + /* Search for bmi2 Abort feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&bmi2_act_recg_sett, BMI2_ACTIVITY_RECOGNITION_SETTINGS, dev); + if (feat_found) + { + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if enabled */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + /* Get the configuration from the page where activity recognition setting feature resides */ + if (rslt == BMI2_OK) + { + rslt = bmi2_get_feat_config(bmi2_act_recg_sett.page, feat_config, dev); + } + + if (rslt == BMI2_OK) + { + /* Define the offset in bytes */ + idx = bmi2_act_recg_sett.start_addr; + if ((sett->act_rec_4 > 10) || (sett->act_rec_5 > 10)) + { + rslt = BMI2_E_INVALID_INPUT; + } + + if (rslt == BMI2_OK) + { + feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], + BMI2_ACT_RECG_POST_PROS_EN_DIS, + sett->act_rec_1); + + /* Increment idx by 2 to point min gdi thres addres */ + idx = idx + 2; + feat_config[idx] = BMI2_GET_LSB(sett->act_rec_2); + idx++; + feat_config[idx] = BMI2_GET_MSB(sett->act_rec_2); + + /* Increment idx by 1 to point max gdi thres addres */ + idx++; + feat_config[idx] = BMI2_GET_LSB(sett->act_rec_3); + idx++; + feat_config[idx] = BMI2_GET_MSB(sett->act_rec_3); + + /* Increment idx by 1 to point buffer size */ + idx++; + feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], BMI2_ACT_RECG_BUFF_SIZE, sett->act_rec_4); + + /* Increment idx by 2 to to point to min segment confidence */ + idx = idx + 2; + feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], BMI2_ACT_RECG_MIN_SEG_CONF, sett->act_rec_5); + + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + + /* Enable Advance power save if disabled while + * configuring and not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + } + + return rslt; +} + +/*! + * @brief This internal API is used to parse the activity output from the + * FIFO in header mode. + */ +int8_t bmi270_context_get_act_recog_output(struct bmi2_act_recog_output *act_recog, + uint16_t *act_frm_len, + struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define header frame */ + uint8_t frame_header = 0; + + /* Variable to index the data bytes */ + uint16_t data_index; + + /* Variable to index activity frames */ + uint16_t act_idx = 0; + + /* Variable to indicate activity frames read */ + uint16_t frame_to_read = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (act_recog != NULL) && (act_frm_len != NULL) && (fifo != NULL)) + { + + /* Store the number of frames to be read */ + frame_to_read = *act_frm_len; + for (data_index = fifo->act_recog_byte_start_idx; data_index < fifo->length;) + { + /* Get frame header byte */ + frame_header = fifo->data[data_index] & BMI2_FIFO_TAG_INTR_MASK; + + /* Skip S4S frames if S4S is enabled */ + rslt = move_if_s4s_frame(&frame_header, &data_index, fifo); + + /* Break if FIFO is empty */ + if (rslt == BMI2_W_FIFO_EMPTY) + { + break; + } + + /* Index shifted to next byte where data starts */ + data_index++; + switch (frame_header) + { + /* If header defines accelerometer frame */ + case BMI2_FIFO_HEADER_ACC_FRM: + rslt = move_next_frame(&data_index, fifo->acc_frm_len, fifo); + break; + + /* If header defines accelerometer and auxiliary frames */ + case BMI2_FIFO_HEADER_AUX_ACC_FRM: + rslt = move_next_frame(&data_index, fifo->acc_aux_frm_len, fifo); + break; + + /* If header defines accelerometer and gyroscope frames */ + case BMI2_FIFO_HEADER_GYR_ACC_FRM: + rslt = move_next_frame(&data_index, fifo->acc_gyr_frm_len, fifo); + break; + + /* If header defines accelerometer, auxiliary and gyroscope frames */ + case BMI2_FIFO_HEADER_ALL_FRM: + rslt = move_next_frame(&data_index, fifo->all_frm_len, fifo); + break; + + /* If header defines only gyroscope frame */ + case BMI2_FIFO_HEADER_GYR_FRM: + rslt = move_next_frame(&data_index, fifo->gyr_frm_len, fifo); + break; + + /* If header defines only auxiliary frame */ + case BMI2_FIFO_HEADER_AUX_FRM: + rslt = move_next_frame(&data_index, fifo->aux_frm_len, fifo); + break; + + /* If header defines auxiliary and gyroscope frame */ + case BMI2_FIFO_HEADER_AUX_GYR_FRM: + rslt = move_next_frame(&data_index, fifo->aux_gyr_frm_len, fifo); + break; + + /* If header defines sensor time frame */ + case BMI2_FIFO_HEADER_SENS_TIME_FRM: + rslt = move_next_frame(&data_index, BMI2_SENSOR_TIME_LENGTH, fifo); + break; + + /* If header defines skip frame */ + case BMI2_FIFO_HEADER_SKIP_FRM: + rslt = move_next_frame(&data_index, BMI2_FIFO_SKIP_FRM_LENGTH, fifo); + break; + + /* If header defines Input configuration frame */ + case BMI2_FIFO_HEADER_INPUT_CFG_FRM: + rslt = move_next_frame(&data_index, BMI2_FIFO_INPUT_CFG_LENGTH, fifo); + break; + + /* If header defines invalid frame or end of valid data */ + case BMI2_FIFO_HEAD_OVER_READ_MSB: + + /* Move the data index to the last byte to mark completion */ + data_index = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + + /* If header defines activity recognition frame */ + case BMI2_FIFO_VIRT_ACT_RECOG_FRM: + + /* Get the activity output */ + rslt = unpack_act_recog_output(&act_recog[(act_idx)], &data_index, fifo); + + /* Update activity frame index */ + (act_idx)++; + break; + default: + + /* Move the data index to the last byte in case of invalid values */ + data_index = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + } + + /* Number of frames to be read is complete or FIFO is empty */ + if ((frame_to_read == act_idx) || (rslt == BMI2_W_FIFO_EMPTY)) + { + break; + } + } + + /* Update the activity frame index */ + (*act_frm_len) = act_idx; + + /* Update the activity byte index */ + fifo->act_recog_byte_start_idx = data_index; + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API updates the gyroscope user-gain. + */ +int8_t bmi270_context_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to select sensor */ + uint8_t sens_sel[2] = { BMI2_GYRO, BMI2_GYRO_GAIN_UPDATE }; + + /* Structure to define sensor configurations */ + struct bmi2_sens_config sens_cfg; + + /* Variable to store status of user-gain update module */ + uint8_t status = 0; + + /* Variable to define count */ + uint8_t count = 100; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (user_gain != NULL)) + { + /* Select type of feature */ + sens_cfg.type = BMI2_GYRO_GAIN_UPDATE; + + /* Get the user gain configurations */ + rslt = bmi270_context_get_sensor_config(&sens_cfg, 1, dev); + if (rslt == BMI2_OK) + { + /* Get the user-defined ratio */ + sens_cfg.cfg.gyro_gain_update = *user_gain; + + /* Set rate ratio for all axes */ + rslt = bmi270_context_set_sensor_config(&sens_cfg, 1, dev); + } + + /* Disable gyroscope */ + if (rslt == BMI2_OK) + { + rslt = bmi270_context_sensor_disable(&sens_sel[0], 1, dev); + } + + /* Enable gyroscope user-gain update module */ + if (rslt == BMI2_OK) + { + rslt = bmi270_context_sensor_enable(&sens_sel[1], 1, dev); + } + + /* Set the command to trigger the computation */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_command_register(BMI2_USR_GAIN_CMD, dev); + } + + if (rslt == BMI2_OK) + { + /* Poll until enable bit of user-gain update is 0 */ + while (count--) + { + rslt = get_user_gain_upd_status(&status, dev); + if ((rslt == BMI2_OK) && (status == 0)) + { + /* Enable compensation of gain defined + * in the GAIN register + */ + rslt = enable_gyro_gain(BMI2_ENABLE, dev); + + /* Enable gyroscope */ + if (rslt == BMI2_OK) + { + rslt = bmi270_context_sensor_enable(&sens_sel[0], 1, dev); + } + + break; + } + + dev->delay_us(10000, dev->intf_ptr); + } + + /* Return error if user-gain update is failed */ + if ((rslt == BMI2_OK) && (status != 0)) + { + rslt = BMI2_E_GYR_USER_GAIN_UPD_FAIL; + } + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API reads the compensated gyroscope user-gain values. + */ +int8_t bmi270_context_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define register data */ + uint8_t reg_data[3] = { 0 }; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (gyr_usr_gain != NULL)) + { + /* Get the gyroscope compensated gain values */ + rslt = bmi2_get_regs(BMI2_GYR_USR_GAIN_0_ADDR, reg_data, 3, dev); + if (rslt == BMI2_OK) + { + /* Gyroscope user gain correction X-axis */ + gyr_usr_gain->x = (int8_t)BMI2_GET_BIT_POS0(reg_data[0], BMI2_GYR_USR_GAIN_X); + + /* Gyroscope user gain correction Y-axis */ + gyr_usr_gain->y = (int8_t)BMI2_GET_BIT_POS0(reg_data[1], BMI2_GYR_USR_GAIN_Y); + + /* Gyroscope user gain correction z-axis */ + gyr_usr_gain->z = (int8_t)BMI2_GET_BIT_POS0(reg_data[2], BMI2_GYR_USR_GAIN_Z); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API maps/unmaps feature interrupts to that of interrupt pins. + */ +int8_t bmi270_context_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_int != NULL)) + { + for (loop = 0; loop < n_sens; loop++) + { + switch (sens_int[loop].type) + { + case BMI2_STEP_COUNTER: + case BMI2_STEP_DETECTOR: + + rslt = bmi2_map_feat_int(sens_int[loop].type, sens_int[loop].hw_int_pin, dev); + break; + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + + /* Return error if interrupt mapping fails */ + if (rslt != BMI2_OK) + { + break; + } + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/***************************************************************************/ + +/*! Local Function Definitions + ****************************************************************************/ + +/*! + * @brief This internal API is used to validate the device structure pointer for + * null conditions. + */ +static int8_t null_ptr_check(const struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + if ((dev == NULL) || (dev->read == NULL) || (dev->write == NULL) || (dev->delay_us == NULL)) + { + /* Device structure pointer is not valid */ + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This internal API selects the sensor/features to be enabled or + * disabled. + */ +static int8_t select_sensor(const uint8_t *sens_list, uint8_t n_sens, uint64_t *sensor_sel) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Variable to define loop */ + uint8_t count; + + for (count = 0; count < n_sens; count++) + { + switch (sens_list[count]) + { + case BMI2_ACCEL: + *sensor_sel |= BMI2_ACCEL_SENS_SEL; + break; + case BMI2_GYRO: + *sensor_sel |= BMI2_GYRO_SENS_SEL; + break; + case BMI2_AUX: + *sensor_sel |= BMI2_AUX_SENS_SEL; + break; + case BMI2_TEMP: + *sensor_sel |= BMI2_TEMP_SENS_SEL; + break; + case BMI2_STEP_DETECTOR: + *sensor_sel |= BMI2_STEP_DETECT_SEL; + break; + case BMI2_STEP_COUNTER: + *sensor_sel |= BMI2_STEP_COUNT_SEL; + break; + case BMI2_GYRO_GAIN_UPDATE: + *sensor_sel |= BMI2_GYRO_GAIN_UPDATE_SEL; + break; + case BMI2_ACTIVITY_RECOGNITION: + *sensor_sel |= BMI2_ACTIVITY_RECOGNITION_SEL; + break; + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + } + + return rslt; +} + +/*! + * @brief This internal API enables the selected sensor/features. + */ +static int8_t sensor_enable(uint64_t sensor_sel, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store register values */ + uint8_t reg_data = 0; + + /* Variable to define loop */ + uint8_t loop = 1; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + /* Enable accelerometer */ + if (sensor_sel & BMI2_ACCEL_SENS_SEL) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_ACC_EN, BMI2_ENABLE); + } + + /* Enable gyroscope */ + if (sensor_sel & BMI2_GYRO_SENS_SEL) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_EN, BMI2_ENABLE); + } + + /* Enable auxiliary sensor */ + if (sensor_sel & BMI2_AUX_SENS_SEL) + { + reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_AUX_EN, BMI2_ENABLE); + } + + /* Enable temperature sensor */ + if (sensor_sel & BMI2_TEMP_SENS_SEL) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_TEMP_EN, BMI2_ENABLE); + } + + /* Enable the sensors that are set in the power control register */ + if (sensor_sel & BMI2_MAIN_SENSORS) + { + rslt = bmi2_set_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + } + } + + if ((rslt == BMI2_OK) && (sensor_sel & ~(BMI2_MAIN_SENSORS))) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if enabled */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + while (loop--) + { + /* Enable step detector feature */ + if (sensor_sel & BMI2_STEP_DETECT_SEL) + { + rslt = set_step_detector(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_STEP_DETECT_SEL; + } + else + { + break; + } + } + + /* Enable step counter feature */ + if (sensor_sel & BMI2_STEP_COUNT_SEL) + { + rslt = set_step_counter(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_STEP_COUNT_SEL; + } + else + { + break; + } + } + + /* Enable gyroscope user gain */ + if (sensor_sel & BMI2_GYRO_GAIN_UPDATE_SEL) + { + rslt = set_gyro_user_gain(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_GYRO_GAIN_UPDATE_SEL; + } + else + { + break; + } + } + + /* Enable activity recognition feature */ + if (sensor_sel & BMI2_ACTIVITY_RECOGNITION_SEL) + { + rslt = set_act_recog(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_ACTIVITY_RECOGNITION_SEL; + } + else + { + break; + } + } + } + + /* Enable Advance power save if disabled while + * configuring and not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + } + + return rslt; +} + +/*! + * @brief This internal API disables the selected sensors/features. + */ +static int8_t sensor_disable(uint64_t sensor_sel, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store register values */ + uint8_t reg_data = 0; + + /* Variable to define loop */ + uint8_t loop = 1; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + /* Disable accelerometer */ + if (sensor_sel & BMI2_ACCEL_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_ACC_EN); + } + + /* Disable gyroscope */ + if (sensor_sel & BMI2_GYRO_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_GYR_EN); + } + + /* Disable auxiliary sensor */ + if (sensor_sel & BMI2_AUX_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_AUX_EN); + } + + /* Disable temperature sensor */ + if (sensor_sel & BMI2_TEMP_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_TEMP_EN); + } + + /* Disable the sensors that are set in the power control register */ + if (sensor_sel & BMI2_MAIN_SENSORS) + { + rslt = bmi2_set_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + } + } + + if ((rslt == BMI2_OK) && (sensor_sel & ~(BMI2_MAIN_SENSORS))) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if enabled */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + while (loop--) + { + /* Disable step detector feature */ + if (sensor_sel & BMI2_STEP_DETECT_SEL) + { + rslt = set_step_detector(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_STEP_DETECT_SEL; + } + else + { + break; + } + } + + /* Disable step counter feature */ + if (sensor_sel & BMI2_STEP_COUNT_SEL) + { + rslt = set_step_counter(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_STEP_COUNT_SEL; + } + else + { + break; + } + } + + /* Disable gyroscope user gain */ + if (sensor_sel & BMI2_GYRO_GAIN_UPDATE_SEL) + { + rslt = set_gyro_user_gain(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_GYRO_GAIN_UPDATE_SEL; + } + else + { + break; + } + } + + if (sensor_sel & BMI2_ACTIVITY_RECOGNITION_SEL) + { + rslt = set_act_recog(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_ACTIVITY_RECOGNITION_SEL; + } + else + { + break; + } + } + + /* Enable Advance power save if disabled while + * configuring and not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + } + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable step detector feature. + */ +static int8_t set_step_detector(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step detector */ + struct bmi2_feature_config step_det_config = { 0, 0, 0 }; + + /* Search for step detector feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_det_config, BMI2_STEP_DETECTOR, dev); + if (feat_found) + { + /* Get the configuration from the page where step detector feature resides */ + rslt = bmi2_get_feat_config(step_det_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of step detector */ + idx = step_det_config.start_addr + BMI2_STEP_COUNT_FEAT_EN_OFFSET; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_STEP_DET_FEAT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable step counter feature. + */ +static int8_t set_step_counter(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step counter */ + struct bmi2_feature_config step_count_config = { 0, 0, 0 }; + + /* Search for step counter feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); + if (feat_found) + { + /* Get the configuration from the page where step-counter feature resides */ + rslt = bmi2_get_feat_config(step_count_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of step counter */ + idx = step_count_config.start_addr + BMI2_STEP_COUNT_FEAT_EN_OFFSET; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_STEP_COUNT_FEAT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable gyroscope user gain + * feature. + */ +static int8_t set_gyro_user_gain(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for gyroscope user gain */ + struct bmi2_feature_config gyr_user_gain_cfg = { 0, 0, 0 }; + + /* Search for user gain feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&gyr_user_gain_cfg, BMI2_GYRO_GAIN_UPDATE, dev); + if (feat_found) + { + /* Get the configuration from the page where user gain feature resides */ + rslt = bmi2_get_feat_config(gyr_user_gain_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of user gain */ + idx = gyr_user_gain_cfg.start_addr + BMI2_GYR_USER_GAIN_FEAT_EN_OFFSET; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_GYR_USER_GAIN_FEAT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API enables/disables the activity recognition feature. + */ +static int8_t set_act_recog(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for activity recognition */ + struct bmi2_feature_config act_recog_cfg = { 0, 0, 0 }; + + /* Search for activity recognition and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&act_recog_cfg, BMI2_ACTIVITY_RECOGNITION, dev); + if (feat_found) + { + /* Get the configuration from the page where activity + * recognition feature resides + */ + rslt = bmi2_get_feat_config(act_recog_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of activity recognition */ + idx = act_recog_cfg.start_addr; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], BMI2_ACTIVITY_RECOG_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API sets step counter parameter configurations. + */ +static int8_t set_step_count_params_config(const uint16_t *step_count_params, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define index */ + uint8_t index = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step counter parameters */ + struct bmi2_feature_config step_params_config = { 0, 0, 0 }; + + /* Variable to index the page number */ + uint8_t page_idx; + + /* Variable to define the start page */ + uint8_t start_page; + + /* Variable to define start address of the parameters */ + uint8_t start_addr; + + /* Variable to define number of bytes */ + uint8_t n_bytes = (BMI2_STEP_CNT_N_PARAMS * 2); + + /* Variable to store number of pages */ + uint8_t n_pages = (n_bytes / 16); + + /* Variable to define the end page */ + uint8_t end_page; + + /* Variable to define the remaining bytes to be read */ + uint8_t remain_len; + + /* Variable to define the maximum words(16 bytes or 8 words) to be read in a page */ + uint8_t max_len = 8; + + /* Variable index bytes in a page */ + uint8_t page_byte_idx; + + /* Variable to index the parameters */ + uint8_t param_idx = 0; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for step counter parameter feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_params_config, BMI2_STEP_COUNTER_PARAMS, dev); + if (feat_found) + { + /* Get the start page for the step counter parameters */ + start_page = step_params_config.page; + + /* Get the end page for the step counter parameters */ + end_page = start_page + n_pages; + + /* Get the start address for the step counter parameters */ + start_addr = step_params_config.start_addr; + + /* Get the remaining length of bytes to be read */ + remain_len = (uint8_t)((n_bytes - (n_pages * 16)) + start_addr); + for (page_idx = start_page; page_idx <= end_page; page_idx++) + { + /* Get the configuration from the respective page */ + rslt = bmi2_get_feat_config(page_idx, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Start from address 0x00 when switched to next page */ + if (page_idx > start_page) + { + start_addr = 0; + } + + /* Remaining number of words to be read in the page */ + if (page_idx == end_page) + { + max_len = (remain_len / 2); + } + + /* Get offset in words since all the features are set in words length */ + page_byte_idx = start_addr / 2; + for (; page_byte_idx < max_len;) + { + /* Set parameters 1 to 25 */ + *(data_p + page_byte_idx) = BMI2_SET_BIT_POS0(*(data_p + page_byte_idx), + BMI2_STEP_COUNT_PARAMS, + step_count_params[param_idx]); + + /* Increment offset by 1 word to set to the next parameter */ + page_byte_idx++; + + /* Increment to next parameter */ + param_idx++; + } + + /* Get total length in bytes to copy from local pointer to the array */ + page_byte_idx = (uint8_t)(page_byte_idx * 2) - step_params_config.start_addr; + + /* Copy the bytes to be set back to the array */ + for (index = 0; index < page_byte_idx; index++) + { + feat_config[step_params_config.start_addr + + index] = *((uint8_t *) data_p + step_params_config.start_addr + index); + } + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/* @brief This internal API sets step counter configurations like water-mark + * level, reset-counter and output-configuration step detector and activity. + */ +static int8_t set_step_config(const struct bmi2_step_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define index */ + uint8_t index = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step counter 4 */ + struct bmi2_feature_config step_count_config = { 0, 0, 0 }; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for step counter feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); + if (feat_found) + { + /* Get the configuration from the page where step counter resides */ + rslt = bmi2_get_feat_config(step_count_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes */ + idx = step_count_config.start_addr; + + /* Get offset in words since all the features are set in words length */ + idx = idx / 2; + + /* Set water-mark level */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_STEP_COUNT_WM_LEVEL, config->watermark_level); + + /* Set reset-counter */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_STEP_COUNT_RST_CNT, config->reset_counter); + + /* Increment offset by 1 word to set output + * configuration of step detector and step activity + */ + idx++; + + /* Set step buffer size */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_STEP_BUFFER_SIZE, config->step_buffer_size); + + /* Increment offset by 1 more word to get the total length in words */ + idx++; + + /* Get total length in bytes to copy from local pointer to the array */ + idx = (uint8_t)(idx * 2) - step_count_config.start_addr; + + /* Copy the bytes to be set back to the array */ + for (index = 0; index < idx; index++) + { + feat_config[step_count_config.start_addr + + index] = *((uint8_t *) data_p + step_count_config.start_addr + index); + } + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets step counter parameter configurations. + */ +static int8_t get_step_count_params_config(uint16_t *step_count_params, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Variable to define LSB */ + uint16_t lsb = 0; + + /* Variable to define MSB */ + uint16_t msb = 0; + + /* Variable to define a word */ + uint16_t lsb_msb = 0; + + /* Initialize feature configuration for step counter 1 */ + struct bmi2_feature_config step_params_config = { 0, 0, 0 }; + + /* Variable to index the page number */ + uint8_t page_idx; + + /* Variable to define the start page */ + uint8_t start_page; + + /* Variable to define start address of the parameters */ + uint8_t start_addr; + + /* Variable to define number of bytes */ + uint8_t n_bytes = (BMI2_STEP_CNT_N_PARAMS * 2); + + /* Variable to store number of pages */ + uint8_t n_pages = (n_bytes / 16); + + /* Variable to define the end page */ + uint8_t end_page; + + /* Variable to define the remaining bytes to be read */ + uint8_t remain_len; + + /* Variable to define the maximum words to be read in a page */ + uint8_t max_len = BMI2_FEAT_SIZE_IN_BYTES; + + /* Variable index bytes in a page */ + uint8_t page_byte_idx; + + /* Variable to index the parameters */ + uint8_t param_idx = 0; + + /* Search for step counter parameter feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_params_config, BMI2_STEP_COUNTER_PARAMS, dev); + if (feat_found) + { + /* Get the start page for the step counter parameters */ + start_page = step_params_config.page; + + /* Get the end page for the step counter parameters */ + end_page = start_page + n_pages; + + /* Get the start address for the step counter parameters */ + start_addr = step_params_config.start_addr; + + /* Get the remaining length of bytes to be read */ + remain_len = (uint8_t)((n_bytes - (n_pages * 16)) + start_addr); + for (page_idx = start_page; page_idx <= end_page; page_idx++) + { + /* Get the configuration from the respective page */ + rslt = bmi2_get_feat_config(page_idx, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Start from address 0x00 when switched to next page */ + if (page_idx > start_page) + { + start_addr = 0; + } + + /* Remaining number of bytes to be read in the page */ + if (page_idx == end_page) + { + max_len = remain_len; + } + + /* Get the offset */ + page_byte_idx = start_addr; + while (page_byte_idx < max_len) + { + /* Get word to calculate the parameter*/ + lsb = (uint16_t) feat_config[page_byte_idx++]; + if (page_byte_idx < max_len) + { + msb = ((uint16_t) feat_config[page_byte_idx++] << 8); + } + + lsb_msb = lsb | msb; + + /* Get parameters 1 to 25 */ + step_count_params[param_idx] = lsb_msb & BMI2_STEP_COUNT_PARAMS_MASK; + + /* Increment to next parameter */ + param_idx++; + } + } + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets step counter/detector/activity configurations. + */ +static int8_t get_step_config(struct bmi2_step_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define LSB */ + uint16_t lsb = 0; + + /* Variable to define MSB */ + uint16_t msb = 0; + + /* Variable to define a word */ + uint16_t lsb_msb = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step counter */ + struct bmi2_feature_config step_count_config = { 0, 0, 0 }; + + /* Search for step counter 4 feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); + if (feat_found) + { + /* Get the configuration from the page where step counter 4 parameter resides */ + rslt = bmi2_get_feat_config(step_count_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for feature enable for step counter/detector/activity */ + idx = step_count_config.start_addr; + + /* Get word to calculate water-mark level and reset counter */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get water-mark level */ + config->watermark_level = lsb_msb & BMI2_STEP_COUNT_WM_LEVEL_MASK; + + /* Get reset counter */ + config->reset_counter = (lsb_msb & BMI2_STEP_COUNT_RST_CNT_MASK) >> BMI2_STEP_COUNT_RST_CNT_POS; + + /* Get word to calculate output configuration of step detector and activity */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + config->step_buffer_size = (lsb_msb & BMI2_STEP_BUFFER_SIZE_MASK) >> BMI2_STEP_BUFFER_SIZE_POS; + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets the output values of step counter. + */ +static int8_t get_step_counter_output(uint32_t *step_count, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variables to define index */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature output for step counter */ + struct bmi2_feature_config step_cnt_out_config = { 0, 0, 0 }; + + /* Search for step counter output feature and extract its configuration details */ + feat_found = extract_output_feat_config(&step_cnt_out_config, BMI2_STEP_COUNTER, dev); + if (feat_found) + { + /* Get the feature output configuration for step-counter */ + rslt = bmi2_get_feat_config(step_cnt_out_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for step counter output */ + idx = step_cnt_out_config.start_addr; + + /* Get the step counter output in 4 bytes */ + *step_count = (uint32_t) feat_config[idx++]; + *step_count |= ((uint32_t) feat_config[idx++] << 8); + *step_count |= ((uint32_t) feat_config[idx++] << 16); + *step_count |= ((uint32_t) feat_config[idx++] << 24); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets the error status related to NVM. + */ +static int8_t get_nvm_error_status(struct bmi2_nvm_err_status *nvm_err_stat, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variables to define index */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature output for NVM error status */ + struct bmi2_feature_config nvm_err_cfg = { 0, 0, 0 }; + + /* Search for NVM error status feature and extract its configuration details */ + feat_found = extract_output_feat_config(&nvm_err_cfg, BMI2_NVM_STATUS, dev); + if (feat_found) + { + /* Get the feature output configuration for NVM error status */ + rslt = bmi2_get_feat_config(nvm_err_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for NVM error status */ + idx = nvm_err_cfg.start_addr; + + /* Increment index to get the error status */ + idx++; + + /* Error when NVM load action fails */ + nvm_err_stat->load_error = BMI2_GET_BIT_POS0(feat_config[idx], BMI2_NVM_LOAD_ERR_STATUS); + + /* Error when NVM program action fails */ + nvm_err_stat->prog_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_PROG_ERR_STATUS); + + /* Error when NVM erase action fails */ + nvm_err_stat->erase_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_ERASE_ERR_STATUS); + + /* Error when NVM program limit is exceeded */ + nvm_err_stat->exceed_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_END_EXCEED_STATUS); + + /* Error when NVM privilege mode is not acquired */ + nvm_err_stat->privil_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_PRIV_ERR_STATUS); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to get enable status of gyroscope user gain + * update. + */ +static int8_t get_user_gain_upd_status(uint8_t *status, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Variable to check APS status */ + uint8_t aps_stat = 0; + + /* Initialize feature configuration for gyroscope user gain */ + struct bmi2_feature_config gyr_user_gain_cfg = { 0, 0, 0 }; + + /* Search for user gain feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&gyr_user_gain_cfg, BMI2_GYRO_GAIN_UPDATE, dev); + if (feat_found) + { + /* Disable advance power save */ + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + /* Get the configuration from the page where user gain feature resides */ + rslt = bmi2_get_feat_config(gyr_user_gain_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of user gain */ + idx = gyr_user_gain_cfg.start_addr + BMI2_GYR_USER_GAIN_FEAT_EN_OFFSET; + + /* Set the feature enable status */ + *status = BMI2_GET_BITS(feat_config[idx], BMI2_GYR_USER_GAIN_FEAT_EN); + } + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + /* Enable Advance power save if disabled while configuring and not when already disabled */ + if ((rslt == BMI2_OK) && (aps_stat == BMI2_ENABLE)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + + return rslt; +} + +/*! + * @brief This internal API is used to parse and store the activity recognition + * output from the FIFO data. + */ +static int8_t unpack_act_recog_output(struct bmi2_act_recog_output *act_recog, + uint16_t *data_index, + const struct bmi2_fifo_frame *fifo) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Variables to define 4 bytes of sensor time */ + uint32_t time_stamp_byte4 = 0; + uint32_t time_stamp_byte3 = 0; + uint32_t time_stamp_byte2 = 0; + uint32_t time_stamp_byte1 = 0; + + /* Validate data index */ + if ((*data_index + BMI2_FIFO_VIRT_ACT_DATA_LENGTH) >= fifo->length) + { + /* Update the data index to the last byte */ + (*data_index) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + } + else + { + /* Get time-stamp from the activity recognition frame */ + time_stamp_byte4 = ((uint32_t)(fifo->data[(*data_index) + 3]) << 24); + time_stamp_byte3 = ((uint32_t)(fifo->data[(*data_index) + 2]) << 16); + time_stamp_byte2 = fifo->data[(*data_index) + 1] << 8; + time_stamp_byte1 = fifo->data[(*data_index)]; + + /* Update time-stamp from the virtual frame */ + act_recog->time_stamp = (time_stamp_byte4 | time_stamp_byte3 | time_stamp_byte2 | time_stamp_byte1); + + /* Move the data index by 4 bytes */ + (*data_index) = (*data_index) + BMI2_FIFO_VIRT_ACT_TIME_LENGTH; + + /* Update the previous activity from the virtual frame */ + act_recog->prev_act = fifo->data[(*data_index)]; + + /* Move the data index by 1 byte */ + (*data_index) = (*data_index) + BMI2_FIFO_VIRT_ACT_TYPE_LENGTH; + + /* Update the current activity from the virtual frame */ + act_recog->curr_act = fifo->data[(*data_index)]; + + /* Move the data index by 1 byte */ + (*data_index) = (*data_index) + BMI2_FIFO_VIRT_ACT_STAT_LENGTH; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + } + + return rslt; +} + +/*! + * @brief This internal API gets the error status related to virtual frames. + */ +static int8_t get_vfrm_error_status(struct bmi2_vfrm_err_status *vfrm_err_stat, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variables to define index */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature output for VFRM error status */ + struct bmi2_feature_config vfrm_err_cfg = { 0, 0, 0 }; + + /* Search for VFRM error status feature and extract its configuration details */ + feat_found = extract_output_feat_config(&vfrm_err_cfg, BMI2_VFRM_STATUS, dev); + if (feat_found) + { + /* Get the feature output configuration for VFRM error status */ + rslt = bmi2_get_feat_config(vfrm_err_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for VFRM error status */ + idx = vfrm_err_cfg.start_addr; + + /* Increment index to get the error status */ + idx++; + + /* Internal error while acquiring lock for FIFO */ + vfrm_err_stat->lock_error = BMI2_GET_BITS(feat_config[idx], BMI2_VFRM_LOCK_ERR_STATUS); + + /* Internal error while writing byte into FIFO */ + vfrm_err_stat->write_error = BMI2_GET_BITS(feat_config[idx], BMI2_VFRM_WRITE_ERR_STATUS); + + /* Internal error while writing into FIFO */ + vfrm_err_stat->fatal_error = BMI2_GET_BITS(feat_config[idx], BMI2_VFRM_FATAL_ERR_STATUS); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API skips S4S frame in the FIFO data while getting + * step activity output. + */ +static int8_t move_if_s4s_frame(const uint8_t *frame_header, uint16_t *data_index, const struct bmi2_fifo_frame *fifo) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Variable to extract virtual header byte */ + uint8_t virtual_header_mode; + + /* Variable to define pay-load in words */ + uint8_t payload_word = 0; + + /* Variable to define pay-load in bytes */ + uint8_t payload_bytes = 0; + + /* Extract virtual header mode from the frame header */ + virtual_header_mode = BMI2_GET_BITS(*frame_header, BMI2_FIFO_VIRT_FRM_MODE); + + /* If the extracted header byte is a virtual header */ + if (virtual_header_mode == BMI2_FIFO_VIRT_FRM_MODE) + { + /* If frame header is not activity recognition header */ + if (*frame_header != 0xC8) + { + /* Extract pay-load in words from the header byte */ + payload_word = BMI2_GET_BITS(*frame_header, BMI2_FIFO_VIRT_PAYLOAD) + 1; + + /* Convert to bytes */ + payload_bytes = (uint8_t)(payload_word * 2); + + /* Move the data index by those pay-load bytes */ + rslt = move_next_frame(data_index, payload_bytes, fifo); + } + } + + return rslt; +} + +/*! + * @brief This internal API enables/disables compensation of the gain defined + * in the GAIN register. + */ +static int8_t enable_gyro_gain(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define register data */ + uint8_t reg_data = 0; + + rslt = bmi2_get_regs(BMI2_GYR_OFF_COMP_6_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_GAIN_EN, enable); + rslt = bmi2_set_regs(BMI2_GYR_OFF_COMP_6_ADDR, ®_data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This internal API is used to extract the output feature configuration + * details from the look-up table. + */ +static uint8_t extract_output_feat_config(struct bmi2_feature_config *feat_output, + uint8_t type, + const struct bmi2_dev *dev) +{ + /* Variable to define loop */ + uint8_t loop = 0; + + /* Variable to set flag */ + uint8_t feat_found = BMI2_FALSE; + + /* Search for the output feature from the output configuration array */ + while (loop < dev->out_sens) + { + if (dev->feat_output[loop].type == type) + { + *feat_output = dev->feat_output[loop]; + feat_found = BMI2_TRUE; + break; + } + + loop++; + } + + /* Return flag */ + return feat_found; +} + +/*! + * @brief This internal API is used to move the data index ahead of the + * current_frame_length parameter when unnecessary FIFO data appears while + * extracting the user specified data. + */ +static int8_t move_next_frame(uint16_t *data_index, uint8_t current_frame_length, const struct bmi2_fifo_frame *fifo) +{ + /* Variables to define error */ + int8_t rslt = BMI2_OK; + + /* Validate data index */ + if (((*data_index) + current_frame_length) > fifo->length) + { + /* Move the data index to the last byte */ + (*data_index) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + } + else + { + /* Move the data index to next frame */ + (*data_index) = (*data_index) + current_frame_length; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_context.h b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_context.h new file mode 100644 index 000000000..4708dc7db --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_context.h @@ -0,0 +1,493 @@ +/** +* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. +* +* BSD-3-Clause +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* 3. Neither the name of the copyright holder nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING +* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* @file bmi270_context.h +* @date 2020-11-04 +* @version v2.63.1 +* +*/ + +/** + * \ingroup bmi2xy + * \defgroup bmi270_context BMI270_CONTEXT + * @brief Sensor driver for BMI270_CONTEXT sensor + */ + +#ifndef BMI270_CONTEXT_H_ +#define BMI270_CONTEXT_H_ + +/*! CPP guard */ +#ifdef __cplusplus +extern "C" { +#endif + +/***************************************************************************/ + +/*! Header files + ****************************************************************************/ +#include "bmi2.h" + +/***************************************************************************/ + +/*! Macro definitions + ****************************************************************************/ + +/*! @name BMI270_CONTEXT Chip identifier */ +#define BMI270_CONTEXT_CHIP_ID UINT8_C(0x24) + +/*! @name BMI270_CONTEXT feature input start addresses */ +#define BMI270_CONTEXT_CONFIG_ID_STRT_ADDR UINT8_C(0x06) +#define BMI270_CONTEXT_STEP_CNT_1_STRT_ADDR UINT8_C(0x00) +#define BMI270_CONTEXT_STEP_CNT_4_STRT_ADDR UINT8_C(0x02) +#define BMI270_CONTEXT_MAX_BURST_LEN_STRT_ADDR UINT8_C(0x08) +#define BMI270_CONTEXT_CRT_GYRO_SELF_TEST_STRT_ADDR UINT8_C(0x09) +#define BMI270_CONTEXT_ABORT_STRT_ADDR UINT8_C(0x09) +#define BMI270_CONTEXT_NVM_PROG_PREP_STRT_ADDR UINT8_C(0x0A) +#define BMI270_CONTEXT_ACT_RGN_SETT_STRT_ADDR UINT8_C(0x00) +#define BMI270_CONTEXT_ACT_RGN_STRT_ADDR UINT8_C(0x0A) + +/*! @name BMI270_CONTEXT feature output start addresses */ +#define BMI270_CONTEXT_STEP_CNT_OUT_STRT_ADDR UINT8_C(0x00) +#define BMI270_CONTEXT_GYR_USER_GAIN_OUT_STRT_ADDR UINT8_C(0x04) +#define BMI270_CONTEXT_GYRO_CROSS_SENSE_STRT_ADDR UINT8_C(0x0C) +#define BMI270_CONTEXT_NVM_VFRM_OUT_STRT_ADDR UINT8_C(0x0E) + +/*! @name Defines maximum number of pages */ +#define BMI270_CONTEXT_MAX_PAGE_NUM UINT8_C(8) + +/*! @name Defines maximum number of feature input configurations */ +#define BMI270_CONTEXT_MAX_FEAT_IN UINT8_C(10) + +/*! @name Defines maximum number of feature outputs */ +#define BMI270_CONTEXT_MAX_FEAT_OUT UINT8_C(5) + +/*! @name Mask definitions for feature interrupt status bits */ +#define BMI270_CONTEXT_STEP_CNT_STATUS_MASK UINT8_C(0x01) + +/*! @name Mask definitions for feature interrupt mapping bits */ +#define BMI270_C_INT_STEP_COUNTER_MASK UINT8_C(0x01) +#define BMI270_C_INT_STEP_DETECTOR_MASK UINT8_C(0x01) + +/*! @name Defines maximum number of feature interrupts */ +#define BMI270_C_MAX_INT_MAP UINT8_C(2) + +/***************************************************************************/ + +/*! BMI270_CONTEXT User Interface function prototypes + ****************************************************************************/ + +/** + * \ingroup bmi270_context + * \defgroup bmi270_contextApiInit Initialization + * @brief Initialize the sensor and device structure + */ + +/*! + * \ingroup bmi270_contextApiInit + * \page bmi270_context_api_bmi270_context_init bmi270_context_init + * \code + * int8_t bmi270_context_init(struct bmi2_dev *dev); + * \endcode + * @details This API: + * 1) updates the device structure with address of the configuration file. + * 2) Initializes BMI270_CONTEXT sensor. + * 3) Writes the configuration file. + * 4) Updates the feature offset parameters in the device structure. + * 5) Updates the maximum number of pages, in the device structure. + * + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_context_init(struct bmi2_dev *dev); + +/** + * \ingroup bmi270_context + * \defgroup bmi270_contextApiSensor Feature Set + * @brief Enable / Disable features of the sensor + */ + +/*! + * \ingroup bmi270_contextApiSensor + * \page bmi270_context_api_bmi270_context_sensor_enable bmi270_context_sensor_enable + * \code + * int8_t bmi270_context_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API selects the sensors/features to be enabled. + * + * @param[in] sens_list : Pointer to select the sensor/feature. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features that can be enabled. + * + *@verbatim + * sens_list | Values + * ----------------------------|----------- + * BMI2_ACCEL | 0 + * BMI2_GYRO | 1 + * BMI2_AUX | 2 + * BMI2_STEP_DETECTOR | 6 + * BMI2_STEP_COUNTER | 7 + * BMI2_GYRO_GAIN_UPDATE | 9 + * BMI2_ACTIVITY_RECOGNITION | 34 + *@endverbatim + * + * @note : + * example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO}; + * uint8_t n_sens = 2; + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_context_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + +/*! + * \ingroup bmi270_contextApiSensor + * \page bmi270_context_api_bmi270_context_sensor_disable bmi270_context_sensor_disable + * \code + * int8_t bmi270_context_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API selects the sensors/features to be disabled. + * + * @param[in] sens_list : Pointer to select the sensor/feature. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features that can be disabled. + * + *@verbatim + * sens_list | Values + * ----------------------------|----------- + * BMI2_ACCEL | 0 + * BMI2_GYRO | 1 + * BMI2_AUX | 2 + * BMI2_STEP_DETECTOR | 6 + * BMI2_STEP_COUNTER | 7 + * BMI2_GYRO_GAIN_UPDATE | 9 + * BMI2_ACTIVITY_RECOGNITION | 34 + *@endverbatim + * + * @note : + * example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO}; + * uint8_t n_sens = 2; + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_context_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + +/** + * \ingroup bmi270_context + * \defgroup bmi270_contextApiSensorC Sensor Configuration + * @brief Enable / Disable feature configuration of the sensor + */ + +/*! + * \ingroup bmi270_contextApiSensorC + * \page bmi270_context_api_bmi270_context_set_sensor_config bmi270_context_set_sensor_config + * \code + * int8_t bmi270_context_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API sets the sensor/feature configuration. + * + * @param[in] sens_cfg : Structure instance of bmi2_sens_config. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features that can be configured + * + *@verbatim + * sens_list | Values + * ----------------------------|----------- + * BMI2_STEP_DETECTOR | 6 + * BMI2_STEP_COUNTER | 7 + * BMI2_STEP_COUNTER_PARAMS | 28 + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_context_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + +/*! + * \ingroup bmi270_contextApiSensorC + * \page bmi270_context_api_bmi270_context_get_sensor_config bmi270_context_get_sensor_config + * \code + * int8_t bmi270_context_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API gets the sensor/feature configuration. + * + * @param[in] sens_cfg : Structure instance of bmi2_sens_config. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features whose configurations can be read. + * + *@verbatim + * sens_list | Values + * ----------------------------|----------- + * BMI2_STEP_DETECTOR | 6 + * BMI2_STEP_COUNTER | 7 + * BMI2_STEP_COUNTER_PARAMS | 28 + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_context_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + +/** + * \ingroup bmi270_context + * \defgroup bmi270_contextApiSensorD Sensor Data + * @brief Get sensor data + */ + +/*! + * \ingroup bmi270_contextApiSensorD + * \page bmi270_context_api_bmi270_context_get_sensor_data bmi270_context_get_sensor_data + * \code + * int8_t bmi270_context_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API gets the sensor/feature data for accelerometer, gyroscope, + * auxiliary sensor, step counter, high-g, gyroscope user-gain update, + * orientation, gyroscope cross sensitivity and error status for NVM and VFRM. + * + * @param[out] sensor_data : Structure instance of bmi2_sensor_data. + * @param[in] n_sens : Number of sensors selected. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features whose data can be read + * + *@verbatim + * sens_list | Values + * ---------------------|----------- + * BMI2_STEP_COUNTER | 7 + * BMI2_NVM_STATUS | 38 + * BMI2_VFRM_STATUS | 39 + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_context_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev); + +/** + * \ingroup bmi270_context + * \defgroup bmi270_contextApiARecog Activity recognition settings + * @brief Set / Get activity recognition settings of the sensor + */ + +/*! + * \ingroup bmi270_contextApiARecog + * \page bmi270_context_api_bmi270_context_get_act_recg_sett bmi270_context_get_act_recg_sett + * \code + * int8_t bmi270_context_get_act_recg_sett(struct bmi2_act_recg_sett *sett, struct bmi2_dev *dev); + * \endcode + * @details This api is used for retrieving the following activity recognition settings currently set. + * enable/disable post processing(0/1) by default -> 1(enable), + * Setting the min & max Gini's diversity index (GDI) threshold. min_GDI_tres(0-0XFFFF) by default ->(0x06e1) + * max_GDI_tres(0-0xFFFF) by default ->(0x0A66) + * buffer size for post processing. range (1-0x0A) default -> (0x0A) + * min segment confidence. range (1-0x0A) default -> (0x0A) + * + * @param[in] sett : Structure instance of bmi2_act_recg_sett. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_context_get_act_recg_sett(struct bmi2_act_recg_sett *sett, struct bmi2_dev *dev); + +/*! + * \ingroup bmi270_contextApiARecog + * \page bmi270_context_api_bmi270_context_set_act_recg_sett bmi270_context_set_act_recg_sett + * \code + * int8_t bmi270_context_set_act_recg_sett(const struct bmi2_act_recg_sett *sett, struct bmi2_dev *dev); + * \endcode + * @details This api is used for setting the following activity recognition settings + * enable/disable post processing(0/1) by default -> 1(enable), + * Setting the min & max Gini's diversity index (GDI) threshold. min_GDI_tres(0-0XFFFF) by default ->(0x06e1) + * max_GDI_tres(0-0xFFFF) by default ->(0x0A66) + * buffer size for post processing. range (1-0x0A) default -> (0x0A) + * min segment confidence. range (1-0x0A) default -> (0x0A) + * + * @param[in] sett : Structure instance of bmi2_act_recg_sett. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_context_set_act_recg_sett(const struct bmi2_act_recg_sett *sett, struct bmi2_dev *dev); + +/** + * \ingroup bmi270_contex + * \defgroup bmi270_contextApiactOut Activity Output + * @brief Activity output operations of the sensor + */ + +/*! + * \ingroup bmi270_contextApiactOut + * \page bmi270_context_api_bmi270_context_get_act_recog_output bmi270_context_get_act_recog_output + * \code + * int8_t bmi270_context_get_act_recog_output(struct bmi2_act_recog_output *act_recog, + * uint16_t *act_frm_len, + * struct bmi2_fifo_frame *fifo, + * const struct bmi2_dev *dev); + * + * \endcode + * @details This internal API is used to parse the activity output from the + * FIFO in header mode. + * + * @param[out] act_recog : Pointer to buffer where the parsed activity data + * bytes are stored. + * @param[in] act_frm_len : Number of activity frames parsed. + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @verbatim + * ---------------------------------------------------------------------------- + * bmi2_act_rec_output | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * time_stamp | time-stamp (expressed in 50Hz ticks) + * -------------------------|--------------------------------------------------- + * type | Type of activity + * -------------------------|--------------------------------------------------- + * stat | Activity status + * -------------------------|--------------------------------------------------- + * @endverbatim + * + *@verbatim + * type | Activities + *----------|--------------------- + * 0 | UNKNOWN + * 1 | STILL + * 2 | WALK + * 3 | RUN + * 4 | BIKE + * 5 | VEHICLE + * 6 | TILTED + *@endverbatim + * + * + *@verbatim + * stat | Activity status + *----------|--------------------- + * 1 | START + * 2 | END + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_context_get_act_recog_output(struct bmi2_act_recog_output *act_recog, + uint16_t *act_frm_len, + struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev); + +/** + * \ingroup bmi270_contex + * \defgroup bmi270_contextApiGyroUG Gyro User Gain + * @brief Set / Get Gyro User Gain of the sensor + */ + +/*! + * \ingroup bmi270_contextApiGyroUG + * \page bmi270_context_api_bmi270_context_update_gyro_user_gain bmi270_context_update_gyro_user_gain + * \code + * int8_t bmi270_context_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev); + * \endcode + * @details This API updates the gyroscope user-gain. + * + * @param[in] user_gain : Structure that stores user-gain configurations. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_context_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev); + +/*! + * \ingroup bmi270_contextApiGyroUG + * \page bmi270_context_api_bmi270_context_read_gyro_user_gain bmi270_context_read_gyro_user_gain + * \code + * int8_t bmi270_context_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, const struct bmi2_dev *dev); + * \endcode + * @details This API reads the compensated gyroscope user-gain values. + * + * @param[out] gyr_usr_gain : Structure that stores gain values. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_context_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, struct bmi2_dev *dev); + +/*! + * \ingroup bmi270_contextApiInt + * \page bmi270_context_api_bmi270_context_map_feat_int bmi270_context_map_feat_int + * \code + * int8_t bmi270_context_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev) + * \endcode + * @details This API maps/unmaps feature interrupts to that of interrupt pins. + * + * @param[in] sens_int : Structure instance of bmi2_sens_int_config. + * @param[in] n_sens : Number of interrupts to be mapped. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_context_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev); + +/******************************************************************************/ +/*! @name C++ Guard Macros */ +/******************************************************************************/ +#ifdef __cplusplus +} +#endif /* End of CPP guard */ + +#endif /* BMI270_CONTEXT_H_ */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_hc.c b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_hc.c new file mode 100644 index 000000000..a7c9172dd --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_hc.c @@ -0,0 +1,3165 @@ +/** +* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. +* +* BSD-3-Clause +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* 3. Neither the name of the copyright holder nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING +* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* @file bmi270_hc.c +* @date 2020-11-04 +* @version v2.63.1 +* +*/ + +/***************************************************************************/ + +/*! Header files + ****************************************************************************/ +#include "bmi270_hc.h" + +/***************************************************************************/ + +/*! Global Variable + ****************************************************************************/ + +/*! @name Global array that stores the configuration file of BMI270_huawei_context */ +const uint8_t bmi270_hc_config_file[] = { + 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x00, 0xb0, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0xa8, + 0x03, 0x80, 0x2e, 0x91, 0x03, 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x7d, 0xb0, 0x50, 0x30, 0x21, 0x2e, 0x59, 0xf5, + 0x10, 0x30, 0x21, 0x2e, 0x6a, 0xf5, 0x80, 0x2e, 0xae, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x39, 0x01, 0x00, 0x22, + 0x00, 0x76, 0x00, 0x00, 0x10, 0x00, 0x10, 0xd1, 0x00, 0x72, 0xb3, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0xfd, 0x2d, 0xd0, 0x86, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x00, 0x08, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x01, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1a, 0x24, 0x22, 0x00, 0x80, 0x2e, 0x48, 0x02, 0x01, 0x2e, 0x49, 0xf1, 0x0b, + 0xbc, 0x10, 0x50, 0x0f, 0xb8, 0x00, 0x90, 0xfb, 0x7f, 0x07, 0x2f, 0x03, 0x2e, 0x21, 0xf2, 0x02, 0x31, 0x4a, 0x0a, + 0x23, 0x2e, 0x21, 0xf2, 0x09, 0x2c, 0x00, 0x30, 0x98, 0x2e, 0x0e, 0xc7, 0x03, 0x2e, 0x21, 0xf2, 0xf2, 0x3e, 0x4a, + 0x08, 0x23, 0x2e, 0x21, 0xf2, 0xfb, 0x6f, 0xf0, 0x5f, 0xb8, 0x2e, 0x13, 0x52, 0x00, 0x2e, 0x60, 0x40, 0x41, 0x40, + 0x0d, 0xbc, 0x98, 0xbc, 0xc0, 0x2e, 0x01, 0x0a, 0x0f, 0xb8, 0x43, 0x86, 0x25, 0x40, 0x04, 0x40, 0xd8, 0xbe, 0x2c, + 0x0b, 0x22, 0x11, 0x54, 0x42, 0x03, 0x80, 0x4b, 0x0e, 0xf6, 0x2f, 0xb8, 0x2e, 0x15, 0x50, 0x10, 0x50, 0x17, 0x52, + 0x05, 0x2e, 0x7e, 0x00, 0xfb, 0x7f, 0x00, 0x2e, 0x13, 0x40, 0x93, 0x42, 0x41, 0x0e, 0xfb, 0x2f, 0x98, 0x2e, 0xe0, + 0x03, 0x98, 0x2e, 0x87, 0xcf, 0x01, 0x2e, 0x8b, 0x00, 0x00, 0xb2, 0x08, 0x2f, 0x01, 0x2e, 0x69, 0xf7, 0xb1, 0x3f, + 0x01, 0x08, 0x01, 0x30, 0x23, 0x2e, 0x8b, 0x00, 0x21, 0x2e, 0x69, 0xf7, 0xfb, 0x6f, 0xf0, 0x5f, 0xb8, 0x2e, 0x98, + 0x00, 0xff, 0x3f, 0x00, 0x0c, 0xff, 0x0f, 0x00, 0x04, 0xc0, 0x00, 0x5b, 0xf5, 0x8f, 0x00, 0x1e, 0xf2, 0xfd, 0xf5, + 0x8d, 0x00, 0x95, 0x00, 0x95, 0x00, 0xe0, 0x00, 0x19, 0xf4, 0x66, 0xf5, 0x00, 0x18, 0xff, 0x00, 0x64, 0xf5, 0x9c, + 0x00, 0x81, 0x00, 0x83, 0x00, 0x7f, 0x00, 0xff, 0xfb, 0x21, 0x02, 0x00, 0x10, 0x00, 0x40, 0x3a, 0x0f, 0xeb, 0x00, + 0x7f, 0xff, 0xc2, 0xf5, 0x68, 0xf7, 0x34, 0x0f, 0x28, 0x0f, 0x2e, 0x0f, 0x80, 0x00, 0x4d, 0x0f, 0x58, 0xf7, 0x5b, + 0xf7, 0x50, 0x0f, 0x00, 0x80, 0xff, 0x7f, 0x86, 0x00, 0x3f, 0x0f, 0x52, 0x0f, 0xb3, 0xf1, 0x4c, 0x0f, 0x6c, 0xf7, + 0xb9, 0xf1, 0xc6, 0xf1, 0x00, 0xe0, 0x00, 0xff, 0xd1, 0xf5, 0x54, 0x0f, 0x57, 0x0f, 0xff, 0x03, 0x00, 0xfc, 0xf0, + 0x3f, 0xb9, 0x00, 0x2d, 0xf5, 0xca, 0xf5, 0x6d, 0x03, 0xf0, 0x07, 0xc3, 0x11, 0x1d, 0x08, 0x12, 0x08, 0xb7, 0x15, + 0x9c, 0x01, 0xed, 0x14, 0xc2, 0x53, 0xca, 0x10, 0xa2, 0x00, 0x74, 0x01, 0xba, 0x17, 0xb4, 0x56, 0xe8, 0x16, 0x9f, + 0x00, 0xd7, 0x13, 0x2f, 0x52, 0x8a, 0x51, 0x64, 0x01, 0xc6, 0x53, 0xf4, 0x0b, 0x06, 0x08, 0xfc, 0x57, 0xe3, 0x44, + 0xad, 0x55, 0x4c, 0x01, 0x62, 0x01, 0x0b, 0x08, 0xa0, 0x18, 0x39, 0x59, 0x56, 0x2b, 0x14, 0x01, 0x2a, 0x58, 0x5e, + 0x0c, 0xc1, 0x47, 0x16, 0x5a, 0xd2, 0x07, 0x2b, 0x21, 0x7f, 0x53, 0x03, 0x08, 0xca, 0x59, 0x1a, 0x0b, 0x5f, 0x57, + 0x36, 0x47, 0xa6, 0x17, 0x02, 0x01, 0x33, 0x49, 0xdd, 0x58, 0x71, 0x0c, 0x32, 0x08, 0xc6, 0x59, 0xd4, 0x42, 0x65, + 0x54, 0x15, 0x59, 0x64, 0x08, 0x0c, 0x08, 0x8c, 0x01, 0x98, 0x00, 0x0f, 0x48, 0x02, 0x58, 0x96, 0x0c, 0x1c, 0x43, + 0x77, 0x48, 0xe9, 0x00, 0x1e, 0x0c, 0xf5, 0x41, 0xf6, 0x46, 0x64, 0x51, 0x98, 0x41, 0xcc, 0x01, 0x66, 0x58, 0x70, + 0x45, 0x28, 0x21, 0xe7, 0x59, 0x22, 0x30, 0x00, 0x08, 0x71, 0x7d, 0x49, 0x01, 0x92, 0x02, 0xf5, 0xd6, 0xe8, 0x63, + 0x7a, 0xfa, 0x87, 0x05, 0x0f, 0xcb, 0xa3, 0x72, 0x37, 0xfc, 0xca, 0x03, 0x95, 0xc7, 0x2e, 0x6d, 0x39, 0xc4, 0xc8, + 0x3b, 0x91, 0x37, 0x29, 0x02, 0x9e, 0x01, 0x00, 0xf0, 0x33, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x2e, 0xd5, 0xb5, 0x80, 0x2e, 0x00, 0xc1, 0xfd, 0x2d, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x2d, 0x01, 0xd4, 0x7b, 0x3b, + 0x01, 0xdb, 0x7a, 0x04, 0x00, 0x3f, 0x7b, 0xcd, 0x6c, 0xc3, 0x04, 0x85, 0x09, 0xc3, 0x04, 0xec, 0xe6, 0x0c, 0x46, + 0x01, 0x00, 0x27, 0x00, 0x19, 0x00, 0x96, 0x00, 0xa0, 0x00, 0x01, 0x00, 0x0c, 0x00, 0xf0, 0x3c, 0x00, 0x01, 0x01, + 0x00, 0x03, 0x00, 0x01, 0x00, 0x0e, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x39, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0xe1, 0x06, 0x66, 0x0a, 0x0a, 0x00, 0x0a, 0x00, 0x5a, 0x8d, 0x33, + 0x73, 0xcd, 0x8c, 0x9a, 0x99, 0x71, 0x7d, 0xcd, 0x8c, 0xcd, 0x6c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x50, 0x98, 0x2e, 0xbd, 0x0e, 0x50, 0x32, 0x98, 0x2e, + 0x48, 0x03, 0x10, 0x30, 0x21, 0x2e, 0x21, 0xf2, 0x00, 0x30, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x00, 0x2e, 0x01, + 0x80, 0x06, 0xa2, 0xfb, 0x2f, 0x01, 0x2e, 0x9b, 0x00, 0x00, 0xb2, 0x10, 0x2f, 0x01, 0x2e, 0x18, 0x00, 0x00, 0xb2, + 0x0c, 0x2f, 0x01, 0x54, 0x03, 0x52, 0x01, 0x50, 0x98, 0x2e, 0xc2, 0xc0, 0x98, 0x2e, 0x0e, 0xb1, 0x01, 0x50, 0x98, + 0x2e, 0xdd, 0xb5, 0x10, 0x30, 0x21, 0x2e, 0x19, 0x00, 0x01, 0x2e, 0x86, 0x00, 0x04, 0xae, 0x0b, 0x2f, 0x01, 0x2e, + 0x9b, 0x00, 0x00, 0xb2, 0x07, 0x2f, 0x01, 0x52, 0x98, 0x2e, 0x74, 0x0e, 0x00, 0xb2, 0x02, 0x2f, 0x10, 0x30, 0x21, + 0x2e, 0x79, 0x00, 0x01, 0x2e, 0x79, 0x00, 0x00, 0x90, 0x90, 0x2e, 0x14, 0x03, 0x01, 0x2e, 0x89, 0x00, 0x00, 0xb2, + 0x04, 0x2f, 0x98, 0x2e, 0x15, 0x0e, 0x00, 0x30, 0x21, 0x2e, 0x7c, 0x00, 0x01, 0x2e, 0x7c, 0x00, 0x00, 0xb2, 0x12, + 0x2f, 0x01, 0x2e, 0x86, 0x00, 0x00, 0x90, 0x02, 0x2f, 0x98, 0x2e, 0x05, 0x0e, 0x09, 0x2d, 0x98, 0x2e, 0x5d, 0x0d, + 0x01, 0x2e, 0x86, 0x00, 0x04, 0x90, 0x02, 0x2f, 0x50, 0x32, 0x98, 0x2e, 0x48, 0x03, 0x00, 0x30, 0x21, 0x2e, 0x7c, + 0x00, 0x01, 0x2e, 0x78, 0x00, 0x00, 0xb2, 0x90, 0x2e, 0x2c, 0x03, 0x01, 0x2e, 0x78, 0x00, 0x81, 0x30, 0x01, 0x08, + 0x00, 0xb2, 0x61, 0x2f, 0x03, 0x2e, 0x24, 0x02, 0x01, 0x2e, 0x86, 0x00, 0x98, 0xbc, 0x98, 0xb8, 0x05, 0xb2, 0x0d, + 0x58, 0x23, 0x2f, 0x07, 0x90, 0x07, 0x54, 0x00, 0x30, 0x37, 0x2f, 0x15, 0x41, 0x04, 0x41, 0xdc, 0xbe, 0x44, 0xbe, + 0xdc, 0xba, 0x2c, 0x01, 0x61, 0x00, 0x0d, 0x56, 0x4a, 0x0f, 0x0c, 0x2f, 0xd1, 0x42, 0x94, 0xb8, 0xc1, 0x42, 0x11, + 0x30, 0x05, 0x2e, 0x6a, 0xf7, 0x2c, 0xbd, 0x2f, 0xb9, 0x80, 0xb2, 0x08, 0x22, 0x98, 0x2e, 0xa4, 0xb1, 0x21, 0x2d, + 0x61, 0x30, 0x23, 0x2e, 0x86, 0x00, 0x98, 0x2e, 0xa4, 0xb1, 0x00, 0x30, 0x21, 0x2e, 0x5a, 0xf5, 0x18, 0x2d, 0xf1, + 0x7f, 0x50, 0x30, 0x98, 0x2e, 0x48, 0x03, 0x0d, 0x52, 0x05, 0x50, 0x50, 0x42, 0x70, 0x30, 0x0b, 0x54, 0x42, 0x42, + 0x7e, 0x82, 0xf2, 0x6f, 0x80, 0xb2, 0x42, 0x42, 0x05, 0x2f, 0x21, 0x2e, 0x86, 0x00, 0x10, 0x30, 0x98, 0x2e, 0xa4, + 0xb1, 0x03, 0x2d, 0x60, 0x30, 0x21, 0x2e, 0x86, 0x00, 0x01, 0x2e, 0x86, 0x00, 0x06, 0x90, 0x18, 0x2f, 0x01, 0x2e, + 0x77, 0x00, 0x09, 0x54, 0x05, 0x52, 0xf0, 0x7f, 0x98, 0x2e, 0x7a, 0xc1, 0xf1, 0x6f, 0x08, 0x1a, 0x40, 0x30, 0x08, + 0x2f, 0x21, 0x2e, 0x86, 0x00, 0x20, 0x30, 0x98, 0x2e, 0xea, 0x03, 0x50, 0x32, 0x98, 0x2e, 0x48, 0x03, 0x05, 0x2d, + 0x98, 0x2e, 0x1e, 0x0e, 0x00, 0x30, 0x21, 0x2e, 0x86, 0x00, 0x00, 0x30, 0x21, 0x2e, 0x78, 0x00, 0x18, 0x2d, 0x01, + 0x2e, 0x86, 0x00, 0x03, 0xaa, 0x01, 0x2f, 0x98, 0x2e, 0x2b, 0x0e, 0x01, 0x2e, 0x86, 0x00, 0x3f, 0x80, 0x03, 0xa2, + 0x01, 0x2f, 0x00, 0x2e, 0x02, 0x2d, 0x98, 0x2e, 0x41, 0x0e, 0x30, 0x30, 0x98, 0x2e, 0xaf, 0xb1, 0x00, 0x30, 0x21, + 0x2e, 0x79, 0x00, 0x50, 0x32, 0x98, 0x2e, 0x48, 0x03, 0x01, 0x2e, 0x19, 0x00, 0x00, 0xb2, 0x10, 0x2f, 0x01, 0x2e, + 0x87, 0x00, 0x21, 0x2e, 0x8f, 0x00, 0x0f, 0x52, 0x7e, 0x82, 0x11, 0x50, 0x41, 0x40, 0x18, 0xb9, 0x11, 0x42, 0x02, + 0x42, 0x02, 0x80, 0x00, 0x2e, 0x01, 0x40, 0x01, 0x42, 0x98, 0x2e, 0xe1, 0x00, 0x00, 0x30, 0x21, 0x2e, 0x19, 0x00, + 0x21, 0x2e, 0x9b, 0x00, 0x80, 0x2e, 0x52, 0x02, 0x21, 0x2e, 0x59, 0xf5, 0x10, 0x30, 0xc0, 0x2e, 0x21, 0x2e, 0x4a, + 0xf1, 0x80, 0x2e, 0x00, 0xc1, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x9a, 0x01, + 0x34, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x20, 0x50, 0xe7, 0x7f, 0xf6, 0x7f, 0x46, 0x30, 0x0f, 0x2e, 0xa4, 0xf1, 0xbe, 0x09, 0x80, 0xb3, 0x06, + 0x2f, 0x0d, 0x2e, 0x86, 0x00, 0x84, 0xaf, 0x02, 0x2f, 0x16, 0x30, 0x2d, 0x2e, 0x7c, 0x00, 0x86, 0x30, 0x2d, 0x2e, + 0x60, 0xf5, 0xf6, 0x6f, 0xe7, 0x6f, 0xe0, 0x5f, 0xc8, 0x2e, 0xa0, 0x50, 0x80, 0x7f, 0xe7, 0x7f, 0xd5, 0x7f, 0xc4, + 0x7f, 0xb3, 0x7f, 0xa2, 0x7f, 0x91, 0x7f, 0xf6, 0x7f, 0x7b, 0x7f, 0x00, 0x2e, 0x01, 0x2e, 0x60, 0xf5, 0x60, 0x7f, + 0x98, 0x2e, 0xcd, 0x00, 0x62, 0x6f, 0x01, 0x32, 0x91, 0x08, 0x80, 0xb2, 0x0d, 0x2f, 0x00, 0xb2, 0x03, 0x2f, 0x05, + 0x2e, 0x18, 0x00, 0x80, 0x90, 0x05, 0x2f, 0x25, 0x56, 0x02, 0x30, 0xc1, 0x42, 0xc2, 0x86, 0x00, 0x2e, 0xc2, 0x42, + 0x23, 0x2e, 0x60, 0xf5, 0x00, 0x90, 0x00, 0x30, 0x06, 0x2f, 0x21, 0x2e, 0x7a, 0x00, 0x23, 0x50, 0x21, 0x2e, 0x5a, + 0xf2, 0x98, 0x2e, 0xfb, 0x01, 0xf6, 0x6f, 0x80, 0x6f, 0x91, 0x6f, 0xa2, 0x6f, 0xb3, 0x6f, 0xc4, 0x6f, 0xd5, 0x6f, + 0xe7, 0x6f, 0x7b, 0x6f, 0x60, 0x5f, 0xc8, 0x2e, 0x03, 0x2e, 0x7e, 0x00, 0x16, 0xb8, 0x02, 0x34, 0x4a, 0x0c, 0x21, + 0x2e, 0x2d, 0xf5, 0xc0, 0x2e, 0x23, 0x2e, 0x7e, 0x00, 0x03, 0xbc, 0x21, 0x2e, 0x87, 0x00, 0x03, 0x2e, 0x87, 0x00, + 0x40, 0xb2, 0x10, 0x30, 0x21, 0x2e, 0x19, 0x00, 0x01, 0x30, 0x05, 0x2f, 0x05, 0x2e, 0x8a, 0x00, 0x80, 0x90, 0x01, + 0x2f, 0x23, 0x2e, 0x6f, 0xf5, 0xc0, 0x2e, 0x21, 0x2e, 0x8b, 0x00, 0x80, 0x2e, 0x00, 0xc1, 0xc0, 0x50, 0xe7, 0x7f, + 0xf6, 0x7f, 0x26, 0x30, 0x0f, 0x2e, 0x61, 0xf5, 0x2f, 0x2e, 0x78, 0x00, 0x0f, 0x2e, 0x78, 0x00, 0xbe, 0x09, 0xa2, + 0x7f, 0x91, 0x7f, 0x80, 0x7f, 0x80, 0xb3, 0xd5, 0x7f, 0xc4, 0x7f, 0xb3, 0x7f, 0x7b, 0x7f, 0x11, 0x2f, 0x19, 0x50, + 0x1a, 0x25, 0x12, 0x40, 0x42, 0x7f, 0x74, 0x82, 0x12, 0x40, 0x52, 0x7f, 0x00, 0x2e, 0x00, 0x40, 0x60, 0x7f, 0x98, + 0x2e, 0x6a, 0xd6, 0x01, 0x2e, 0x61, 0xf7, 0x01, 0x31, 0x01, 0x0a, 0x21, 0x2e, 0x61, 0xf7, 0x80, 0x30, 0x03, 0x2e, + 0x78, 0x00, 0x08, 0x08, 0x00, 0xb2, 0x42, 0x2f, 0x03, 0x2e, 0x24, 0x02, 0x01, 0x2e, 0x24, 0x02, 0x97, 0xbc, 0x06, + 0xbc, 0x9f, 0xb8, 0x0f, 0xb8, 0x00, 0x90, 0x23, 0x2e, 0x8a, 0x00, 0x10, 0x30, 0x01, 0x30, 0x2a, 0x2f, 0x03, 0x2e, + 0x86, 0x00, 0x44, 0xb2, 0x05, 0x2f, 0x47, 0xb2, 0x00, 0x30, 0x2d, 0x2f, 0x21, 0x2e, 0x78, 0x00, 0x2b, 0x2d, 0x03, + 0x2e, 0xfd, 0xf5, 0x9e, 0xbc, 0x9f, 0xb8, 0x40, 0x90, 0x14, 0x2f, 0x03, 0x2e, 0xfc, 0xf5, 0x99, 0xbc, 0x9f, 0xb8, + 0x40, 0x90, 0x0e, 0x2f, 0x03, 0x2e, 0x49, 0xf1, 0x1b, 0x54, 0x4a, 0x08, 0x40, 0x90, 0x08, 0x2f, 0x98, 0x2e, 0xcd, + 0x00, 0x00, 0xb2, 0x10, 0x30, 0x03, 0x2f, 0x50, 0x30, 0x21, 0x2e, 0x86, 0x00, 0x10, 0x2d, 0x98, 0x2e, 0xea, 0x03, + 0x00, 0x30, 0x21, 0x2e, 0x78, 0x00, 0x0a, 0x2d, 0x05, 0x2e, 0x69, 0xf7, 0x2d, 0xbd, 0x2f, 0xb9, 0x80, 0xb2, 0x01, + 0x2f, 0x21, 0x2e, 0x79, 0x00, 0x23, 0x2e, 0x78, 0x00, 0xe0, 0x31, 0x21, 0x2e, 0x61, 0xf5, 0xf6, 0x6f, 0xe7, 0x6f, + 0x80, 0x6f, 0xa2, 0x6f, 0xb3, 0x6f, 0xc4, 0x6f, 0xd5, 0x6f, 0x7b, 0x6f, 0x91, 0x6f, 0x40, 0x5f, 0xc8, 0x2e, 0x90, + 0x50, 0xf7, 0x7f, 0xe6, 0x7f, 0xd5, 0x7f, 0xc4, 0x7f, 0xb3, 0x7f, 0xa1, 0x7f, 0x90, 0x7f, 0x82, 0x7f, 0x7b, 0x7f, + 0x98, 0x2e, 0xcd, 0x00, 0x00, 0xb2, 0x12, 0x30, 0x5c, 0x2f, 0x01, 0x2e, 0x21, 0x02, 0x03, 0x2e, 0x28, 0x02, 0x9f, + 0xbc, 0x9f, 0xb8, 0x21, 0x56, 0x8a, 0x08, 0x03, 0x08, 0x82, 0x0a, 0x25, 0x2e, 0x18, 0x00, 0x05, 0x2e, 0xc1, 0xf5, + 0x2e, 0xbc, 0x05, 0x2e, 0x86, 0x00, 0x84, 0xa2, 0x0e, 0xb8, 0x31, 0x30, 0x88, 0x04, 0x07, 0x2f, 0x01, 0x2e, 0x18, + 0x00, 0x00, 0x90, 0x03, 0x2f, 0x01, 0x2e, 0x7b, 0x00, 0x00, 0xb2, 0x19, 0x2f, 0x1d, 0x50, 0x01, 0x52, 0x98, 0x2e, + 0xd6, 0x00, 0x05, 0x2e, 0x7a, 0x00, 0x25, 0x2e, 0x9b, 0x00, 0x05, 0x2e, 0x7a, 0x00, 0x80, 0x90, 0x02, 0x2f, 0x12, + 0x30, 0x25, 0x2e, 0x7a, 0x00, 0x01, 0x2e, 0x7b, 0x00, 0x00, 0xb2, 0x05, 0x2e, 0x18, 0x00, 0x02, 0x2f, 0x10, 0x30, + 0x21, 0x2e, 0x18, 0x00, 0x25, 0x2e, 0x7b, 0x00, 0x05, 0x2e, 0x18, 0x00, 0x80, 0xb2, 0x20, 0x2f, 0x01, 0x2e, 0xc0, + 0xf5, 0xf2, 0x30, 0x02, 0x08, 0x07, 0xaa, 0x73, 0x30, 0x03, 0x2e, 0x7d, 0x00, 0x18, 0x22, 0x41, 0x1a, 0x05, 0x2f, + 0x03, 0x2e, 0x66, 0xf5, 0x9f, 0xbc, 0x9f, 0xb8, 0x40, 0x90, 0x0c, 0x2f, 0x1f, 0x52, 0x03, 0x30, 0x53, 0x42, 0x2b, + 0x30, 0x90, 0x04, 0x5b, 0x42, 0x21, 0x2e, 0x7d, 0x00, 0x24, 0xbd, 0x7e, 0x80, 0x81, 0x84, 0x43, 0x42, 0x02, 0x42, + 0x02, 0x32, 0x25, 0x2e, 0x62, 0xf5, 0x05, 0x2e, 0x88, 0x00, 0x81, 0x84, 0x25, 0x2e, 0x88, 0x00, 0x02, 0x31, 0x25, + 0x2e, 0x60, 0xf5, 0x05, 0x2e, 0x25, 0x02, 0x10, 0x30, 0x90, 0x08, 0x80, 0xb2, 0x0b, 0x2f, 0x05, 0x2e, 0xca, 0xf5, + 0xf0, 0x3e, 0x90, 0x08, 0x25, 0x2e, 0xca, 0xf5, 0x05, 0x2e, 0x59, 0xf5, 0xe0, 0x3f, 0x90, 0x08, 0x25, 0x2e, 0x59, + 0xf5, 0x90, 0x6f, 0xa1, 0x6f, 0xb3, 0x6f, 0xc4, 0x6f, 0xd5, 0x6f, 0xe6, 0x6f, 0xf7, 0x6f, 0x7b, 0x6f, 0x82, 0x6f, + 0x70, 0x5f, 0xc8, 0x2e, 0x31, 0x50, 0xe0, 0x50, 0x12, 0x40, 0x06, 0x40, 0x1a, 0x25, 0x77, 0x88, 0x75, 0x8a, 0x21, + 0x56, 0x6c, 0xbf, 0x00, 0x30, 0xd3, 0x08, 0x6c, 0xbb, 0x60, 0x7f, 0x00, 0x43, 0x40, 0x43, 0xc0, 0xb2, 0xd6, 0x7f, + 0xe5, 0x7f, 0xf4, 0x7f, 0xc3, 0x7f, 0xbb, 0x7f, 0x74, 0x2f, 0x01, 0x2e, 0x85, 0x00, 0x00, 0xb2, 0x0b, 0x2f, 0x27, + 0x52, 0x01, 0x2e, 0x80, 0x00, 0xa2, 0x7f, 0x98, 0x2e, 0xbb, 0xcc, 0x01, 0x30, 0x23, 0x2e, 0x85, 0x00, 0xa2, 0x6f, + 0xc3, 0x6f, 0x1a, 0x25, 0x26, 0xbc, 0x86, 0xba, 0x25, 0xbc, 0x0f, 0xb8, 0x54, 0xb1, 0x00, 0xb2, 0xa6, 0x7f, 0x0c, + 0x2f, 0x29, 0x50, 0x2b, 0x54, 0x0b, 0x30, 0x0b, 0x2e, 0x21, 0x02, 0x2f, 0x58, 0x1b, 0x42, 0x9b, 0x42, 0x6c, 0x09, + 0x0b, 0x42, 0x2b, 0x2e, 0x21, 0x02, 0x8b, 0x42, 0x72, 0x84, 0x33, 0x50, 0x58, 0x09, 0x03, 0x52, 0x01, 0x50, 0x92, + 0x7f, 0x85, 0x7f, 0x98, 0x2e, 0xc2, 0xc0, 0x01, 0x2e, 0x80, 0x00, 0xf5, 0x6f, 0xe4, 0x6f, 0x83, 0x6f, 0x92, 0x6f, + 0x27, 0x52, 0x2d, 0x5c, 0x98, 0x2e, 0x06, 0xcd, 0xc3, 0x6f, 0x29, 0x50, 0x72, 0x6f, 0xb4, 0xbc, 0x14, 0x40, 0x80, + 0xb2, 0x9f, 0xba, 0x02, 0x40, 0x01, 0x30, 0xf0, 0x7f, 0x05, 0x2f, 0x40, 0xb3, 0x03, 0x2f, 0x2b, 0x5c, 0x11, 0x30, + 0x94, 0x43, 0x82, 0x43, 0xb3, 0xbd, 0xbf, 0xb9, 0xc0, 0xb2, 0x1c, 0x2f, 0x53, 0x6f, 0x23, 0x01, 0x63, 0x6f, 0x93, + 0x02, 0x02, 0x42, 0x40, 0x91, 0x29, 0x2e, 0x81, 0x00, 0x2b, 0x50, 0x12, 0x2f, 0x2b, 0x56, 0x00, 0x2e, 0xd5, 0x40, + 0xc3, 0x40, 0x65, 0x05, 0xd3, 0x06, 0xc0, 0xaa, 0x04, 0x2f, 0xc0, 0x90, 0x08, 0x2f, 0xa3, 0x6f, 0x5d, 0x0f, 0x05, + 0x2f, 0xa5, 0x6f, 0x40, 0xb3, 0x02, 0x2f, 0x14, 0x42, 0x02, 0x42, 0x11, 0x30, 0xd0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, + 0xf2, 0x6f, 0x15, 0x52, 0x01, 0x2e, 0x81, 0x00, 0x82, 0x40, 0x50, 0x42, 0x08, 0x2c, 0x42, 0x42, 0x11, 0x30, 0x23, + 0x2e, 0x85, 0x00, 0x01, 0x30, 0xd0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, 0x00, 0x2e, 0xbb, 0x6f, 0x20, 0x5f, 0xb8, 0x2e, + 0x11, 0x30, 0x81, 0x08, 0x01, 0x2e, 0x6a, 0xf7, 0x71, 0x3f, 0x23, 0xbd, 0x01, 0x08, 0x02, 0x0a, 0xc0, 0x2e, 0x21, + 0x2e, 0x6a, 0xf7, 0x30, 0x25, 0x00, 0x30, 0x21, 0x2e, 0x5a, 0xf5, 0x10, 0x50, 0x21, 0x2e, 0x7c, 0x00, 0x21, 0x2e, + 0x78, 0x00, 0xfb, 0x7f, 0x98, 0x2e, 0xa4, 0xb1, 0x40, 0x30, 0x21, 0x2e, 0x86, 0x00, 0xfb, 0x6f, 0xf0, 0x5f, 0x03, + 0x25, 0x80, 0x2e, 0xea, 0x03, 0x51, 0x56, 0x05, 0x40, 0x69, 0x18, 0x0d, 0x17, 0xe1, 0x18, 0x19, 0x05, 0x16, 0x25, + 0x37, 0x25, 0x4a, 0x17, 0x54, 0x18, 0xec, 0x18, 0x04, 0x30, 0x64, 0x07, 0xea, 0x18, 0x8e, 0x00, 0x5f, 0x02, 0x35, + 0x56, 0x93, 0x00, 0x4c, 0x02, 0x2f, 0xb9, 0x91, 0xbc, 0x91, 0x0a, 0x02, 0x42, 0xb8, 0x2e, 0x0b, 0x00, 0x40, 0xb3, + 0x94, 0x02, 0x0b, 0x2f, 0x50, 0xa1, 0x03, 0x2f, 0x70, 0x8b, 0x85, 0x14, 0x07, 0x2c, 0x00, 0x30, 0x04, 0x31, 0x25, + 0x05, 0x04, 0x13, 0x95, 0x14, 0x05, 0x14, 0x94, 0x0a, 0x47, 0x5a, 0x45, 0x01, 0x04, 0x30, 0x94, 0x02, 0xd8, 0xba, + 0xc0, 0x2e, 0x28, 0xbd, 0x2a, 0x0a, 0x4a, 0x18, 0x00, 0x30, 0xc1, 0x18, 0x6f, 0xb8, 0xc0, 0x2e, 0xf1, 0xbc, 0x01, + 0x0a, 0x4e, 0x86, 0x80, 0x40, 0x70, 0x50, 0xd1, 0x40, 0x86, 0x84, 0xf0, 0x7f, 0x84, 0x30, 0xce, 0x8a, 0xe2, 0x7f, + 0x20, 0x04, 0x54, 0x41, 0xc2, 0x40, 0x4c, 0x04, 0x43, 0x41, 0x93, 0x06, 0x49, 0x18, 0x03, 0x31, 0xd8, 0x04, 0x30, + 0x88, 0xd1, 0x18, 0xd4, 0x7f, 0x00, 0xb2, 0x72, 0x8b, 0xd1, 0x18, 0xc3, 0x7f, 0xbb, 0x7f, 0x0a, 0x2f, 0x10, 0xa0, + 0x03, 0x2f, 0xd1, 0x6f, 0xb9, 0x13, 0x06, 0x2c, 0x07, 0x30, 0xc1, 0x6f, 0x79, 0x14, 0xb0, 0x12, 0xf8, 0x13, 0x91, + 0x0b, 0x51, 0x41, 0x4e, 0x85, 0x00, 0xb2, 0x94, 0x40, 0xb2, 0x86, 0x82, 0x40, 0x26, 0x01, 0xa3, 0x7f, 0x97, 0x02, + 0x43, 0x41, 0x4c, 0x00, 0x9a, 0x02, 0x0a, 0x2f, 0x10, 0xa0, 0x03, 0x2f, 0xd1, 0x6f, 0x51, 0x12, 0x06, 0x2c, 0x02, + 0x30, 0xc3, 0x6f, 0xd3, 0x14, 0x48, 0x12, 0x90, 0x12, 0x4b, 0x0a, 0x98, 0x2e, 0x79, 0xc0, 0xa1, 0x6f, 0x4f, 0x8a, + 0x72, 0x8d, 0x43, 0x41, 0x42, 0x40, 0xf5, 0x6f, 0xab, 0xbc, 0x35, 0xba, 0x25, 0xb9, 0xbb, 0xbd, 0xf0, 0x7f, 0x75, + 0x25, 0x98, 0x2e, 0xdb, 0xb1, 0xd0, 0x7f, 0x57, 0x25, 0x91, 0x41, 0x8e, 0x81, 0x82, 0x41, 0x13, 0x40, 0x04, 0x40, + 0x32, 0x8c, 0x98, 0x2e, 0xdb, 0xb1, 0xc0, 0x7f, 0x57, 0x25, 0x91, 0x41, 0x8e, 0x81, 0x82, 0x41, 0x13, 0x40, 0x04, + 0x40, 0x32, 0x8c, 0x98, 0x2e, 0xdb, 0xb1, 0xa0, 0x7f, 0x57, 0x25, 0x91, 0x41, 0x8e, 0x81, 0x82, 0x41, 0x13, 0x40, + 0x04, 0x40, 0x32, 0x8c, 0x98, 0x2e, 0xdb, 0xb1, 0x90, 0x7f, 0x57, 0x25, 0x91, 0x41, 0x8e, 0x81, 0x82, 0x41, 0x13, + 0x40, 0x04, 0x40, 0x32, 0x8c, 0x98, 0x2e, 0xdb, 0xb1, 0x57, 0x25, 0x91, 0x41, 0x8e, 0x89, 0xe7, 0x6f, 0x13, 0x41, + 0x82, 0x41, 0x04, 0x41, 0xe0, 0x7f, 0x98, 0x2e, 0xdb, 0xb1, 0x57, 0x25, 0xf1, 0x6f, 0x52, 0x41, 0xf0, 0x7f, 0x98, + 0x2e, 0xf3, 0xb1, 0xd1, 0x6f, 0x52, 0x41, 0x30, 0x25, 0x98, 0x2e, 0xf3, 0xb1, 0xc1, 0x6f, 0x52, 0x41, 0xd0, 0x7f, + 0x98, 0x2e, 0xf3, 0xb1, 0xa1, 0x6f, 0x52, 0x41, 0xc0, 0x7f, 0x98, 0x2e, 0xf3, 0xb1, 0x91, 0x6f, 0x52, 0x41, 0xa0, + 0x7f, 0x98, 0x2e, 0xf3, 0xb1, 0xe1, 0x6f, 0x52, 0x41, 0x40, 0x25, 0x98, 0x2e, 0xf3, 0xb1, 0x70, 0x25, 0x42, 0x41, + 0xf1, 0x6f, 0x57, 0x25, 0x98, 0x2e, 0xf3, 0xb1, 0x7b, 0x52, 0xd9, 0x0f, 0x90, 0x2e, 0x70, 0xb3, 0x81, 0x32, 0x69, + 0x0e, 0x06, 0x2f, 0x01, 0x32, 0x41, 0x0e, 0x0d, 0x53, 0x0f, 0x55, 0x0a, 0x22, 0x80, 0x2e, 0x71, 0xb3, 0x51, 0x34, + 0x59, 0x0e, 0xd1, 0x6f, 0xc2, 0x6f, 0x90, 0x2e, 0x4f, 0xb3, 0x99, 0x5e, 0x57, 0x0e, 0x7c, 0x2f, 0xaf, 0x5e, 0x4f, + 0x0e, 0x4d, 0x2f, 0x02, 0xa2, 0x2f, 0x2f, 0xe9, 0x50, 0x60, 0x0e, 0x06, 0x2f, 0x07, 0x55, 0x4a, 0x0e, 0x09, 0x53, + 0x0b, 0x55, 0x0a, 0x22, 0x80, 0x2e, 0x71, 0xb3, 0xeb, 0x50, 0x60, 0x0e, 0x10, 0x2f, 0xfb, 0x52, 0xd1, 0x0f, 0x0a, + 0x2f, 0x45, 0xa3, 0x01, 0x53, 0x03, 0x55, 0x4a, 0x22, 0xa2, 0x6f, 0xfd, 0x50, 0x50, 0x0e, 0xff, 0x54, 0x11, 0x22, + 0x80, 0x2e, 0x71, 0xb3, 0x05, 0x51, 0x80, 0x2e, 0x71, 0xb3, 0xed, 0x54, 0x4a, 0x0e, 0x07, 0x2f, 0xa1, 0x6f, 0xf5, + 0x54, 0x4a, 0x0e, 0xf7, 0x52, 0xf9, 0x54, 0x0a, 0x22, 0x80, 0x2e, 0x71, 0xb3, 0xef, 0x52, 0x59, 0x0e, 0xf1, 0x52, + 0xf3, 0x54, 0x0a, 0x22, 0x80, 0x2e, 0x71, 0xb3, 0xcb, 0x52, 0x61, 0x0e, 0x0e, 0x2f, 0xa1, 0x6f, 0xdd, 0x54, 0xca, + 0x0f, 0x08, 0x2f, 0x01, 0xa2, 0xe1, 0x52, 0xe3, 0x54, 0x4a, 0x22, 0xdf, 0x54, 0x62, 0x0e, 0xe5, 0x54, 0x77, 0x2c, + 0x0a, 0x22, 0x75, 0x2c, 0xe7, 0x50, 0xd7, 0x52, 0x51, 0x0e, 0xd9, 0x52, 0xdb, 0x54, 0x4a, 0x22, 0x72, 0x35, 0x5a, + 0x0e, 0xd5, 0x54, 0x6b, 0x2c, 0x11, 0x22, 0xb1, 0x56, 0x53, 0x0e, 0x0a, 0x2f, 0xa1, 0x6f, 0xcf, 0x54, 0x4a, 0x0e, + 0xd1, 0x52, 0xd3, 0x54, 0x4a, 0x22, 0xcb, 0x54, 0x62, 0x0e, 0xcd, 0x54, 0x5d, 0x2c, 0x11, 0x22, 0x0c, 0xa2, 0x05, + 0x2f, 0xc5, 0x52, 0x61, 0x0e, 0xc7, 0x52, 0xc9, 0x54, 0x55, 0x2c, 0x0a, 0x22, 0xb3, 0x54, 0x62, 0x0e, 0x0a, 0x2f, + 0xa2, 0x6f, 0xbf, 0x50, 0x50, 0x0e, 0xc1, 0x54, 0xc3, 0x50, 0x90, 0x22, 0xbb, 0x50, 0x48, 0x0e, 0xbd, 0x52, 0x47, + 0x2c, 0x0a, 0x22, 0x43, 0xa3, 0xb5, 0x52, 0xb7, 0x54, 0x4a, 0x22, 0x54, 0xa3, 0xb9, 0x54, 0x3f, 0x2c, 0x0a, 0x22, + 0x41, 0xa3, 0x14, 0x2f, 0xa0, 0x37, 0x50, 0x0e, 0x0f, 0x2f, 0xa1, 0x54, 0x4a, 0x0e, 0x0a, 0x2f, 0xa7, 0x52, 0x61, + 0x0e, 0xa9, 0x52, 0xab, 0x54, 0x4a, 0x22, 0xa2, 0x6f, 0xa5, 0x50, 0x50, 0x0e, 0xad, 0x54, 0x2c, 0x2c, 0x0a, 0x22, + 0x2a, 0x2c, 0xa3, 0x50, 0x28, 0x2c, 0x9f, 0x50, 0x01, 0xa2, 0x9b, 0x52, 0x9d, 0x54, 0x23, 0x2c, 0x0a, 0x22, 0x20, + 0x33, 0x58, 0x0e, 0x09, 0x2f, 0x91, 0x50, 0x48, 0x0e, 0x93, 0x52, 0x95, 0x50, 0x48, 0x22, 0x8f, 0x50, 0x50, 0x0e, + 0x97, 0x54, 0x16, 0x2c, 0x0a, 0x22, 0x7d, 0x54, 0x62, 0x0e, 0x0e, 0x2f, 0x81, 0x54, 0xe2, 0x0f, 0x09, 0x2f, 0x87, + 0x54, 0x4a, 0x0e, 0x89, 0x52, 0x8b, 0x54, 0x4a, 0x22, 0x83, 0x54, 0x62, 0x0e, 0x85, 0x54, 0x06, 0x2c, 0x11, 0x22, + 0x04, 0x2c, 0x8d, 0x50, 0x02, 0x2c, 0x7f, 0x50, 0x11, 0x51, 0xbb, 0x6f, 0x90, 0x5f, 0xb8, 0x2e, 0x32, 0x25, 0xb0, + 0x51, 0xc2, 0x32, 0x82, 0x00, 0xe0, 0x7f, 0xd2, 0x7f, 0x04, 0x30, 0x80, 0x40, 0x01, 0x80, 0x90, 0x42, 0xc2, 0x7f, + 0x10, 0x30, 0x85, 0x40, 0x2c, 0x03, 0x94, 0x42, 0x4a, 0x25, 0x85, 0x40, 0x28, 0x28, 0x80, 0x42, 0x25, 0x3d, 0xc3, + 0x80, 0x2b, 0x89, 0x95, 0x00, 0x81, 0x7f, 0xb2, 0x7f, 0xa4, 0x7f, 0x90, 0x7f, 0x7b, 0x7f, 0x00, 0x2e, 0xd1, 0x40, + 0x03, 0x54, 0x53, 0x7f, 0x64, 0x7f, 0x98, 0x2e, 0xd9, 0xc0, 0x64, 0x6f, 0x53, 0x6f, 0x91, 0x6f, 0x10, 0x43, 0x59, + 0x0e, 0xf3, 0x2f, 0xa1, 0x6f, 0x98, 0x2e, 0xb3, 0xc0, 0xb1, 0x6f, 0x13, 0x33, 0x40, 0x42, 0x00, 0xac, 0xcb, 0x00, + 0x02, 0x2f, 0xe1, 0x6f, 0x53, 0x54, 0x42, 0x42, 0xd1, 0x3d, 0x59, 0x00, 0xc3, 0x40, 0xcf, 0xb0, 0xce, 0x00, 0x72, + 0x80, 0xc2, 0x40, 0x11, 0x40, 0x91, 0x00, 0xd2, 0x42, 0x89, 0x16, 0xc4, 0x40, 0x22, 0x03, 0x02, 0x40, 0x05, 0x33, + 0x05, 0x00, 0xd4, 0x42, 0xb3, 0x7f, 0x90, 0x7f, 0x98, 0x2e, 0xfe, 0xc9, 0x91, 0x6f, 0xd2, 0x3d, 0xb3, 0x6f, 0x00, + 0x18, 0x8a, 0x00, 0xc0, 0x40, 0xb2, 0x88, 0x06, 0x00, 0xd0, 0x42, 0xa0, 0x35, 0xc5, 0x40, 0x6f, 0x03, 0x46, 0x40, + 0x8f, 0xb1, 0x96, 0x00, 0x20, 0x00, 0xc5, 0x42, 0x92, 0x7f, 0xb0, 0x7f, 0x02, 0x32, 0x01, 0x41, 0x33, 0x30, 0x98, + 0x2e, 0x0f, 0xca, 0xb5, 0x6f, 0x11, 0x3b, 0x63, 0x41, 0x64, 0x41, 0x44, 0x85, 0x45, 0x41, 0xa6, 0x40, 0x87, 0x40, + 0x51, 0x00, 0xf7, 0x7f, 0xb1, 0x7f, 0x20, 0x25, 0x98, 0x2e, 0x67, 0xcc, 0xb1, 0x6f, 0xb3, 0x33, 0xcb, 0x00, 0xea, + 0x82, 0xc2, 0x40, 0x1a, 0xa4, 0xe5, 0x6f, 0x04, 0x30, 0x13, 0x30, 0x01, 0x2f, 0x80, 0xa0, 0x03, 0x2f, 0x2e, 0xac, + 0x0a, 0x2f, 0x80, 0xa4, 0x08, 0x2f, 0x90, 0x6f, 0x04, 0x80, 0x77, 0x34, 0x06, 0x40, 0x6f, 0x01, 0xa2, 0x04, 0xf3, + 0x28, 0x42, 0x43, 0x03, 0x42, 0xd3, 0x3d, 0x42, 0x40, 0xcb, 0x00, 0xf2, 0x82, 0x8f, 0xb0, 0xde, 0x00, 0x43, 0x80, + 0x42, 0x40, 0xb3, 0x7f, 0x90, 0x7f, 0xb3, 0x30, 0x13, 0x53, 0x98, 0x2e, 0x5a, 0xca, 0x3a, 0x25, 0xe8, 0x82, 0xee, + 0x86, 0xe2, 0x6f, 0x82, 0x84, 0x43, 0x7f, 0x20, 0x7f, 0x51, 0x7f, 0x61, 0x7f, 0x32, 0x7f, 0x05, 0x30, 0xa4, 0x6f, + 0x83, 0x30, 0x00, 0x30, 0x11, 0x41, 0x22, 0x6f, 0x14, 0x7f, 0x00, 0x7f, 0xf5, 0x7e, 0x98, 0x2e, 0x0f, 0xca, 0x33, + 0x6f, 0x51, 0x6f, 0xc3, 0x40, 0x50, 0x42, 0x51, 0x7f, 0xe0, 0x7e, 0xc0, 0x90, 0x02, 0x2f, 0x91, 0x6f, 0x00, 0x2e, + 0x40, 0x42, 0x00, 0x2e, 0xe2, 0x6e, 0x90, 0x6f, 0x15, 0x53, 0x98, 0x2e, 0xc3, 0xb1, 0x93, 0x6f, 0xe1, 0x6e, 0xd2, + 0x40, 0x0a, 0x18, 0x01, 0x6f, 0x0e, 0x00, 0x93, 0x7f, 0x83, 0x30, 0x14, 0x6f, 0xf1, 0x6e, 0x42, 0x6f, 0x62, 0x0e, + 0x4f, 0x03, 0xd9, 0x2f, 0x35, 0x52, 0xc1, 0x00, 0x01, 0x30, 0xa9, 0x02, 0x91, 0x6f, 0x7c, 0x82, 0x21, 0xbd, 0xbf, + 0xb9, 0x1b, 0x30, 0x0a, 0x25, 0xda, 0x0a, 0x5b, 0x42, 0x25, 0x80, 0x33, 0x7f, 0x51, 0x7f, 0x20, 0x7f, 0x90, 0x7f, + 0xd3, 0x30, 0x64, 0x6f, 0x55, 0x6f, 0x10, 0x41, 0x52, 0x41, 0x31, 0x6f, 0x55, 0x7f, 0x10, 0x7f, 0x04, 0x7f, 0x98, + 0x2e, 0x0f, 0xca, 0x11, 0x6f, 0x20, 0x25, 0x98, 0x2e, 0xfe, 0xc9, 0x21, 0x6f, 0x04, 0x6f, 0x50, 0x42, 0x21, 0x7f, + 0xd3, 0x30, 0xa1, 0x6f, 0x61, 0x0e, 0xea, 0x2f, 0xb1, 0x6f, 0x45, 0x84, 0x32, 0x25, 0x90, 0x40, 0x84, 0x40, 0x91, + 0x6f, 0xb4, 0x7f, 0x92, 0x7f, 0x30, 0x7f, 0x23, 0x7f, 0x98, 0x2e, 0xb3, 0xc0, 0x53, 0x6f, 0xb1, 0x32, 0x19, 0x01, + 0x83, 0xb9, 0x31, 0x6f, 0x4b, 0x00, 0x02, 0x41, 0xb0, 0x6f, 0x03, 0x30, 0xc3, 0x02, 0x8f, 0xb0, 0xb4, 0x7f, 0xd5, + 0x3d, 0x25, 0x01, 0xa2, 0x6f, 0xa4, 0x7f, 0x26, 0x01, 0x27, 0x6f, 0x90, 0x6f, 0x07, 0x89, 0xc1, 0x43, 0x94, 0x7f, + 0x03, 0x42, 0x00, 0x2e, 0x11, 0x41, 0x31, 0x7f, 0x54, 0x7f, 0x00, 0x2e, 0x91, 0x40, 0x03, 0x41, 0x23, 0x7f, 0x12, + 0x7f, 0x98, 0x2e, 0x74, 0xc0, 0x31, 0x6f, 0xc8, 0x00, 0x90, 0x6f, 0x01, 0x30, 0x54, 0x6f, 0x22, 0x6f, 0x03, 0x42, + 0xd1, 0x02, 0x41, 0x6f, 0x12, 0x6f, 0x23, 0x43, 0x94, 0x7f, 0x51, 0x0e, 0xe7, 0x2f, 0xb1, 0x6f, 0xa3, 0x6f, 0x42, + 0x40, 0x8f, 0xb0, 0xf8, 0x82, 0xde, 0x00, 0xc9, 0x86, 0x52, 0x34, 0xb3, 0x7f, 0x8a, 0x00, 0xc6, 0x86, 0xa2, 0x7f, + 0x93, 0x7f, 0x00, 0x2e, 0xa5, 0x6f, 0xe2, 0x6f, 0x63, 0x41, 0x64, 0x41, 0x44, 0x8f, 0x82, 0x40, 0xe6, 0x41, 0xc0, + 0x41, 0xc4, 0x8f, 0x45, 0x41, 0xf0, 0x7f, 0xa7, 0x7f, 0x51, 0x7f, 0x98, 0x2e, 0x67, 0xcc, 0x00, 0x18, 0x09, 0x52, + 0x71, 0x00, 0x03, 0x30, 0xbb, 0x02, 0x1b, 0xba, 0xb3, 0x6f, 0x25, 0xbc, 0x51, 0x6f, 0xc5, 0x40, 0x42, 0x82, 0x20, + 0x0a, 0x28, 0x00, 0xd0, 0x42, 0x2b, 0xb9, 0xc0, 0x40, 0x82, 0x02, 0xd2, 0x42, 0xb3, 0x7f, 0x00, 0x2e, 0x92, 0x6f, + 0x5a, 0x0e, 0xd9, 0x2f, 0xa1, 0x6f, 0x43, 0x3d, 0x8b, 0x00, 0x9a, 0x82, 0x83, 0x6f, 0x41, 0x40, 0xc3, 0x40, 0xe0, + 0x6f, 0x14, 0x33, 0x04, 0x00, 0x82, 0x40, 0x4b, 0x12, 0x51, 0x0e, 0xb0, 0x7f, 0x90, 0x2e, 0x7a, 0xb5, 0x02, 0x40, + 0x8f, 0xb0, 0xd1, 0x3d, 0x41, 0x00, 0x72, 0x30, 0xd3, 0x04, 0x73, 0x80, 0x4e, 0x00, 0x02, 0x31, 0xd3, 0x05, 0xa0, + 0x7f, 0xf0, 0x8c, 0x04, 0x40, 0x52, 0x40, 0xc0, 0xb2, 0x50, 0x40, 0x4c, 0x17, 0x96, 0x7f, 0x57, 0x7f, 0x0a, 0x2f, + 0xd0, 0xa0, 0x03, 0x2f, 0x95, 0x6f, 0x65, 0x15, 0x06, 0x2c, 0x04, 0x30, 0x56, 0x6f, 0xa6, 0x13, 0x6b, 0x15, 0x23, + 0x15, 0x6e, 0x0b, 0x14, 0x05, 0x64, 0x18, 0x45, 0x07, 0xec, 0x18, 0xc0, 0xb2, 0xec, 0x18, 0x0a, 0x2f, 0xd0, 0xa0, + 0x03, 0x2f, 0x94, 0x6f, 0xbc, 0x13, 0x06, 0x2c, 0x07, 0x30, 0x54, 0x6f, 0x3c, 0x15, 0x73, 0x13, 0xfb, 0x13, 0xac, + 0x0b, 0x44, 0x40, 0x26, 0x05, 0x54, 0x42, 0xc0, 0xb2, 0x44, 0x40, 0x27, 0x07, 0x44, 0x42, 0x08, 0x2f, 0xd0, 0xa0, + 0x02, 0x2f, 0x91, 0x6f, 0x05, 0x2c, 0x81, 0x10, 0x51, 0x6f, 0x41, 0x14, 0xd3, 0x12, 0x99, 0x0a, 0xa1, 0x6f, 0xf3, + 0x32, 0x42, 0x42, 0x4b, 0x00, 0x13, 0x30, 0x42, 0x40, 0xd3, 0x28, 0x53, 0x42, 0xa1, 0x7f, 0xc2, 0xa2, 0x30, 0x2f, + 0x82, 0x6f, 0xe1, 0x6f, 0x98, 0x2e, 0xfa, 0xb1, 0x81, 0x6f, 0x41, 0x84, 0x00, 0x2e, 0x81, 0x40, 0x40, 0x90, 0x02, + 0x2f, 0x00, 0x2e, 0x07, 0x2c, 0x0c, 0xb8, 0x30, 0x25, 0xe1, 0x6f, 0x20, 0x33, 0x48, 0x00, 0x98, 0x2e, 0x07, 0xb6, + 0xe1, 0x6f, 0xf3, 0x32, 0x4b, 0x00, 0xe0, 0x7f, 0x00, 0x2e, 0x44, 0x40, 0x20, 0x1a, 0x15, 0x2f, 0xd3, 0x6f, 0xc1, + 0x6f, 0xc3, 0x40, 0x23, 0x5a, 0x42, 0x40, 0x83, 0x7e, 0x08, 0xbc, 0x25, 0x09, 0x92, 0x7e, 0xc4, 0x0a, 0x42, 0x82, + 0xa3, 0x7e, 0xd1, 0x7f, 0x34, 0x30, 0x63, 0x6f, 0x82, 0x30, 0x31, 0x30, 0x98, 0x2e, 0xb2, 0x00, 0xd1, 0x6f, 0xe3, + 0x6f, 0x43, 0x42, 0x00, 0x2e, 0xa1, 0x6f, 0xb2, 0x6f, 0x43, 0x40, 0xc1, 0x86, 0xc2, 0xa2, 0x43, 0x42, 0x03, 0x30, + 0x00, 0x2f, 0x83, 0x42, 0xd2, 0x3d, 0x40, 0x40, 0x0f, 0xb0, 0x0a, 0x01, 0x26, 0x00, 0x05, 0x32, 0x98, 0x2e, 0x7e, + 0xb5, 0x65, 0x00, 0x00, 0x2e, 0x43, 0x42, 0x00, 0x2e, 0x7b, 0x6f, 0x50, 0x5e, 0xb8, 0x2e, 0x0f, 0x82, 0x02, 0x30, + 0x12, 0x42, 0x41, 0x0e, 0xfc, 0x2f, 0xb8, 0x2e, 0xc1, 0x35, 0x40, 0x51, 0x01, 0x01, 0x02, 0x30, 0x1a, 0x25, 0x13, + 0x30, 0x12, 0x42, 0x44, 0x0e, 0xfc, 0x2f, 0x54, 0x3a, 0x04, 0x01, 0x1d, 0x5b, 0x05, 0x7f, 0x75, 0x34, 0x13, 0x5f, + 0x07, 0x43, 0x25, 0x01, 0x47, 0x5a, 0x05, 0x43, 0x27, 0x89, 0x67, 0x5a, 0x05, 0x43, 0x18, 0x8b, 0x19, 0x59, 0x17, + 0x51, 0xd4, 0x7e, 0x43, 0x43, 0xc0, 0x7e, 0xe0, 0x7e, 0x6c, 0x88, 0x1b, 0x5d, 0xf6, 0x7e, 0x71, 0x86, 0x42, 0x81, + 0x15, 0x41, 0x15, 0x42, 0x63, 0x0e, 0xfb, 0x2f, 0x21, 0x59, 0x1f, 0x5d, 0x25, 0x5f, 0x23, 0x5b, 0x34, 0x7f, 0x16, + 0x7f, 0x45, 0x7f, 0x57, 0x7f, 0x22, 0x7f, 0x76, 0x88, 0xd5, 0x40, 0x15, 0x42, 0x5c, 0x0e, 0xfb, 0x2f, 0x29, 0x57, + 0x27, 0x5d, 0x2d, 0x5f, 0x2b, 0x5b, 0x83, 0x7f, 0x66, 0x7f, 0x95, 0x7f, 0xa7, 0x7f, 0x72, 0x7f, 0x7b, 0x86, 0x15, + 0x41, 0x15, 0x42, 0x63, 0x0e, 0xfb, 0x2f, 0x31, 0x59, 0x33, 0x5d, 0x2f, 0x5b, 0xc5, 0x7f, 0xd4, 0x7f, 0xf6, 0x7f, + 0xb2, 0x7f, 0xe2, 0x7f, 0x40, 0x82, 0xd2, 0x40, 0x12, 0x42, 0x59, 0x0e, 0xfb, 0x2f, 0xc0, 0x5e, 0xb8, 0x2e, 0x00, + 0x30, 0xc0, 0x2e, 0x21, 0x2e, 0x8c, 0x00, 0x35, 0x51, 0xc0, 0x2e, 0x21, 0x2e, 0xad, 0x00, 0x05, 0x2e, 0x28, 0x02, + 0x2f, 0xbd, 0x2f, 0xb9, 0x20, 0x50, 0x80, 0xb2, 0x16, 0x2f, 0x20, 0x25, 0x01, 0x2e, 0x8c, 0x00, 0x00, 0x90, 0x0b, + 0x2f, 0x37, 0x51, 0xf2, 0x7f, 0xeb, 0x7f, 0x98, 0x2e, 0x84, 0xb5, 0x01, 0x2e, 0x8c, 0x00, 0x01, 0x80, 0x21, 0x2e, + 0x8c, 0x00, 0xf2, 0x6f, 0xeb, 0x6f, 0xe0, 0x5f, 0x03, 0x2e, 0xad, 0x00, 0x37, 0x51, 0x80, 0x2e, 0x74, 0xb3, 0x00, + 0x30, 0x21, 0x2e, 0x8c, 0x00, 0xe0, 0x5f, 0xb8, 0x2e, 0x02, 0x30, 0x02, 0x2c, 0x41, 0x00, 0x12, 0x42, 0x41, 0x0e, + 0xfc, 0x2f, 0xb8, 0x2e, 0x81, 0x8e, 0x39, 0x51, 0x07, 0x5c, 0x58, 0x09, 0x9e, 0x09, 0xc3, 0x41, 0x50, 0x50, 0xc3, + 0x89, 0xf3, 0x0e, 0x41, 0x80, 0xf0, 0x7f, 0xdc, 0xb9, 0xe4, 0x7f, 0x44, 0x80, 0x84, 0x8e, 0x43, 0x88, 0xd1, 0x7f, + 0x05, 0x30, 0xc2, 0x7f, 0x21, 0x2f, 0xf1, 0x6f, 0x00, 0x2e, 0x41, 0x40, 0x19, 0x1a, 0x1c, 0x2f, 0x82, 0x84, 0x00, + 0x2e, 0x82, 0x40, 0xf2, 0x0e, 0x05, 0x2f, 0x02, 0x41, 0x81, 0x84, 0x01, 0x30, 0x03, 0x30, 0x22, 0x2c, 0x02, 0x43, + 0xff, 0x84, 0x42, 0x00, 0x60, 0x25, 0x43, 0x40, 0xc1, 0x86, 0x43, 0x42, 0x11, 0x30, 0xc2, 0x41, 0x03, 0x30, 0x97, + 0x41, 0xc1, 0x86, 0xfa, 0x0f, 0x13, 0x2f, 0xc5, 0xa2, 0xf9, 0x2f, 0x03, 0x30, 0x10, 0x2c, 0x01, 0x30, 0xd2, 0x6f, + 0x00, 0x2e, 0x82, 0x40, 0x80, 0x90, 0x04, 0x2f, 0xd2, 0x6f, 0xf1, 0x6f, 0x17, 0x30, 0x87, 0x42, 0x43, 0x42, 0x00, + 0x2e, 0x3b, 0x55, 0xf2, 0x0e, 0x12, 0x30, 0x55, 0x22, 0x40, 0xb2, 0xbb, 0x7f, 0x06, 0x2f, 0x51, 0x30, 0x60, 0x25, + 0x98, 0x2e, 0x00, 0xb6, 0xbf, 0x81, 0x00, 0x2e, 0x05, 0x42, 0x00, 0x2e, 0xe2, 0x6f, 0x01, 0x41, 0x82, 0x40, 0xd0, + 0x6f, 0x13, 0x88, 0xca, 0x0f, 0x49, 0x2f, 0xf2, 0x6f, 0xc0, 0xa6, 0x87, 0x40, 0x43, 0x2f, 0xc5, 0x6f, 0x02, 0x82, + 0x43, 0x8d, 0x42, 0x40, 0x81, 0x8a, 0x86, 0x41, 0x6e, 0x0e, 0x45, 0x42, 0x47, 0x8a, 0xaa, 0x00, 0x1b, 0x30, 0x83, + 0x42, 0x09, 0x84, 0xf2, 0x7f, 0x02, 0x30, 0x03, 0x2f, 0x0b, 0x43, 0x2f, 0x89, 0x00, 0x2e, 0x02, 0x43, 0x51, 0x88, + 0x41, 0x40, 0x15, 0x41, 0x40, 0xb3, 0x4e, 0x22, 0x5f, 0x1a, 0x14, 0x80, 0x03, 0x41, 0x06, 0x2f, 0xc1, 0x84, 0xd6, + 0x0e, 0x02, 0x42, 0x22, 0x2f, 0x00, 0x2e, 0x21, 0x2c, 0x06, 0x42, 0xff, 0x80, 0x41, 0x0f, 0x91, 0xb9, 0x10, 0x22, + 0xf4, 0x6f, 0x61, 0x00, 0xc3, 0x0f, 0x12, 0x2f, 0x13, 0x30, 0x04, 0x30, 0xf6, 0x6f, 0x05, 0x30, 0x05, 0x2c, 0xe7, + 0x7f, 0x97, 0x41, 0x3c, 0x1a, 0xda, 0x23, 0x6f, 0x01, 0x71, 0x0e, 0xf9, 0x2f, 0x68, 0x0f, 0xe6, 0x6f, 0xe6, 0x23, + 0x01, 0x89, 0x28, 0x22, 0x05, 0xa7, 0xee, 0x2f, 0xf2, 0x6f, 0xb8, 0x84, 0x93, 0x82, 0x87, 0x42, 0x40, 0x42, 0x08, + 0x2c, 0x07, 0x25, 0x13, 0x30, 0x50, 0x25, 0x98, 0x2e, 0xb3, 0xb6, 0x00, 0x30, 0x43, 0x43, 0x03, 0x43, 0x00, 0x2e, + 0xbb, 0x6f, 0xb0, 0x5f, 0xb8, 0x2e, 0x15, 0x82, 0x02, 0x30, 0x12, 0x42, 0x41, 0x0e, 0xfc, 0x2f, 0xb8, 0x2e, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0xfd, 0x2d, 0x01, 0x2e, 0x5d, 0xf7, 0x08, 0xbc, 0x80, 0xac, 0x0e, 0xbb, 0x02, 0x2f, + 0x00, 0x30, 0x41, 0x04, 0x82, 0x06, 0xc0, 0xa4, 0x00, 0x30, 0x11, 0x2f, 0x40, 0xa9, 0x03, 0x2f, 0x40, 0x91, 0x0d, + 0x2f, 0x00, 0xa7, 0x0b, 0x2f, 0x80, 0xb3, 0x35, 0x58, 0x02, 0x2f, 0x90, 0xa1, 0x26, 0x13, 0x20, 0x23, 0x80, 0x90, + 0x10, 0x30, 0x01, 0x2f, 0xcc, 0x0e, 0x00, 0x2f, 0x00, 0x30, 0xb8, 0x2e, 0x3d, 0x56, 0x37, 0x54, 0xd0, 0x40, 0xc4, + 0x40, 0x0b, 0x2e, 0xfd, 0xf3, 0x3d, 0x52, 0x90, 0x42, 0x94, 0x42, 0x95, 0x42, 0x05, 0x30, 0x3f, 0x50, 0x0f, 0x88, + 0x06, 0x40, 0x04, 0x41, 0x96, 0x42, 0xc5, 0x42, 0x48, 0xbe, 0x73, 0x30, 0x0d, 0x2e, 0x8a, 0x00, 0x4f, 0xba, 0x84, + 0x42, 0x03, 0x42, 0x81, 0xb3, 0x02, 0x2f, 0x2b, 0x2e, 0x6f, 0xf5, 0x06, 0x2d, 0x05, 0x2e, 0x77, 0xf7, 0x3b, 0x56, + 0x93, 0x08, 0x25, 0x2e, 0x77, 0xf7, 0x39, 0x54, 0x25, 0x2e, 0xc2, 0xf5, 0x07, 0x2e, 0xfd, 0xf3, 0x42, 0x30, 0xb4, + 0x33, 0xda, 0x0a, 0x4c, 0x00, 0x27, 0x2e, 0xfd, 0xf3, 0x43, 0x40, 0xd4, 0x3f, 0xdc, 0x08, 0x43, 0x42, 0x00, 0x2e, + 0x00, 0x2e, 0x43, 0x40, 0x24, 0x30, 0xdc, 0x0a, 0x43, 0x42, 0x04, 0x80, 0x03, 0x2e, 0xfd, 0xf3, 0x4a, 0x0a, 0x23, + 0x2e, 0xfd, 0xf3, 0x61, 0x34, 0xc0, 0x2e, 0x01, 0x42, 0x00, 0x2e, 0xd0, 0x51, 0xfb, 0x7f, 0x98, 0x2e, 0xcf, 0x0d, + 0x5a, 0x25, 0x98, 0x2e, 0xf6, 0x0d, 0x6b, 0x87, 0x49, 0x54, 0xe1, 0x7f, 0xa3, 0x7f, 0xb3, 0x7f, 0xb2, 0x88, 0x41, + 0x52, 0xc2, 0x7f, 0x65, 0x8b, 0x43, 0x56, 0x84, 0x7f, 0x61, 0x7f, 0x75, 0x7f, 0xd0, 0x7f, 0x95, 0x7f, 0x53, 0x7f, + 0x14, 0x30, 0x45, 0x54, 0x81, 0x6f, 0x42, 0x7f, 0x00, 0x2e, 0x53, 0x40, 0x45, 0x8c, 0x42, 0x40, 0x90, 0x41, 0xbb, + 0x83, 0x86, 0x41, 0xd8, 0x04, 0x16, 0x06, 0x00, 0xac, 0x81, 0x7f, 0x02, 0x2f, 0x02, 0x30, 0xd3, 0x04, 0x10, 0x06, + 0xc1, 0x84, 0x01, 0x30, 0xc1, 0x02, 0x0b, 0x16, 0x04, 0x09, 0x14, 0x01, 0x99, 0x02, 0xc1, 0xb9, 0xaf, 0xbc, 0x59, + 0x0a, 0x64, 0x6f, 0x51, 0x43, 0xa1, 0xb4, 0x12, 0x41, 0x13, 0x41, 0x41, 0x43, 0x35, 0x7f, 0x64, 0x7f, 0x26, 0x31, + 0xe5, 0x6f, 0xd4, 0x6f, 0x98, 0x2e, 0x37, 0xca, 0x32, 0x6f, 0x75, 0x6f, 0x83, 0x40, 0x42, 0x41, 0x23, 0x7f, 0x12, + 0x7f, 0xf6, 0x30, 0x40, 0x25, 0x51, 0x25, 0x98, 0x2e, 0x37, 0xca, 0x14, 0x6f, 0x20, 0x05, 0x70, 0x6f, 0x25, 0x6f, + 0x69, 0x07, 0xa2, 0x6f, 0x31, 0x6f, 0x0b, 0x30, 0x04, 0x42, 0x9b, 0x42, 0x8b, 0x42, 0x55, 0x42, 0x32, 0x7f, 0x40, + 0xa9, 0xc3, 0x6f, 0x71, 0x7f, 0x02, 0x30, 0xd0, 0x40, 0xc3, 0x7f, 0x03, 0x2f, 0x40, 0x91, 0x15, 0x2f, 0x00, 0xa7, + 0x13, 0x2f, 0x00, 0xa4, 0x11, 0x2f, 0x84, 0xbd, 0x98, 0x2e, 0x79, 0xca, 0x55, 0x6f, 0x51, 0x54, 0x54, 0x41, 0x82, + 0x00, 0xf3, 0x3f, 0x45, 0x41, 0xcb, 0x02, 0xf6, 0x30, 0x98, 0x2e, 0x37, 0xca, 0x33, 0x6f, 0xa4, 0x6f, 0xc1, 0x42, + 0x03, 0x2c, 0x00, 0x43, 0xa4, 0x6f, 0x33, 0x6f, 0x00, 0x2e, 0x42, 0x6f, 0x55, 0x6f, 0x91, 0x40, 0x42, 0x8b, 0x00, + 0x41, 0x41, 0x00, 0x01, 0x43, 0x55, 0x7f, 0x14, 0x30, 0xc1, 0x40, 0x95, 0x40, 0x4d, 0x02, 0xc5, 0x6f, 0x4f, 0x50, + 0x68, 0x0e, 0x75, 0x6f, 0xd1, 0x42, 0xa3, 0x7f, 0x8a, 0x2f, 0x09, 0x2e, 0x8a, 0x00, 0x01, 0xb3, 0x22, 0x2f, 0x49, + 0x58, 0x90, 0x6f, 0x17, 0x30, 0x13, 0x41, 0xb6, 0x6f, 0xe4, 0x7f, 0x00, 0x2e, 0x91, 0x41, 0x14, 0x40, 0x92, 0x41, + 0x15, 0x40, 0x17, 0x2e, 0x6f, 0xf5, 0xb6, 0x7f, 0xd0, 0x7f, 0xcb, 0x7f, 0x98, 0x2e, 0x00, 0x0c, 0x07, 0x15, 0xc2, + 0x6f, 0x14, 0x0b, 0x29, 0x2e, 0x6f, 0xf5, 0xc3, 0xa3, 0xc1, 0x8f, 0xe4, 0x6f, 0xd0, 0x6f, 0xe6, 0x2f, 0x14, 0x30, + 0x05, 0x2e, 0x6f, 0xf5, 0x14, 0x0b, 0x29, 0x2e, 0x6f, 0xf5, 0x44, 0x2d, 0x4b, 0x54, 0x01, 0x32, 0x51, 0x58, 0x05, + 0x30, 0x23, 0x50, 0x83, 0x40, 0xd8, 0x08, 0x91, 0x01, 0xb8, 0xbd, 0x38, 0xb5, 0xe6, 0x7f, 0x0a, 0x16, 0xb1, 0x6f, + 0x2a, 0xbb, 0xa6, 0xbd, 0x1c, 0x01, 0x06, 0xbc, 0x52, 0x40, 0x06, 0x0a, 0x53, 0x40, 0x45, 0x03, 0xb1, 0x7f, 0xf6, + 0x30, 0x98, 0x2e, 0x37, 0xca, 0x1a, 0xbd, 0x16, 0xb6, 0x86, 0xba, 0x00, 0xa9, 0xaa, 0x0a, 0x53, 0x52, 0x0f, 0x2f, + 0x00, 0x91, 0x53, 0x52, 0x03, 0x2f, 0x53, 0x5a, 0x55, 0x0f, 0x53, 0x52, 0x08, 0x2f, 0x3f, 0xa1, 0x04, 0x2f, 0x3f, + 0x91, 0x03, 0x2f, 0x51, 0x58, 0xd4, 0x0f, 0x00, 0x2f, 0x51, 0x54, 0x12, 0x25, 0xf2, 0x33, 0x98, 0x2e, 0xd9, 0xc0, + 0xe4, 0x6f, 0xf5, 0x37, 0x45, 0x09, 0x21, 0x85, 0x05, 0x43, 0x05, 0x30, 0x4d, 0x52, 0x51, 0x0e, 0x01, 0x32, 0x51, + 0x58, 0xc5, 0x2f, 0x47, 0x54, 0x09, 0x2e, 0x77, 0xf7, 0x22, 0x0b, 0x29, 0x2e, 0x77, 0xf7, 0xfb, 0x6f, 0x30, 0x5e, + 0xb8, 0x2e, 0x10, 0x50, 0x01, 0x2e, 0x86, 0x00, 0x00, 0xb2, 0xfb, 0x7f, 0x5d, 0x2f, 0x01, 0xb2, 0x54, 0x2f, 0x02, + 0xb2, 0x4e, 0x2f, 0x03, 0x90, 0x63, 0x2f, 0x59, 0x50, 0x39, 0x82, 0x02, 0x40, 0x81, 0x88, 0x5b, 0x54, 0x41, 0x40, + 0x61, 0x56, 0x04, 0x42, 0x00, 0x2e, 0x94, 0x40, 0x95, 0x40, 0xd8, 0xbe, 0x2c, 0x0b, 0x45, 0x40, 0x6c, 0x01, 0x55, + 0x42, 0x0c, 0x17, 0x45, 0x40, 0x2c, 0x03, 0x54, 0x42, 0x53, 0x0e, 0xf2, 0x2f, 0x63, 0x56, 0x3e, 0x82, 0xe2, 0x40, + 0xc3, 0x40, 0x28, 0xbd, 0x93, 0x0a, 0x43, 0x40, 0xda, 0x00, 0x53, 0x42, 0x8a, 0x16, 0x43, 0x40, 0x9a, 0x02, 0x52, + 0x42, 0x00, 0x2e, 0x41, 0x40, 0x47, 0x54, 0x4a, 0x0e, 0x3b, 0x2f, 0x3a, 0x82, 0x00, 0x30, 0x41, 0x40, 0x21, 0x2e, + 0x52, 0x0f, 0x40, 0xb2, 0x0a, 0x2f, 0x98, 0x2e, 0x61, 0x0c, 0x98, 0x2e, 0x2b, 0x0e, 0x98, 0x2e, 0x41, 0x0e, 0xfb, + 0x6f, 0xf0, 0x5f, 0x00, 0x30, 0x80, 0x2e, 0xaf, 0xb1, 0x5f, 0x54, 0x55, 0x56, 0x83, 0x42, 0x8f, 0x86, 0x74, 0x30, + 0x5d, 0x54, 0xc4, 0x42, 0x11, 0x30, 0x23, 0x2e, 0x86, 0x00, 0xa1, 0x42, 0x23, 0x30, 0x27, 0x2e, 0x89, 0x00, 0x21, + 0x2e, 0x88, 0x00, 0xba, 0x82, 0x18, 0x2c, 0x81, 0x42, 0x30, 0x30, 0x21, 0x2e, 0x86, 0x00, 0x13, 0x2d, 0x21, 0x30, + 0x00, 0x30, 0x23, 0x2e, 0x86, 0x00, 0x21, 0x2e, 0x7b, 0xf7, 0x0c, 0x2d, 0x77, 0x30, 0x98, 0x2e, 0x1f, 0x0c, 0x57, + 0x50, 0x0c, 0x82, 0x12, 0x30, 0x40, 0x42, 0x25, 0x2e, 0x86, 0x00, 0x2f, 0x2e, 0x7b, 0xf7, 0xfb, 0x6f, 0xf0, 0x5f, + 0xb8, 0x2e, 0x70, 0x50, 0x0a, 0x25, 0x39, 0x86, 0xfb, 0x7f, 0xe1, 0x32, 0x62, 0x30, 0x98, 0x2e, 0xc2, 0xc4, 0x23, + 0x56, 0xa5, 0x6f, 0xab, 0x08, 0x91, 0x6f, 0x4b, 0x08, 0x65, 0x56, 0xc4, 0x6f, 0x23, 0x09, 0x4d, 0xba, 0x93, 0xbc, + 0x8c, 0x0b, 0xd1, 0x6f, 0x0b, 0x09, 0x49, 0x52, 0x67, 0x5e, 0x56, 0x42, 0xaf, 0x09, 0x4d, 0xba, 0x23, 0xbd, 0x94, + 0x0a, 0xe5, 0x6f, 0x68, 0xbb, 0xeb, 0x08, 0xbd, 0xb9, 0x63, 0xbe, 0xfb, 0x6f, 0x52, 0x42, 0xe3, 0x0a, 0xc0, 0x2e, + 0x43, 0x42, 0x90, 0x5f, 0x4f, 0x50, 0x03, 0x2e, 0x25, 0xf3, 0x12, 0x40, 0x00, 0x40, 0x28, 0xba, 0x9b, 0xbc, 0x88, + 0xbd, 0x93, 0xb4, 0xe3, 0x0a, 0x89, 0x16, 0x08, 0xb6, 0xc0, 0x2e, 0x19, 0x00, 0x62, 0x02, 0x10, 0x50, 0xfb, 0x7f, + 0x98, 0x2e, 0x5d, 0x0d, 0x01, 0x2e, 0x86, 0x00, 0x31, 0x30, 0x08, 0x04, 0xfb, 0x6f, 0x01, 0x30, 0xf0, 0x5f, 0x23, + 0x2e, 0x88, 0x00, 0x21, 0x2e, 0x89, 0x00, 0xb8, 0x2e, 0x01, 0x2e, 0x89, 0x00, 0x03, 0x2e, 0x88, 0x00, 0x48, 0x0e, + 0x01, 0x2f, 0x80, 0x2e, 0x05, 0x0e, 0xb8, 0x2e, 0x69, 0x50, 0x21, 0x34, 0x01, 0x42, 0x82, 0x30, 0xc1, 0x32, 0x25, + 0x2e, 0x62, 0xf5, 0x01, 0x00, 0x22, 0x30, 0x01, 0x40, 0x4a, 0x0a, 0x01, 0x42, 0xb8, 0x2e, 0x69, 0x54, 0xf0, 0x3b, + 0x83, 0x40, 0xd8, 0x08, 0x6b, 0x52, 0x83, 0x42, 0x00, 0x30, 0x83, 0x30, 0x50, 0x42, 0xc4, 0x32, 0x27, 0x2e, 0x64, + 0xf5, 0x94, 0x00, 0x50, 0x42, 0x40, 0x42, 0xd3, 0x3f, 0x84, 0x40, 0x7d, 0x82, 0xe3, 0x08, 0x40, 0x42, 0x83, 0x42, + 0xb8, 0x2e, 0x5f, 0x52, 0x00, 0x30, 0x40, 0x42, 0x7c, 0x86, 0x37, 0x52, 0x09, 0x2e, 0x3d, 0x0f, 0x3d, 0x54, 0xc4, + 0x42, 0xd3, 0x86, 0x54, 0x40, 0x55, 0x40, 0x94, 0x42, 0x85, 0x42, 0x21, 0x2e, 0x89, 0x00, 0x42, 0x40, 0x25, 0x2e, + 0xfd, 0xf3, 0xc0, 0x42, 0x7e, 0x82, 0x05, 0x2e, 0x79, 0x00, 0x80, 0xb2, 0x14, 0x2f, 0x05, 0x2e, 0x24, 0x02, 0x27, + 0xbd, 0x2f, 0xb9, 0x80, 0x90, 0x02, 0x2f, 0x21, 0x2e, 0x6f, 0xf5, 0x0c, 0x2d, 0x07, 0x2e, 0x3e, 0x0f, 0x14, 0x30, + 0x1c, 0x09, 0x05, 0x2e, 0x77, 0xf7, 0x3b, 0x56, 0x47, 0xbe, 0x93, 0x08, 0x94, 0x0a, 0x25, 0x2e, 0x77, 0xf7, 0x6d, + 0x54, 0x50, 0x42, 0x4a, 0x0e, 0xfc, 0x2f, 0xb8, 0x2e, 0x50, 0x50, 0x02, 0x30, 0x43, 0x86, 0x6b, 0x50, 0xfb, 0x7f, + 0xe3, 0x7f, 0xd2, 0x7f, 0xc0, 0x7f, 0xb1, 0x7f, 0x00, 0x2e, 0x41, 0x40, 0x00, 0x40, 0x48, 0x04, 0x98, 0x2e, 0x74, + 0xc0, 0x1e, 0xaa, 0xd3, 0x6f, 0x14, 0x30, 0xb1, 0x6f, 0xe3, 0x22, 0xc0, 0x6f, 0x52, 0x40, 0xe4, 0x6f, 0x4c, 0x0e, + 0x12, 0x42, 0xd3, 0x7f, 0xeb, 0x2f, 0x03, 0x2e, 0x53, 0x0f, 0x40, 0x90, 0x11, 0x30, 0x03, 0x2f, 0x23, 0x2e, 0x53, + 0x0f, 0x02, 0x2c, 0x00, 0x30, 0xd0, 0x6f, 0xfb, 0x6f, 0xb0, 0x5f, 0xb8, 0x2e, 0x40, 0x50, 0xf1, 0x7f, 0x0a, 0x25, + 0x3c, 0x86, 0xeb, 0x7f, 0x41, 0x33, 0x22, 0x30, 0x98, 0x2e, 0xc2, 0xc4, 0xd3, 0x6f, 0xf4, 0x30, 0xdc, 0x09, 0x71, + 0x58, 0xc2, 0x6f, 0x94, 0x09, 0x73, 0x58, 0x6a, 0xbb, 0xdc, 0x08, 0xb4, 0xb9, 0xb1, 0xbd, 0x6f, 0x5a, 0x95, 0x08, + 0x21, 0xbd, 0xf6, 0xbf, 0x77, 0x0b, 0x51, 0xbe, 0xf1, 0x6f, 0xeb, 0x6f, 0x52, 0x42, 0x54, 0x42, 0xc0, 0x2e, 0x43, + 0x42, 0xc0, 0x5f, 0x50, 0x50, 0x77, 0x52, 0x93, 0x30, 0x53, 0x42, 0xfb, 0x7f, 0x7b, 0x30, 0x4b, 0x42, 0x13, 0x30, + 0x42, 0x82, 0x20, 0x33, 0x43, 0x42, 0xc8, 0x00, 0x01, 0x2e, 0x80, 0x03, 0x05, 0x2e, 0x7e, 0x00, 0x19, 0x52, 0xe2, + 0x7f, 0xd0, 0x7f, 0xc3, 0x7f, 0x98, 0x2e, 0x9c, 0x0e, 0xd1, 0x6f, 0x48, 0x0a, 0xd1, 0x7f, 0x3a, 0x25, 0xfb, 0x86, + 0x01, 0x33, 0x12, 0x30, 0x98, 0x2e, 0xc2, 0xc4, 0xd1, 0x6f, 0x48, 0x0a, 0x40, 0xb2, 0x0d, 0x2f, 0xe0, 0x6f, 0x03, + 0x2e, 0x80, 0x03, 0x53, 0x30, 0x07, 0x80, 0x27, 0x2e, 0x21, 0xf2, 0x98, 0xbc, 0x01, 0x42, 0x98, 0x2e, 0xe0, 0x03, + 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0xb1, 0x6f, 0x9b, 0xb8, 0x07, 0x2e, 0x1b, 0x00, 0x19, 0x1a, 0xb1, 0x7f, 0x71, + 0x30, 0x04, 0x2f, 0x23, 0x2e, 0x21, 0xf2, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x98, 0x2e, 0x6d, 0xc0, 0x98, 0x2e, + 0x5d, 0xc0, 0x98, 0x2e, 0xd9, 0xb5, 0x20, 0x26, 0xc1, 0x6f, 0x02, 0x31, 0x52, 0x42, 0xab, 0x30, 0x4b, 0x42, 0x20, + 0x33, 0x79, 0x56, 0xf1, 0x37, 0xc4, 0x40, 0xa2, 0x0a, 0xc2, 0x42, 0xd8, 0x00, 0x01, 0x2e, 0x5e, 0xf7, 0x41, 0x08, + 0x23, 0x2e, 0x93, 0x00, 0xe3, 0x7f, 0x98, 0x2e, 0xe1, 0x00, 0xe1, 0x6f, 0x83, 0x30, 0x43, 0x42, 0x03, 0x30, 0xfb, + 0x6f, 0x75, 0x50, 0x02, 0x30, 0x00, 0x2e, 0x00, 0x2e, 0x81, 0x84, 0x50, 0x0e, 0xfa, 0x2f, 0x43, 0x42, 0x11, 0x30, + 0xb0, 0x5f, 0x23, 0x2e, 0x21, 0xf2, 0xb8, 0x2e, 0xc1, 0x4a, 0x00, 0x00, 0x6d, 0x57, 0x00, 0x00, 0x77, 0x8e, 0x00, + 0x00, 0xe0, 0xff, 0xff, 0xff, 0xd3, 0xff, 0xff, 0xff, 0xe5, 0xff, 0xff, 0xff, 0xee, 0xe1, 0xff, 0xff, 0x7c, 0x13, + 0x00, 0x00, 0x46, 0xe6, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0xfd, 0x2d +}; + +/*! @name Global array that stores the feature input configuration of + * BMI270_HUAWEI_CONTEXT + */ +const struct bmi2_feature_config bmi270_hc_feat_in[BMI270_HC_MAX_FEAT_IN] = { + { .type = BMI2_CONFIG_ID, .page = BMI2_PAGE_4, .start_addr = BMI270_HC_CONFIG_ID_STRT_ADDR }, + { .type = BMI2_STEP_COUNTER_PARAMS, .page = BMI2_PAGE_1, .start_addr = BMI270_HC_STEP_CNT_1_STRT_ADDR }, + { .type = BMI2_STEP_DETECTOR, .page = BMI2_PAGE_4, .start_addr = BMI270_HC_STEP_CNT_4_STRT_ADDR }, + { .type = BMI2_STEP_COUNTER, .page = BMI2_PAGE_4, .start_addr = BMI270_HC_STEP_CNT_4_STRT_ADDR }, + { .type = BMI2_NVM_PROG_PREP, .page = BMI2_PAGE_4, .start_addr = BMI270_HC_NVM_PROG_PREP_STRT_ADDR }, + { .type = BMI2_MAX_BURST_LEN, .page = BMI2_PAGE_4, .start_addr = BMI270_HC_MAX_BURST_LEN_STRT_ADDR }, + { .type = BMI2_CRT_GYRO_SELF_TEST, .page = BMI2_PAGE_4, .start_addr = BMI270_HC_CRT_GYRO_SELF_TEST_STRT_ADDR }, + { .type = BMI2_ABORT_CRT_GYRO_SELF_TEST, .page = BMI2_PAGE_4, .start_addr = BMI270_HC_ABORT_STRT_ADDR }, + { .type = BMI2_ACTIVITY_RECOGNITION_SETTINGS, .page = BMI2_PAGE_5, .start_addr = BMI270_HC_ACT_RGN_SETT_STRT_ADDR }, + { .type = BMI2_ACTIVITY_RECOGNITION, .page = BMI2_PAGE_5, .start_addr = BMI270_HC_ACT_RGN_STRT_ADDR }, +}; + +/*! @name Global array that stores the feature output configuration */ +const struct bmi2_feature_config bmi270_hc_feat_out[BMI270_HC_MAX_FEAT_OUT] = { + { .type = BMI2_STEP_COUNTER, .page = BMI2_PAGE_0, .start_addr = BMI270_HC_STEP_CNT_OUT_STRT_ADDR }, + { .type = BMI2_GYRO_CROSS_SENSE, .page = BMI2_PAGE_0, .start_addr = BMI270_HC_GYRO_CROSS_SENSE_STRT_ADDR }, + { .type = BMI2_GYRO_GAIN_UPDATE, .page = BMI2_PAGE_0, .start_addr = BMI270_HC_GYR_USER_GAIN_OUT_STRT_ADDR }, + { .type = BMI2_NVM_STATUS, .page = BMI2_PAGE_0, .start_addr = BMI270_HC_NVM_VFRM_OUT_STRT_ADDR }, + { .type = BMI2_VFRM_STATUS, .page = BMI2_PAGE_0, .start_addr = BMI270_HC_NVM_VFRM_OUT_STRT_ADDR } +}; + +/*! @name Global array that stores the feature interrupts of BMI270_HUAWEI_CONTEXT */ +struct bmi2_map_int bmi270_hc_map_int[BMI270_HC_MAX_INT_MAP] = { + { .type = BMI2_STEP_COUNTER, .sens_map_int = BMI270_HC_INT_STEP_COUNTER_MASK }, + { .type = BMI2_STEP_DETECTOR, .sens_map_int = BMI270_HC_INT_STEP_DETECTOR_MASK }, +}; + +/******************************************************************************/ + +/*! Local Function Prototypes + ******************************************************************************/ + +/*! + * @brief This internal API is used to validate the device pointer for + * null conditions. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t null_ptr_check(const struct bmi2_dev *dev); + +/*! + * @brief This internal API enables the selected sensor/features. + * + * @param[in] sensor_sel : Selects the desired sensor. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t sensor_enable(uint64_t sensor_sel, struct bmi2_dev *dev); + +/*! + * @brief This internal API disables the selected sensor/features. + * + * @param[in] sensor_sel : Selects the desired sensor. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t sensor_disable(uint64_t sensor_sel, struct bmi2_dev *dev); + +/*! + * @brief This internal API selects the sensors/features to be enabled or + * disabled. + * + * @param[in] sens_list : Pointer to select the sensor. + * @param[in] n_sens : Number of sensors selected. + * @param[out] sensor_sel : Gets the selected sensor. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t select_sensor(const uint8_t *sens_list, uint8_t n_sens, uint64_t *sensor_sel); + +/*! + * @brief This internal API is used to enable/disable step detector feature. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables step-detector. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables step detector + * BMI2_ENABLE | Enables step detector + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_step_detector(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to enable/disable step counter feature. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables step counter. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables step counter + * BMI2_ENABLE | Enables step counter + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_step_counter(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API enables/disables the activity recognition feature. + * + * @param[in] enable : Enables/Disables activity recognition. + * @param[in] dev : Structure instance of bmi2_dev. + * + * enable | Description + * -------------|--------------- + * BMI2_ENABLE | Enables activity recognition. + * BMI2_DISABLE | Disables activity recognition. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_act_recog(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to enable/disable gyroscope user gain + * feature. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables gyroscope user gain. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables gyroscope user gain + * BMI2_ENABLE | Enables gyroscope user gain + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_gyro_user_gain(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets step counter parameter configurations. + * + * @param[in] step_count_params : Array that stores parameters 1 to 25. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_step_count_params_config(const uint16_t *step_count_params, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets step counter/detector/activity configurations. + * + * @param[in] config : Structure instance of bmi2_step_config. + * @param[in] dev : Structure instance of bmi2_dev. + * + *--------------------------------------------------------------------------- + * bmi2_step_config | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * | The Step-counter will trigger output every time + * | the number of steps are counted. Holds implicitly + * water-mark level | a 20x factor, so the range is 0 to 10230, + * | with resolution of 20 steps. + * -------------------------|--------------------------------------------------- + * reset counter | Flag to reset the counted steps. + * -------------------------|--------------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_step_config(const struct bmi2_step_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets step counter parameter configurations. + * + * @param[in] step_count_params : Array that stores parameters 1 to 25. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_step_count_params_config(uint16_t *step_count_params, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets step counter/detector/activity configurations. + * + * @param[out] config : Structure instance of bmi2_step_config. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @verbatim + *---------------------------------------------------------------------------- + * bmi2_step_config | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * | The Step-counter will trigger output every time + * | the number of steps are counted. Holds implicitly + * water-mark level | a 20x factor, so the range is 0 to 10230, + * | with resolution of 20 steps. + * -------------------------|--------------------------------------------------- + * reset counter | Flag to reset the counted steps. + * -------------------------|--------------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_step_config(struct bmi2_step_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to parse and store the activity recognition + * output from the FIFO data. + * + * @param[out] act_recog : Structure to retrieve output of activity + * recognition frame. + * @param[in] data_index : Index of the FIFO data which contains + * activity recognition frame. + * @param[out] fifo : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t unpack_act_recog_output(struct bmi2_act_recog_output *act_recog, + uint16_t *data_index, + const struct bmi2_fifo_frame *fifo); + +/*! + * @brief This internal API gets the output values of step counter. + * + * @param[out] step_count : Pointer to the stored step counter data. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_step_counter_output(uint32_t *step_count, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets the error status related to NVM. + * + * @param[out] nvm_err_stat : Stores the NVM error status. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_nvm_error_status(struct bmi2_nvm_err_status *nvm_err_stat, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets the error status related to virtual frames. + * + * @param[out] vfrm_err_stat : Stores the VFRM related error status. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_vfrm_error_status(struct bmi2_vfrm_err_status *vfrm_err_stat, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to get enable status of gyroscope user gain + * update. + * + * @param[out] status : Stores status of gyroscope user gain update. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_user_gain_upd_status(uint8_t *status, struct bmi2_dev *dev); + +/*! + * @brief This internal API skips S4S frame in the FIFO data while getting + * activity recognition output. + * + * @param[in, out] frame_header : FIFO frame header. + * @param[in, out] data_index : Index value of the FIFO data bytes + * from which S4S frame header is to be + * skipped. + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t move_if_s4s_frame(const uint8_t *frame_header, uint16_t *data_index, const struct bmi2_fifo_frame *fifo); + +/*! + * @brief This internal API enables/disables compensation of the gain defined + * in the GAIN register. + * + * @param[in] enable : Enables/Disables gain compensation + * @param[in] dev : Structure instance of bmi2_dev. + * + * enable | Description + * -------------|--------------- + * BMI2_ENABLE | Enable gain compensation. + * BMI2_DISABLE | Disable gain compensation. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t enable_gyro_gain(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to extract the output feature configuration + * details like page and start address from the look-up table. + * + * @param[out] feat_output : Structure that stores output feature + * configurations. + * @param[in] type : Type of feature or sensor. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Returns the feature found flag. + * + * @retval BMI2_FALSE : Feature not found + * BMI2_TRUE : Feature found + */ +static uint8_t extract_output_feat_config(struct bmi2_feature_config *feat_output, + uint8_t type, + const struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to move the data index ahead of the + * current frame length parameter when unnecessary FIFO data appears while + * extracting the user specified data. + * + * @param[in,out] data_index : Index of the FIFO data which is to be + * moved ahead of the current frame length + * @param[in] current_frame_length : Number of bytes in the current frame. + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t move_next_frame(uint16_t *data_index, uint8_t current_frame_length, const struct bmi2_fifo_frame *fifo); + +/***************************************************************************/ + +/*! User Interface Definitions + ****************************************************************************/ + +/*! + * @brief This API: + * 1) Updates the device structure with address of the configuration file. + * 2) Initializes BMI270_HUAWEI_CONTEXT sensor. + * 3) Writes the configuration file. + * 4) Updates the feature offset parameters in the device structure. + * 5) Updates the maximum number of pages, in the device structure. + */ +int8_t bmi270_hc_init(struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + /* Assign chip id of BMI270_HUAWEI_CONTEXT */ + dev->chip_id = BMI270_HC_CHIP_ID; + + /* Get the size of config array */ + dev->config_size = sizeof(bmi270_hc_config_file); + + /* Enable the variant specific features if any */ + dev->variant_feature = BMI2_CRT_RTOSK_ENABLE | BMI2_GYRO_CROSS_SENS_ENABLE; + + /* An extra dummy byte is read during SPI read */ + if (dev->intf == BMI2_SPI_INTF) + { + dev->dummy_byte = 1; + } + else + { + dev->dummy_byte = 0; + } + + /* If configuration file pointer is not assigned any address */ + if (!dev->config_file_ptr) + { + /* Give the address of the configuration file array to + * the device pointer + */ + dev->config_file_ptr = bmi270_hc_config_file; + } + + /* Initialize BMI2 sensor */ + rslt = bmi2_sec_init(dev); + if (rslt == BMI2_OK) + { + /* Assign the offsets of the feature input + * configuration to the device structure + */ + dev->feat_config = bmi270_hc_feat_in; + + /* Assign the offsets of the feature output to + * the device structure + */ + dev->feat_output = bmi270_hc_feat_out; + + /* Assign the maximum number of pages to the + * device structure + */ + dev->page_max = BMI270_HC_MAX_PAGE_NUM; + + /* Assign maximum number of input sensors/ + * features to device structure + */ + dev->input_sens = BMI270_HC_MAX_FEAT_IN; + + /* Assign maximum number of output sensors/ + * features to device structure + */ + dev->out_sens = BMI270_HC_MAX_FEAT_OUT; + + /* Assign the offsets of the feature interrupt + * to the device structure + */ + dev->map_int = bmi270_hc_map_int; + + /* Assign maximum number of feature interrupts + * to device structure + */ + dev->sens_int_map = BMI270_HC_MAX_INT_MAP; + } + } + + return rslt; +} + +/*! + * @brief This API selects the sensors/features to be enabled. + */ +int8_t bmi270_hc_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to select sensor */ + uint64_t sensor_sel = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_list != NULL)) + { + /* Get the selected sensors */ + rslt = select_sensor(sens_list, n_sens, &sensor_sel); + if (rslt == BMI2_OK) + { + /* Enable the selected sensors */ + rslt = sensor_enable(sensor_sel, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API selects the sensors/features to be disabled. + */ +int8_t bmi270_hc_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to select sensor */ + uint64_t sensor_sel = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_list != NULL)) + { + /* Get the selected sensors */ + rslt = select_sensor(sens_list, n_sens, &sensor_sel); + if (rslt == BMI2_OK) + { + /* Disable the selected sensors */ + rslt = sensor_disable(sensor_sel, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API sets the sensor/feature configuration. + */ +int8_t bmi270_hc_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_cfg != NULL)) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + + for (loop = 0; loop < n_sens; loop++) + { + if ((sens_cfg[loop].type == BMI2_ACCEL) || (sens_cfg[loop].type == BMI2_GYRO) || + (sens_cfg[loop].type == BMI2_AUX) || (sens_cfg[loop].type == BMI2_GYRO_GAIN_UPDATE)) + { + rslt = bmi2_set_sensor_config(&sens_cfg[loop], 1, dev); + } + else + { + /* Disable Advance power save if enabled for auxiliary + * and feature configurations + */ + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if + * enabled + */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + switch (sens_cfg[loop].type) + { + /* Set the step counter parameters */ + case BMI2_STEP_COUNTER_PARAMS: + rslt = set_step_count_params_config(sens_cfg[loop].cfg.step_counter_params, dev); + break; + + /* Set step counter/detector/activity configuration */ + case BMI2_STEP_DETECTOR: + case BMI2_STEP_COUNTER: + rslt = set_step_config(&sens_cfg[loop].cfg.step_counter, dev); + break; + + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + } + + /* Return error if any of the set configurations fail */ + if (rslt != BMI2_OK) + { + break; + } + } + } + + /* Enable Advance power save if disabled while configuring and + * not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API gets the sensor/feature configuration. + */ +int8_t bmi270_hc_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_cfg != NULL)) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + for (loop = 0; loop < n_sens; loop++) + { + if ((sens_cfg[loop].type == BMI2_ACCEL) || (sens_cfg[loop].type == BMI2_GYRO) || + (sens_cfg[loop].type == BMI2_AUX) || (sens_cfg[loop].type == BMI2_GYRO_GAIN_UPDATE)) + { + rslt = bmi2_get_sensor_config(&sens_cfg[loop], 1, dev); + } + else + { + /* Disable Advance power save if enabled for auxiliary + * and feature configurations + */ + if ((sens_cfg[loop].type >= BMI2_MAIN_SENS_MAX_NUM) || (sens_cfg[loop].type == BMI2_AUX)) + { + + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if + * enabled + */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + } + + if (rslt == BMI2_OK) + { + switch (sens_cfg[loop].type) + { + /* Set the step counter parameters */ + case BMI2_STEP_COUNTER_PARAMS: + rslt = get_step_count_params_config(sens_cfg[loop].cfg.step_counter_params, dev); + break; + + /* Get step counter/detector/activity configuration */ + case BMI2_STEP_DETECTOR: + case BMI2_STEP_COUNTER: + rslt = get_step_config(&sens_cfg[loop].cfg.step_counter, dev); + break; + + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + } + + /* Return error if any of the get configurations fail */ + if (rslt != BMI2_OK) + { + break; + } + } + } + + /* Enable Advance power save if disabled while configuring and + * not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API gets the sensor/feature data for accelerometer, gyroscope, + * auxiliary sensor, step counter, high-g, gyroscope user-gain update, + * orientation, gyroscope cross sensitivity and error status for NVM and VFRM. + */ +int8_t bmi270_hc_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sensor_data != NULL)) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + for (loop = 0; loop < n_sens; loop++) + { + if ((sensor_data[loop].type == BMI2_ACCEL) || (sensor_data[loop].type == BMI2_GYRO) || + (sensor_data[loop].type == BMI2_AUX) || (sensor_data[loop].type == BMI2_GYRO_GAIN_UPDATE) || + (sensor_data[loop].type == BMI2_GYRO_CROSS_SENSE)) + { + rslt = bmi2_get_sensor_data(&sensor_data[loop], 1, dev); + } + else + { + /* Disable Advance power save if enabled for feature + * configurations + */ + if (sensor_data[loop].type >= BMI2_MAIN_SENS_MAX_NUM) + { + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if + * enabled + */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + } + + if (rslt == BMI2_OK) + { + switch (sensor_data[loop].type) + { + case BMI2_STEP_COUNTER: + + /* Get step counter output */ + rslt = get_step_counter_output(&sensor_data[loop].sens_data.step_counter_output, dev); + break; + case BMI2_NVM_STATUS: + + /* Get NVM error status */ + rslt = get_nvm_error_status(&sensor_data[loop].sens_data.nvm_status, dev); + break; + case BMI2_VFRM_STATUS: + + /* Get VFRM error status */ + rslt = get_vfrm_error_status(&sensor_data[loop].sens_data.vfrm_status, dev); + break; + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + + /* Return error if any of the get sensor data fails */ + if (rslt != BMI2_OK) + { + break; + } + } + } + + /* Enable Advance power save if disabled while + * configuring and not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This api is used for retrieving the activity recognition settings currently set for bmi270hc. + */ +int8_t bmi270_hc_get_act_recg_sett(struct bmi2_hc_act_recg_sett *sett, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat; + + /* Variable to set flag */ + uint8_t feat_found; + uint16_t msb_lsb; + uint8_t lsb; + uint8_t msb; + + /* Initialize feature configuration for activity recognition */ + struct bmi2_feature_config bmi2_act_recg_sett = { 0, 0, 0 }; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + /* Search for activity recognition feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&bmi2_act_recg_sett, BMI2_ACTIVITY_RECOGNITION_SETTINGS, dev); + if (feat_found) + { + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if enabled */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + /* Get the configuration from the page where activity recognition setting feature resides */ + if (rslt == BMI2_OK) + { + rslt = bmi2_get_feat_config(bmi2_act_recg_sett.page, feat_config, dev); + } + + if (rslt == BMI2_OK) + { + /* Define the offset in bytes */ + idx = bmi2_act_recg_sett.start_addr; + + lsb = feat_config[idx++]; + msb = feat_config[idx++]; + msb_lsb = ((uint16_t)msb << 8) | lsb; + + sett->segment_size = msb_lsb & BMI2_HC_ACT_RECG_SEGMENT_SIZE_MASK; + + /* Increment idx by 2 to point post processing enable/disable address */ + idx = 4; + lsb = feat_config[idx++]; + msb = feat_config[idx++]; + msb_lsb = ((uint16_t)msb << 8) | lsb; + sett->pp_en = msb_lsb & BMI2_HC_ACT_RECG_PP_EN_MASK; + + /* Increment idx by 2 to point mix gdi thres addres */ + idx = 6; + lsb = feat_config[idx++]; + msb = feat_config[idx++]; + msb_lsb = ((uint16_t)msb << 8) | lsb; + sett->min_gdi_thres = msb_lsb & BMI2_HC_ACT_RECG_MIN_GDI_THRES_MASK; + + /* Increment idx by 2 to point max gdi thres addres */ + idx = 8; + lsb = feat_config[idx++]; + msb = feat_config[idx++]; + msb_lsb = ((uint16_t)msb << 8) | lsb; + sett->max_gdi_thres = msb_lsb & BMI2_HC_ACT_RECG_MAX_GDI_THRES_MASK; + + /* Increment idx by 2 to point to buffer size */ + idx = 10; + lsb = feat_config[idx++]; + msb = feat_config[idx++]; + msb_lsb = ((uint16_t)msb << 8) | lsb; + sett->buf_size = msb_lsb & BMI2_HC_ACT_RECG_BUF_SIZE_MASK; + + /* Increment idx by 2 to to point to min segment confidence */ + idx = 12; + lsb = feat_config[idx++]; + msb = feat_config[idx++]; + msb_lsb = ((uint16_t)msb << 8) | lsb; + sett->min_seg_conf = msb_lsb & BMI2_HC_ACT_RECG_MIN_SEG_CONF_MASK; + } + + /* Enable Advance power save if disabled while + * configuring and not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + } + + return rslt; +} + +/*! + * @brief This api is used for setting the activity recognition settings for bmi270hc. + */ +int8_t bmi270_hc_set_act_recg_sett(const struct bmi2_hc_act_recg_sett *sett, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Variable to define index */ + uint8_t index = 0; + + /* Initialize feature configuration for activity recognition */ + struct bmi2_feature_config bmi2_act_recg_sett = { 0, 0, 0 }; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + /* Search for activity recognition feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&bmi2_act_recg_sett, BMI2_ACTIVITY_RECOGNITION_SETTINGS, dev); + if (feat_found) + { + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if enabled */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + /* Get the configuration from the page where activity recognition setting feature resides */ + rslt = bmi2_get_feat_config(bmi2_act_recg_sett.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes */ + idx = bmi2_act_recg_sett.start_addr; + + /* Get offset in words since all the features are set in words length */ + idx = idx / 2; + + /* Set segment size */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), + BMI2_HC_ACT_RECG_SEGMENT_SIZE, + sett->segment_size); + + /* Starting address of post processing represented in word length */ + idx = 2; + + /* Set post processing */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_HC_ACT_RECG_PP_EN, sett->pp_en); + + /* Starting address of min_gdi_thres represented in word length */ + idx = 3; + + /* Set minimum gdi threshold */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), + BMI2_HC_ACT_RECG_MIN_GDI_THRES, + sett->min_gdi_thres); + + /* Starting address of max_gdi_thres represented in word length */ + idx = 4; + + /* Set maximum gdi threshold */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), + BMI2_HC_ACT_RECG_MAX_GDI_THRES, + sett->max_gdi_thres); + + /* Starting address of buffer size represented in word length */ + idx = 5; + + /* Set buffer size */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_HC_ACT_RECG_BUF_SIZE, sett->buf_size); + + /* Starting address of min_seg_conf represented in word length */ + idx = 6; + + /* Set min segment confidence */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), + BMI2_HC_ACT_RECG_MIN_SEG_CONF, + sett->min_seg_conf); + + /* Increment offset by 1 more word to get the total length in words */ + idx++; + + /* Get total length in bytes to copy from local pointer to the array */ + idx = (uint8_t)(idx * 2) - bmi2_act_recg_sett.start_addr; + + /* Copy the bytes to be set back to the array */ + for (index = 0; index <= idx; index++) + { + feat_config[bmi2_act_recg_sett.start_addr + + index] = *((uint8_t *) data_p + bmi2_act_recg_sett.start_addr + index); + } + + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + + /* Enable Advance power save if disabled while + * configuring and not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + } + + return rslt; +} + +/*! + * @brief This internal API is used to parse the activity output from the + * FIFO in header mode. + */ +int8_t bmi270_hc_get_act_recog_output(struct bmi2_act_recog_output *act_recog, + uint16_t *act_frm_len, + struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define header frame */ + uint8_t frame_header = 0; + + /* Variable to index the data bytes */ + uint16_t data_index; + + /* Variable to index activity frames */ + uint16_t act_idx = 0; + + /* Variable to indicate activity frames read */ + uint16_t frame_to_read = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (act_recog != NULL) && (act_frm_len != NULL) && (fifo != NULL)) + { + + /* Store the number of frames to be read */ + frame_to_read = *act_frm_len; + for (data_index = fifo->act_recog_byte_start_idx; data_index < fifo->length;) + { + /* Get frame header byte */ + frame_header = fifo->data[data_index] & BMI2_FIFO_TAG_INTR_MASK; + + /* Skip S4S frames if S4S is enabled */ + rslt = move_if_s4s_frame(&frame_header, &data_index, fifo); + + /* Break if FIFO is empty */ + if (rslt == BMI2_W_FIFO_EMPTY) + { + break; + } + + /* Index shifted to next byte where data starts */ + data_index++; + switch (frame_header) + { + /* If header defines accelerometer frame */ + case BMI2_FIFO_HEADER_ACC_FRM: + rslt = move_next_frame(&data_index, fifo->acc_frm_len, fifo); + break; + + /* If header defines accelerometer and auxiliary frames */ + case BMI2_FIFO_HEADER_AUX_ACC_FRM: + rslt = move_next_frame(&data_index, fifo->acc_aux_frm_len, fifo); + break; + + /* If header defines accelerometer and gyroscope frames */ + case BMI2_FIFO_HEADER_GYR_ACC_FRM: + rslt = move_next_frame(&data_index, fifo->acc_gyr_frm_len, fifo); + break; + + /* If header defines accelerometer, auxiliary and gyroscope frames */ + case BMI2_FIFO_HEADER_ALL_FRM: + rslt = move_next_frame(&data_index, fifo->all_frm_len, fifo); + break; + + /* If header defines only gyroscope frame */ + case BMI2_FIFO_HEADER_GYR_FRM: + rslt = move_next_frame(&data_index, fifo->gyr_frm_len, fifo); + break; + + /* If header defines only auxiliary frame */ + case BMI2_FIFO_HEADER_AUX_FRM: + rslt = move_next_frame(&data_index, fifo->aux_frm_len, fifo); + break; + + /* If header defines auxiliary and gyroscope frame */ + case BMI2_FIFO_HEADER_AUX_GYR_FRM: + rslt = move_next_frame(&data_index, fifo->aux_gyr_frm_len, fifo); + break; + + /* If header defines sensor time frame */ + case BMI2_FIFO_HEADER_SENS_TIME_FRM: + rslt = move_next_frame(&data_index, BMI2_SENSOR_TIME_LENGTH, fifo); + break; + + /* If header defines skip frame */ + case BMI2_FIFO_HEADER_SKIP_FRM: + rslt = move_next_frame(&data_index, BMI2_FIFO_SKIP_FRM_LENGTH, fifo); + break; + + /* If header defines Input configuration frame */ + case BMI2_FIFO_HEADER_INPUT_CFG_FRM: + rslt = move_next_frame(&data_index, BMI2_FIFO_INPUT_CFG_LENGTH, fifo); + break; + + /* If header defines invalid frame or end of valid data */ + case BMI2_FIFO_HEAD_OVER_READ_MSB: + + /* Move the data index to the last byte to mark completion */ + data_index = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + + /* If header defines activity recognition frame */ + case BMI2_FIFO_VIRT_ACT_RECOG_FRM: + + /* Get the activity output */ + rslt = unpack_act_recog_output(&act_recog[(act_idx)], &data_index, fifo); + + /* Update activity frame index */ + (act_idx)++; + break; + default: + + /* Move the data index to the last byte in case of invalid values */ + data_index = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + } + + /* Number of frames to be read is complete or FIFO is empty */ + if ((frame_to_read == act_idx) || (rslt == BMI2_W_FIFO_EMPTY)) + { + break; + } + } + + /* Update the activity frame index */ + (*act_frm_len) = act_idx; + + /* Update the activity byte index */ + fifo->act_recog_byte_start_idx = data_index; + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API updates the gyroscope user-gain. + */ +int8_t bmi270_hc_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to select sensor */ + uint8_t sens_sel[2] = { BMI2_GYRO, BMI2_GYRO_GAIN_UPDATE }; + + /* Structure to define sensor configurations */ + struct bmi2_sens_config sens_cfg; + + /* Variable to store status of user-gain update module */ + uint8_t status = 0; + + /* Variable to define count */ + uint8_t count = 100; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (user_gain != NULL)) + { + /* Select type of feature */ + sens_cfg.type = BMI2_GYRO_GAIN_UPDATE; + + /* Get the user gain configurations */ + rslt = bmi270_hc_get_sensor_config(&sens_cfg, 1, dev); + if (rslt == BMI2_OK) + { + /* Get the user-defined ratio */ + sens_cfg.cfg.gyro_gain_update = *user_gain; + + /* Set rate ratio for all axes */ + rslt = bmi270_hc_set_sensor_config(&sens_cfg, 1, dev); + } + + /* Disable gyroscope */ + if (rslt == BMI2_OK) + { + rslt = bmi270_hc_sensor_disable(&sens_sel[0], 1, dev); + } + + /* Enable gyroscope user-gain update module */ + if (rslt == BMI2_OK) + { + rslt = bmi270_hc_sensor_enable(&sens_sel[1], 1, dev); + } + + /* Set the command to trigger the computation */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_command_register(BMI2_USR_GAIN_CMD, dev); + } + + if (rslt == BMI2_OK) + { + /* Poll until enable bit of user-gain update is 0 */ + while (count--) + { + rslt = get_user_gain_upd_status(&status, dev); + if ((rslt == BMI2_OK) && (status == 0)) + { + /* Enable compensation of gain defined + * in the GAIN register + */ + rslt = enable_gyro_gain(BMI2_ENABLE, dev); + + /* Enable gyroscope */ + if (rslt == BMI2_OK) + { + rslt = bmi270_hc_sensor_enable(&sens_sel[0], 1, dev); + } + + break; + } + + dev->delay_us(10000, dev->intf_ptr); + } + + /* Return error if user-gain update is failed */ + if ((rslt == BMI2_OK) && (status != 0)) + { + rslt = BMI2_E_GYR_USER_GAIN_UPD_FAIL; + } + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API reads the compensated gyroscope user-gain values. + */ +int8_t bmi270_hc_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define register data */ + uint8_t reg_data[3] = { 0 }; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (gyr_usr_gain != NULL)) + { + /* Get the gyroscope compensated gain values */ + rslt = bmi2_get_regs(BMI2_GYR_USR_GAIN_0_ADDR, reg_data, 3, dev); + if (rslt == BMI2_OK) + { + /* Gyroscope user gain correction X-axis */ + gyr_usr_gain->x = (int8_t)BMI2_GET_BIT_POS0(reg_data[0], BMI2_GYR_USR_GAIN_X); + + /* Gyroscope user gain correction Y-axis */ + gyr_usr_gain->y = (int8_t)BMI2_GET_BIT_POS0(reg_data[1], BMI2_GYR_USR_GAIN_Y); + + /* Gyroscope user gain correction z-axis */ + gyr_usr_gain->z = (int8_t)BMI2_GET_BIT_POS0(reg_data[2], BMI2_GYR_USR_GAIN_Z); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API maps/unmaps feature interrupts to that of interrupt pins. + */ +int8_t bmi270_hc_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_int != NULL)) + { + for (loop = 0; loop < n_sens; loop++) + { + switch (sens_int[loop].type) + { + case BMI2_STEP_COUNTER: + case BMI2_STEP_DETECTOR: + + rslt = bmi2_map_feat_int(sens_int[loop].type, sens_int[loop].hw_int_pin, dev); + break; + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + + /* Return error if interrupt mapping fails */ + if (rslt != BMI2_OK) + { + break; + } + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/***************************************************************************/ + +/*! Local Function Definitions + ****************************************************************************/ + +/*! + * @brief This internal API is used to validate the device structure pointer for + * null conditions. + */ +static int8_t null_ptr_check(const struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + if ((dev == NULL) || (dev->read == NULL) || (dev->write == NULL) || (dev->delay_us == NULL)) + { + /* Device structure pointer is not valid */ + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This internal API selects the sensor/features to be enabled or + * disabled. + */ +static int8_t select_sensor(const uint8_t *sens_list, uint8_t n_sens, uint64_t *sensor_sel) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Variable to define loop */ + uint8_t count; + + for (count = 0; count < n_sens; count++) + { + switch (sens_list[count]) + { + case BMI2_ACCEL: + *sensor_sel |= BMI2_ACCEL_SENS_SEL; + break; + case BMI2_GYRO: + *sensor_sel |= BMI2_GYRO_SENS_SEL; + break; + case BMI2_AUX: + *sensor_sel |= BMI2_AUX_SENS_SEL; + break; + case BMI2_TEMP: + *sensor_sel |= BMI2_TEMP_SENS_SEL; + break; + case BMI2_STEP_DETECTOR: + *sensor_sel |= BMI2_STEP_DETECT_SEL; + break; + case BMI2_STEP_COUNTER: + *sensor_sel |= BMI2_STEP_COUNT_SEL; + break; + case BMI2_GYRO_GAIN_UPDATE: + *sensor_sel |= BMI2_GYRO_GAIN_UPDATE_SEL; + break; + case BMI2_ACTIVITY_RECOGNITION: + *sensor_sel |= BMI2_ACTIVITY_RECOGNITION_SEL; + break; + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + } + + return rslt; +} + +/*! + * @brief This internal API enables the selected sensor/features. + */ +static int8_t sensor_enable(uint64_t sensor_sel, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store register values */ + uint8_t reg_data = 0; + + /* Variable to define loop */ + uint8_t loop = 1; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + /* Enable accelerometer */ + if (sensor_sel & BMI2_ACCEL_SENS_SEL) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_ACC_EN, BMI2_ENABLE); + } + + /* Enable gyroscope */ + if (sensor_sel & BMI2_GYRO_SENS_SEL) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_EN, BMI2_ENABLE); + } + + /* Enable auxiliary sensor */ + if (sensor_sel & BMI2_AUX_SENS_SEL) + { + reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_AUX_EN, BMI2_ENABLE); + } + + /* Enable temperature sensor */ + if (sensor_sel & BMI2_TEMP_SENS_SEL) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_TEMP_EN, BMI2_ENABLE); + } + + /* Enable the sensors that are set in the power control register */ + if (sensor_sel & BMI2_MAIN_SENSORS) + { + rslt = bmi2_set_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + } + } + + if ((rslt == BMI2_OK) && (sensor_sel & ~(BMI2_MAIN_SENSORS))) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if enabled */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + while (loop--) + { + /* Enable step detector feature */ + if (sensor_sel & BMI2_STEP_DETECT_SEL) + { + rslt = set_step_detector(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_STEP_DETECT_SEL; + } + else + { + break; + } + } + + /* Enable step counter feature */ + if (sensor_sel & BMI2_STEP_COUNT_SEL) + { + rslt = set_step_counter(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_STEP_COUNT_SEL; + } + else + { + break; + } + } + + /* Enable activity recognition feature */ + if (sensor_sel & BMI2_ACTIVITY_RECOGNITION_SEL) + { + rslt = set_act_recog(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_ACTIVITY_RECOGNITION_SEL; + } + else + { + break; + } + } + + /* Enable gyroscope user gain */ + if (sensor_sel & BMI2_GYRO_GAIN_UPDATE_SEL) + { + rslt = set_gyro_user_gain(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_GYRO_GAIN_UPDATE_SEL; + } + else + { + break; + } + } + } + + /* Enable Advance power save if disabled while + * configuring and not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + } + + return rslt; +} + +/*! + * @brief This internal API disables the selected sensors/features. + */ +static int8_t sensor_disable(uint64_t sensor_sel, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store register values */ + uint8_t reg_data = 0; + + /* Variable to define loop */ + uint8_t loop = 1; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + /* Disable accelerometer */ + if (sensor_sel & BMI2_ACCEL_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_ACC_EN); + } + + /* Disable gyroscope */ + if (sensor_sel & BMI2_GYRO_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_GYR_EN); + } + + /* Disable auxiliary sensor */ + if (sensor_sel & BMI2_AUX_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_AUX_EN); + } + + /* Disable temperature sensor */ + if (sensor_sel & BMI2_TEMP_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_TEMP_EN); + } + + /* Disable the sensors that are set in the power control register */ + if (sensor_sel & BMI2_MAIN_SENSORS) + { + rslt = bmi2_set_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + } + } + + if ((rslt == BMI2_OK) && (sensor_sel & ~(BMI2_MAIN_SENSORS))) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if enabled */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + while (loop--) + { + /* Disable step detector feature */ + if (sensor_sel & BMI2_STEP_DETECT_SEL) + { + rslt = set_step_detector(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_STEP_DETECT_SEL; + } + else + { + break; + } + } + + /* Disable step counter feature */ + if (sensor_sel & BMI2_STEP_COUNT_SEL) + { + rslt = set_step_counter(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_STEP_COUNT_SEL; + } + else + { + break; + } + } + + /* Disable activity recognition feature */ + if (sensor_sel & BMI2_ACTIVITY_RECOGNITION_SEL) + { + rslt = set_act_recog(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_ACTIVITY_RECOGNITION_SEL; + } + else + { + break; + } + } + + /* Disable gyroscope user gain */ + if (sensor_sel & BMI2_GYRO_GAIN_UPDATE_SEL) + { + rslt = set_gyro_user_gain(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_GYRO_GAIN_UPDATE_SEL; + } + else + { + break; + } + } + + /* Enable Advance power save if disabled while + * configuring and not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + } + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable step detector feature. + */ +static int8_t set_step_detector(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step detector */ + struct bmi2_feature_config step_det_config = { 0, 0, 0 }; + + /* Search for step detector feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_det_config, BMI2_STEP_DETECTOR, dev); + if (feat_found) + { + /* Get the configuration from the page where step detector feature resides */ + rslt = bmi2_get_feat_config(step_det_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of step detector */ + idx = step_det_config.start_addr + BMI2_STEP_COUNT_FEAT_EN_OFFSET; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_STEP_DET_FEAT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable step counter feature. + */ +static int8_t set_step_counter(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step counter */ + struct bmi2_feature_config step_count_config = { 0, 0, 0 }; + + /* Search for step counter feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); + if (feat_found) + { + /* Get the configuration from the page where step-counter feature resides */ + rslt = bmi2_get_feat_config(step_count_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of step counter */ + idx = step_count_config.start_addr + BMI2_STEP_COUNT_FEAT_EN_OFFSET; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_STEP_COUNT_FEAT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! +* @brief This internal API enables/disables the activity recognition feature. +*/ +static int8_t set_act_recog(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for activity recognition */ + struct bmi2_feature_config act_recog_cfg = { 0, 0, 0 }; + + /* Search for activity recognition and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&act_recog_cfg, BMI2_ACTIVITY_RECOGNITION, dev); + if (feat_found) + { + /* Get the configuration from the page where activity + * recognition feature resides + */ + rslt = bmi2_get_feat_config(act_recog_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of activity recognition */ + idx = act_recog_cfg.start_addr; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], BMI2_ACTIVITY_RECOG_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable gyroscope user gain + * feature. + */ +static int8_t set_gyro_user_gain(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for gyroscope user gain */ + struct bmi2_feature_config gyr_user_gain_cfg = { 0, 0, 0 }; + + /* Search for user gain feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&gyr_user_gain_cfg, BMI2_GYRO_GAIN_UPDATE, dev); + if (feat_found) + { + /* Get the configuration from the page where user gain feature resides */ + rslt = bmi2_get_feat_config(gyr_user_gain_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of user gain */ + idx = gyr_user_gain_cfg.start_addr + BMI2_GYR_USER_GAIN_FEAT_EN_OFFSET; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_GYR_USER_GAIN_FEAT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API sets step counter parameter configurations. + */ +static int8_t set_step_count_params_config(const uint16_t *step_count_params, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define index */ + uint8_t index = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step counter parameters */ + struct bmi2_feature_config step_params_config = { 0, 0, 0 }; + + /* Variable to index the page number */ + uint8_t page_idx; + + /* Variable to define the start page */ + uint8_t start_page; + + /* Variable to define start address of the parameters */ + uint8_t start_addr; + + /* Variable to define number of bytes */ + uint8_t n_bytes = (BMI2_STEP_CNT_N_PARAMS * 2); + + /* Variable to store number of pages */ + uint8_t n_pages = (n_bytes / 16); + + /* Variable to define the end page */ + uint8_t end_page; + + /* Variable to define the remaining bytes to be read */ + uint8_t remain_len; + + /* Variable to define the maximum words(16 bytes or 8 words) to be read in a page */ + uint8_t max_len = 8; + + /* Variable index bytes in a page */ + uint8_t page_byte_idx; + + /* Variable to index the parameters */ + uint8_t param_idx = 0; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for step counter parameter feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_params_config, BMI2_STEP_COUNTER_PARAMS, dev); + if (feat_found) + { + /* Get the start page for the step counter parameters */ + start_page = step_params_config.page; + + /* Get the end page for the step counter parameters */ + end_page = start_page + n_pages; + + /* Get the start address for the step counter parameters */ + start_addr = step_params_config.start_addr; + + /* Get the remaining length of bytes to be read */ + remain_len = (uint8_t)((n_bytes - (n_pages * 16)) + start_addr); + for (page_idx = start_page; page_idx <= end_page; page_idx++) + { + /* Get the configuration from the respective page */ + rslt = bmi2_get_feat_config(page_idx, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Start from address 0x00 when switched to next page */ + if (page_idx > start_page) + { + start_addr = 0; + } + + /* Remaining number of words to be read in the page */ + if (page_idx == end_page) + { + max_len = (remain_len / 2); + } + + /* Get offset in words since all the features are set in words length */ + page_byte_idx = start_addr / 2; + for (; page_byte_idx < max_len;) + { + /* Set parameters 1 to 25 */ + *(data_p + page_byte_idx) = BMI2_SET_BIT_POS0(*(data_p + page_byte_idx), + BMI2_STEP_COUNT_PARAMS, + step_count_params[param_idx]); + + /* Increment offset by 1 word to set to the next parameter */ + page_byte_idx++; + + /* Increment to next parameter */ + param_idx++; + } + + /* Get total length in bytes to copy from local pointer to the array */ + page_byte_idx = (uint8_t)(page_byte_idx * 2) - step_params_config.start_addr; + + /* Copy the bytes to be set back to the array */ + for (index = 0; index < page_byte_idx; index++) + { + feat_config[step_params_config.start_addr + + index] = *((uint8_t *) data_p + step_params_config.start_addr + index); + } + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/* @brief This internal API sets step counter configurations like water-mark + * level, reset-counter and output-configuration step detector and activity. + */ +static int8_t set_step_config(const struct bmi2_step_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define index */ + uint8_t index = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step counter 4 */ + struct bmi2_feature_config step_count_config = { 0, 0, 0 }; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for step counter feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); + if (feat_found) + { + /* Get the configuration from the page where step counter resides */ + rslt = bmi2_get_feat_config(step_count_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes */ + idx = step_count_config.start_addr; + + /* Get offset in words since all the features are set in words length */ + idx = idx / 2; + + /* Set water-mark level */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_STEP_COUNT_WM_LEVEL, config->watermark_level); + + /* Set reset-counter */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_STEP_COUNT_RST_CNT, config->reset_counter); + + /* Increment offset by 1 word to set output + * configuration of step detector and step activity + */ + idx++; + + /* Set step buffer size */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_STEP_BUFFER_SIZE, config->step_buffer_size); + + /* Increment offset by 1 more word to get the total length in words */ + idx++; + + /* Get total length in bytes to copy from local pointer to the array */ + idx = (uint8_t)(idx * 2) - step_count_config.start_addr; + + /* Copy the bytes to be set back to the array */ + for (index = 0; index < idx; index++) + { + feat_config[step_count_config.start_addr + + index] = *((uint8_t *) data_p + step_count_config.start_addr + index); + } + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets step counter parameter configurations. + */ +static int8_t get_step_count_params_config(uint16_t *step_count_params, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Variable to define LSB */ + uint16_t lsb = 0; + + /* Variable to define MSB */ + uint16_t msb = 0; + + /* Variable to define a word */ + uint16_t lsb_msb = 0; + + /* Initialize feature configuration for step counter 1 */ + struct bmi2_feature_config step_params_config = { 0, 0, 0 }; + + /* Variable to index the page number */ + uint8_t page_idx; + + /* Variable to define the start page */ + uint8_t start_page; + + /* Variable to define start address of the parameters */ + uint8_t start_addr; + + /* Variable to define number of bytes */ + uint8_t n_bytes = (BMI2_STEP_CNT_N_PARAMS * 2); + + /* Variable to store number of pages */ + uint8_t n_pages = (n_bytes / 16); + + /* Variable to define the end page */ + uint8_t end_page; + + /* Variable to define the remaining bytes to be read */ + uint8_t remain_len; + + /* Variable to define the maximum words to be read in a page */ + uint8_t max_len = BMI2_FEAT_SIZE_IN_BYTES; + + /* Variable index bytes in a page */ + uint8_t page_byte_idx; + + /* Variable to index the parameters */ + uint8_t param_idx = 0; + + /* Search for step counter parameter feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_params_config, BMI2_STEP_COUNTER_PARAMS, dev); + if (feat_found) + { + /* Get the start page for the step counter parameters */ + start_page = step_params_config.page; + + /* Get the end page for the step counter parameters */ + end_page = start_page + n_pages; + + /* Get the start address for the step counter parameters */ + start_addr = step_params_config.start_addr; + + /* Get the remaining length of bytes to be read */ + remain_len = (uint8_t)((n_bytes - (n_pages * 16)) + start_addr); + for (page_idx = start_page; page_idx <= end_page; page_idx++) + { + /* Get the configuration from the respective page */ + rslt = bmi2_get_feat_config(page_idx, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Start from address 0x00 when switched to next page */ + if (page_idx > start_page) + { + start_addr = 0; + } + + /* Remaining number of bytes to be read in the page */ + if (page_idx == end_page) + { + max_len = remain_len; + } + + /* Get the offset */ + page_byte_idx = start_addr; + while (page_byte_idx < max_len) + { + /* Get word to calculate the parameter*/ + lsb = (uint16_t) feat_config[page_byte_idx++]; + if (page_byte_idx < max_len) + { + msb = ((uint16_t) feat_config[page_byte_idx++] << 8); + } + + lsb_msb = lsb | msb; + + /* Get parameters 1 to 25 */ + step_count_params[param_idx] = lsb_msb & BMI2_STEP_COUNT_PARAMS_MASK; + + /* Increment to next parameter */ + param_idx++; + } + } + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets step counter/detector/activity configurations. + */ +static int8_t get_step_config(struct bmi2_step_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define LSB */ + uint16_t lsb = 0; + + /* Variable to define MSB */ + uint16_t msb = 0; + + /* Variable to define a word */ + uint16_t lsb_msb = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step counter */ + struct bmi2_feature_config step_count_config = { 0, 0, 0 }; + + /* Search for step counter 4 feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); + if (feat_found) + { + /* Get the configuration from the page where step counter 4 parameter resides */ + rslt = bmi2_get_feat_config(step_count_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for feature enable for step counter/detector/activity */ + idx = step_count_config.start_addr; + + /* Get word to calculate water-mark level and reset counter */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get water-mark level */ + config->watermark_level = lsb_msb & BMI2_STEP_COUNT_WM_LEVEL_MASK; + + /* Get reset counter */ + config->reset_counter = (lsb_msb & BMI2_STEP_COUNT_RST_CNT_MASK) >> BMI2_STEP_COUNT_RST_CNT_POS; + + /* Get word to calculate output configuration of step detector and activity */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + config->step_buffer_size = (lsb_msb & BMI2_STEP_BUFFER_SIZE_MASK) >> BMI2_STEP_BUFFER_SIZE_POS; + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets the output values of step counter. + */ +static int8_t get_step_counter_output(uint32_t *step_count, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variables to define index */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature output for step counter */ + struct bmi2_feature_config step_cnt_out_config = { 0, 0, 0 }; + + /* Search for step counter output feature and extract its configuration details */ + feat_found = extract_output_feat_config(&step_cnt_out_config, BMI2_STEP_COUNTER, dev); + if (feat_found) + { + /* Get the feature output configuration for step-counter */ + rslt = bmi2_get_feat_config(step_cnt_out_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for step counter output */ + idx = step_cnt_out_config.start_addr; + + /* Get the step counter output in 4 bytes */ + *step_count = (uint32_t) feat_config[idx++]; + *step_count |= ((uint32_t) feat_config[idx++] << 8); + *step_count |= ((uint32_t) feat_config[idx++] << 16); + *step_count |= ((uint32_t) feat_config[idx++] << 24); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets the error status related to NVM. + */ +static int8_t get_nvm_error_status(struct bmi2_nvm_err_status *nvm_err_stat, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variables to define index */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature output for NVM error status */ + struct bmi2_feature_config nvm_err_cfg = { 0, 0, 0 }; + + /* Search for NVM error status feature and extract its configuration details */ + feat_found = extract_output_feat_config(&nvm_err_cfg, BMI2_NVM_STATUS, dev); + if (feat_found) + { + /* Get the feature output configuration for NVM error status */ + rslt = bmi2_get_feat_config(nvm_err_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for NVM error status */ + idx = nvm_err_cfg.start_addr; + + /* Increment index to get the error status */ + idx++; + + /* Error when NVM load action fails */ + nvm_err_stat->load_error = BMI2_GET_BIT_POS0(feat_config[idx], BMI2_NVM_LOAD_ERR_STATUS); + + /* Error when NVM program action fails */ + nvm_err_stat->prog_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_PROG_ERR_STATUS); + + /* Error when NVM erase action fails */ + nvm_err_stat->erase_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_ERASE_ERR_STATUS); + + /* Error when NVM program limit is exceeded */ + nvm_err_stat->exceed_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_END_EXCEED_STATUS); + + /* Error when NVM privilege mode is not acquired */ + nvm_err_stat->privil_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_PRIV_ERR_STATUS); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to get enable status of gyroscope user gain + * update. + */ +static int8_t get_user_gain_upd_status(uint8_t *status, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Variable to check APS status */ + uint8_t aps_stat = 0; + + /* Initialize feature configuration for gyroscope user gain */ + struct bmi2_feature_config gyr_user_gain_cfg = { 0, 0, 0 }; + + /* Search for user gain feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&gyr_user_gain_cfg, BMI2_GYRO_GAIN_UPDATE, dev); + if (feat_found) + { + /* Disable advance power save */ + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + /* Get the configuration from the page where user gain feature resides */ + rslt = bmi2_get_feat_config(gyr_user_gain_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of user gain */ + idx = gyr_user_gain_cfg.start_addr + BMI2_GYR_USER_GAIN_FEAT_EN_OFFSET; + + /* Set the feature enable status */ + *status = BMI2_GET_BITS(feat_config[idx], BMI2_GYR_USER_GAIN_FEAT_EN); + } + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + /* Enable Advance power save if disabled while configuring and not when already disabled */ + if ((rslt == BMI2_OK) && (aps_stat == BMI2_ENABLE)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + + return rslt; +} + +/*! + * @brief This internal API is used to parse and store the activity recognition + * output from the FIFO data. + */ +static int8_t unpack_act_recog_output(struct bmi2_act_recog_output *act_recog, + uint16_t *data_index, + const struct bmi2_fifo_frame *fifo) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Variables to define 4 bytes of sensor time */ + uint32_t time_stamp_byte4 = 0; + uint32_t time_stamp_byte3 = 0; + uint32_t time_stamp_byte2 = 0; + uint32_t time_stamp_byte1 = 0; + + /* Validate data index */ + if ((*data_index + BMI2_FIFO_VIRT_ACT_DATA_LENGTH) >= fifo->length) + { + /* Update the data index to the last byte */ + (*data_index) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + } + else + { + /* Get time-stamp from the activity recognition frame */ + time_stamp_byte4 = ((uint32_t)(fifo->data[(*data_index) + 3]) << 24); + time_stamp_byte3 = ((uint32_t)(fifo->data[(*data_index) + 2]) << 16); + time_stamp_byte2 = fifo->data[(*data_index) + 1] << 8; + time_stamp_byte1 = fifo->data[(*data_index)]; + + /* Update time-stamp from the virtual frame */ + act_recog->time_stamp = (time_stamp_byte4 | time_stamp_byte3 | time_stamp_byte2 | time_stamp_byte1); + + /* Move the data index by 4 bytes */ + (*data_index) = (*data_index) + BMI2_FIFO_VIRT_ACT_TIME_LENGTH; + + /* Update the previous activity from the virtual frame */ + act_recog->prev_act = fifo->data[(*data_index)]; + + /* Move the data index by 1 byte */ + (*data_index) = (*data_index) + BMI2_FIFO_VIRT_ACT_TYPE_LENGTH; + + /* Update the current activity from the virtual frame */ + act_recog->curr_act = fifo->data[(*data_index)]; + + /* Move the data index by 1 byte */ + (*data_index) = (*data_index) + BMI2_FIFO_VIRT_ACT_STAT_LENGTH; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + } + + return rslt; +} + +/*! + * @brief This internal API gets the error status related to virtual frames. + */ +static int8_t get_vfrm_error_status(struct bmi2_vfrm_err_status *vfrm_err_stat, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variables to define index */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature output for VFRM error status */ + struct bmi2_feature_config vfrm_err_cfg = { 0, 0, 0 }; + + /* Search for VFRM error status feature and extract its configuration details */ + feat_found = extract_output_feat_config(&vfrm_err_cfg, BMI2_VFRM_STATUS, dev); + if (feat_found) + { + /* Get the feature output configuration for VFRM error status */ + rslt = bmi2_get_feat_config(vfrm_err_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for VFRM error status */ + idx = vfrm_err_cfg.start_addr; + + /* Increment index to get the error status */ + idx++; + + /* Internal error while acquiring lock for FIFO */ + vfrm_err_stat->lock_error = BMI2_GET_BITS(feat_config[idx], BMI2_VFRM_LOCK_ERR_STATUS); + + /* Internal error while writing byte into FIFO */ + vfrm_err_stat->write_error = BMI2_GET_BITS(feat_config[idx], BMI2_VFRM_WRITE_ERR_STATUS); + + /* Internal error while writing into FIFO */ + vfrm_err_stat->fatal_error = BMI2_GET_BITS(feat_config[idx], BMI2_VFRM_FATAL_ERR_STATUS); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API skips S4S frame in the FIFO data while getting + * step activity output. + */ +static int8_t move_if_s4s_frame(const uint8_t *frame_header, uint16_t *data_index, const struct bmi2_fifo_frame *fifo) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Variable to extract virtual header byte */ + uint8_t virtual_header_mode; + + /* Variable to define pay-load in words */ + uint8_t payload_word = 0; + + /* Variable to define pay-load in bytes */ + uint8_t payload_bytes = 0; + + /* Extract virtual header mode from the frame header */ + virtual_header_mode = BMI2_GET_BITS(*frame_header, BMI2_FIFO_VIRT_FRM_MODE); + + /* If the extracted header byte is a virtual header */ + if (virtual_header_mode == BMI2_FIFO_VIRT_FRM_MODE) + { + /* If frame header is not activity recognition header */ + if (*frame_header != 0xC8) + { + /* Extract pay-load in words from the header byte */ + payload_word = BMI2_GET_BITS(*frame_header, BMI2_FIFO_VIRT_PAYLOAD) + 1; + + /* Convert to bytes */ + payload_bytes = (uint8_t)(payload_word * 2); + + /* Move the data index by those pay-load bytes */ + rslt = move_next_frame(data_index, payload_bytes, fifo); + } + } + + return rslt; +} + +/*! + * @brief This internal API enables/disables compensation of the gain defined + * in the GAIN register. + */ +static int8_t enable_gyro_gain(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define register data */ + uint8_t reg_data = 0; + + rslt = bmi2_get_regs(BMI2_GYR_OFF_COMP_6_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_GAIN_EN, enable); + rslt = bmi2_set_regs(BMI2_GYR_OFF_COMP_6_ADDR, ®_data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This internal API is used to extract the output feature configuration + * details from the look-up table. + */ +static uint8_t extract_output_feat_config(struct bmi2_feature_config *feat_output, + uint8_t type, + const struct bmi2_dev *dev) +{ + /* Variable to define loop */ + uint8_t loop = 0; + + /* Variable to set flag */ + uint8_t feat_found = BMI2_FALSE; + + /* Search for the output feature from the output configuration array */ + while (loop < dev->out_sens) + { + if (dev->feat_output[loop].type == type) + { + *feat_output = dev->feat_output[loop]; + feat_found = BMI2_TRUE; + break; + } + + loop++; + } + + /* Return flag */ + return feat_found; +} + +/*! + * @brief This internal API is used to move the data index ahead of the + * current_frame_length parameter when unnecessary FIFO data appears while + * extracting the user specified data. + */ +static int8_t move_next_frame(uint16_t *data_index, uint8_t current_frame_length, const struct bmi2_fifo_frame *fifo) +{ + /* Variables to define error */ + int8_t rslt = BMI2_OK; + + /* Validate data index */ + if (((*data_index) + current_frame_length) > fifo->length) + { + /* Move the data index to the last byte */ + (*data_index) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + } + else + { + /* Move the data index to next frame */ + (*data_index) = (*data_index) + current_frame_length; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_hc.h b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_hc.h new file mode 100644 index 000000000..2a1b5bc07 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_hc.h @@ -0,0 +1,483 @@ +/** +* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. +* +* BSD-3-Clause +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* 3. Neither the name of the copyright holder nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING +* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* @file bmi270_hc.h +* @date 2020-11-04 +* @version v2.63.1 +* +*/ + +/** + * \ingroup bmi2xy + * \defgroup bmi270_hc BMI270_HC + * @brief Sensor driver for BMI270_HC sensor + */ + +#ifndef BMI270_HC_H_ +#define BMI270_HC_H_ + +/*! CPP guard */ +#ifdef __cplusplus +extern "C" { +#endif + +/***************************************************************************/ + +/*! Header files + ****************************************************************************/ +#include "bmi2.h" + +/***************************************************************************/ + +/*! Macro definitions + ****************************************************************************/ + +/*! @name BMI270_HC Chip identifier */ +#define BMI270_HC_CHIP_ID UINT8_C(0x24) + +/*! @name BMI270_HC feature input start addresses */ +#define BMI270_HC_CONFIG_ID_STRT_ADDR UINT8_C(0x06) +#define BMI270_HC_STEP_CNT_1_STRT_ADDR UINT8_C(0x00) +#define BMI270_HC_STEP_CNT_4_STRT_ADDR UINT8_C(0x02) +#define BMI270_HC_MAX_BURST_LEN_STRT_ADDR UINT8_C(0x08) +#define BMI270_HC_CRT_GYRO_SELF_TEST_STRT_ADDR UINT8_C(0x09) +#define BMI270_HC_ABORT_STRT_ADDR UINT8_C(0x09) +#define BMI270_HC_NVM_PROG_PREP_STRT_ADDR UINT8_C(0x0A) +#define BMI270_HC_ACT_RGN_SETT_STRT_ADDR UINT8_C(0x02) +#define BMI270_HC_ACT_RGN_STRT_ADDR UINT8_C(0x00) + +/*! @name BMI270_HC feature output start addresses */ +#define BMI270_HC_STEP_CNT_OUT_STRT_ADDR UINT8_C(0x00) +#define BMI270_HC_GYR_USER_GAIN_OUT_STRT_ADDR UINT8_C(0x04) +#define BMI270_HC_GYRO_CROSS_SENSE_STRT_ADDR UINT8_C(0x0C) +#define BMI270_HC_NVM_VFRM_OUT_STRT_ADDR UINT8_C(0x0E) + +/*! @name Defines maximum number of pages */ +#define BMI270_HC_MAX_PAGE_NUM UINT8_C(8) + +/*! @name Defines maximum number of feature input configurations */ +#define BMI270_HC_MAX_FEAT_IN UINT8_C(10) + +/*! @name Defines maximum number of feature outputs */ +#define BMI270_HC_MAX_FEAT_OUT UINT8_C(5) + +/*! @name Mask definitions for feature interrupt status bits */ +#define BMI270_HC_STEP_CNT_STATUS_MASK UINT8_C(0x01) + +/*! @name Mask definitions for feature interrupt mapping bits */ +#define BMI270_HC_INT_STEP_COUNTER_MASK UINT8_C(0x01) +#define BMI270_HC_INT_STEP_DETECTOR_MASK UINT8_C(0x01) + +/*! @name Defines maximum number of feature interrupts */ +#define BMI270_HC_MAX_INT_MAP UINT8_C(2) + +/***************************************************************************/ + +/*! BMI270_HC User Interface function prototypes + ****************************************************************************/ + +/** + * \ingroup bmi270_hc + * \defgroup bmi270_hcApiInit Initialization + * @brief Initialize the sensor and device structure + */ + +/*! + * \ingroup bmi270_hcApiInit + * \page bmi270_hc_api_bmi270_hc_init bmi270_hc_init + * \code + * int8_t bmi270_hc_init(struct bmi2_dev *dev); + * \endcode + * @details This API: + * 1) updates the device structure with address of the configuration file. + * 2) Initializes BMI270_HC sensor. + * 3) Writes the configuration file. + * 4) Updates the feature offset parameters in the device structure. + * 5) Updates the maximum number of pages, in the device structure. + * + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_hc_init(struct bmi2_dev *dev); + +/** + * \ingroup bmi270_hc + * \defgroup bmi270_hcApiSensor Feature Set + * @brief Enable / Disable features of the sensor + */ + +/*! + * \ingroup bmi270_hcApiSensor + * \page bmi270_hc_api_bmi270_hc_sensor_enable bmi270_hc_sensor_enable + * \code + * int8_t bmi270_hc_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API selects the sensors/features to be enabled. + * + * @param[in] sens_list : Pointer to select the sensor/feature. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features that can be enabled. + * + *@verbatim + * sens_list | Values + * ----------------------------|----------- + * BMI2_ACCEL | 0 + * BMI2_GYRO | 1 + * BMI2_AUX | 2 + * BMI2_STEP_DETECTOR | 6 + * BMI2_STEP_COUNTER | 7 + * BMI2_GYRO_GAIN_UPDATE | 9 + * BMI2_ACTIVITY_RECOGNITION | 34 + *@endverbatim + * + * @note : + * example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO}; + * uint8_t n_sens = 2; + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_hc_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + +/*! + * \ingroup bmi270_hcApiSensor + * \page bmi270_hc_api_bmi270_hc_sensor_disable bmi270_hc_sensor_disable + * \code + * int8_t bmi270_hc_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API selects the sensors/features to be disabled. + * + * @param[in] sens_list : Pointer to select the sensor/feature. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features that can be disabled. + * + *@verbatim + * sens_list | Values + * ----------------------------|----------- + * BMI2_ACCEL | 0 + * BMI2_GYRO | 1 + * BMI2_AUX | 2 + * BMI2_STEP_DETECTOR | 6 + * BMI2_STEP_COUNTER | 7 + * BMI2_GYRO_GAIN_UPDATE | 9 + * BMI2_ACTIVITY_RECOGNITION | 34 + *@endverbatim + * + * @note : + * example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO}; + * uint8_t n_sens = 2; + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_hc_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + +/** + * \ingroup bmi270_hc + * \defgroup bmi270_hcApiSensorC Sensor Configuration + * @brief Enable / Disable feature configuration of the sensor + */ + +/*! + * \ingroup bmi270_hcApiSensorC + * \page bmi270_hc_api_bmi270_hc_set_sensor_config bmi270_hc_set_sensor_config + * \code + * int8_t bmi270_hc_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API sets the sensor/feature configuration. + * + * @param[in] sens_cfg : Structure instance of bmi2_sens_config. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features that can be configured + * + *@verbatim + * sens_list | Values + * ----------------------------|----------- + * BMI2_STEP_DETECTOR | 6 + * BMI2_STEP_COUNTER | 7 + * BMI2_STEP_COUNTER_PARAMS | 28 + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_hc_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + +/*! + * \ingroup bmi270_hcApiSensorC + * \page bmi270_hc_api_bmi270_hc_get_sensor_config bmi270_hc_get_sensor_config + * \code + * int8_t bmi270_hc_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API gets the sensor/feature configuration. + * + * @param[in] sens_cfg : Structure instance of bmi2_sens_config. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features whose configurations can be read. + * + *@verbatim + * sens_list | Values + * ----------------------------|----------- + * BMI2_STEP_DETECTOR | 6 + * BMI2_STEP_COUNTER | 7 + * BMI2_STEP_COUNTER_PARAMS | 28 + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_hc_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + +/** + * \ingroup bmi270_hc + * \defgroup bmi270_hcApiSensorD Sensor Data + * @brief Get sensor data + */ + +/*! + * \ingroup bmi270_hcApiSensorD + * \page bmi270_hc_api_bmi270_hc_get_sensor_data bmi270_hc_get_sensor_data + * \code + * int8_t bmi270_hc_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API gets the sensor/feature data for accelerometer, gyroscope, + * auxiliary sensor, step counter, high-g, gyroscope user-gain update, + * orientation, gyroscope cross sensitivity and error status for NVM and VFRM. + * + * @param[out] sensor_data : Structure instance of bmi2_sensor_data. + * @param[in] n_sens : Number of sensors selected. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features whose data can be read + * + *@verbatim + * sens_list | Values + * ---------------------|----------- + * BMI2_STEP_COUNTER | 7 + * BMI2_NVM_STATUS | 38 + * BMI2_VFRM_STATUS | 39 + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_hc_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev); + +/** + * \ingroup bmi270_hc + * \defgroup bmi270_hcApiARecoghc Activity recognition settings + * @brief Set / Get activity recognition settings of bmi270hc sensor + */ + +/*! + * \ingroup bmi270_hcApiARecoghc + * \page bmi270_hc_api_bmi270_hc_get_act_recg_sett bmi270_hc_get_act_recg_sett + * \code + * int8_t bmi270_hc_get_act_recg_sett(struct bmi2_hc_act_recg_sett *sett, struct bmi2_dev *dev); + * \endcode + * @details This api is used for retrieving the following activity recognition settings currently set. + * + * @param[in] sett : Structure instance of bmi2_hc_act_recg_sett. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_hc_get_act_recg_sett(struct bmi2_hc_act_recg_sett *sett, struct bmi2_dev *dev); + +/*! + * \ingroup bmi270_hcApiARecoghc + * \page bmi270_hc_api_bmi270_hc_set_act_recg_sett bmi270_hc_set_act_recg_sett + * \code + * int8_t bmi270_hc_set_act_recg_sett(const struct bmi2_hc_act_recg_sett *sett, struct bmi2_dev *dev); + * \endcode + * @details This api is used for setting the following activity recognition settings + * + * @param[in] sett : Structure instance of bmi2_hc_act_recg_sett. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_hc_set_act_recg_sett(const struct bmi2_hc_act_recg_sett *sett, struct bmi2_dev *dev); + +/** + * \ingroup bmi270_hc + * \defgroup bmi270_hcApiactOut Activity Output + * @brief Activity output operations of the sensor + */ + +/*! + * \ingroup bmi270_hcApiactOut + * \page bmi270_hc_api_bmi270_hc_get_act_recog_output bmi270_hc_get_act_recog_output + * \code + * int8_t bmi270_hc_get_act_recog_output(struct bmi2_act_recog_output *act_recog, + * uint16_t *act_frm_len, + * struct bmi2_fifo_frame *fifo, + * const struct bmi2_dev *dev); + * + * \endcode + * @details This internal API is used to parse the activity output from the + * FIFO in header mode. + * + * @param[out] act_recog : Pointer to buffer where the parsed activity data + * bytes are stored. + * @param[in] act_frm_len : Number of activity frames parsed. + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @verbatim + * ---------------------------------------------------------------------------- + * bmi2_act_rec_output | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * time_stamp | time-stamp (expressed in 50Hz ticks) + * -------------------------|--------------------------------------------------- + * type | Type of activity + * -------------------------|--------------------------------------------------- + * stat | Activity status + * -------------------------|--------------------------------------------------- + * @endverbatim + * + *@verbatim + * type | Activities + *----------|--------------------- + * 0 | UNKNOWN + * 1 | STILL + * 2 | WALK + * 3 | RUN + * 4 | BIKE + * 5 | VEHICLE + * 6 | TILTED + *@endverbatim + * + * + *@verbatim + * stat | Activity status + *----------|--------------------- + * 1 | START + * 2 | END + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_hc_get_act_recog_output(struct bmi2_act_recog_output *act_recog, + uint16_t *act_frm_len, + struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev); + +/** + * \ingroup bmi270_hc + * \defgroup bmi270_hcApiGyroUG Gyro User Gain + * @brief Set / Get Gyro User Gain of the sensor + */ + +/*! + * \ingroup bmi270_hcApiGyroUG + * \page bmi270_hc_api_bmi270_hc_update_gyro_user_gain bmi270_hc_update_gyro_user_gain + * \code + * int8_t bmi270_hc_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev); + * \endcode + * @details This API updates the gyroscope user-gain. + * + * @param[in] user_gain : Structure that stores user-gain configurations. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_hc_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev); + +/*! + * \ingroup bmi270_hcApiGyroUG + * \page bmi270_hc_api_bmi270_hc_read_gyro_user_gain bmi270_hc_read_gyro_user_gain + * \code + * int8_t bmi270_hc_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, const struct bmi2_dev *dev); + * \endcode + * @details This API reads the compensated gyroscope user-gain values. + * + * @param[out] gyr_usr_gain : Structure that stores gain values. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_hc_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, struct bmi2_dev *dev); + +/*! + * \ingroup bmi270_hcApiInt + * \page bmi270_hc_api_bmi270_hc_map_feat_int bmi270_hc_map_feat_int + * \code + * int8_t bmi270_hc_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev) + * \endcode + * @details This API maps/unmaps feature interrupts to that of interrupt pins. + * + * @param[in] sens_int : Structure instance of bmi2_sens_int_config. + * @param[in] n_sens : Number of interrupts to be mapped. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_hc_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev); + +/******************************************************************************/ +/*! @name C++ Guard Macros */ +/******************************************************************************/ +#ifdef __cplusplus +} +#endif /* End of CPP guard */ + +#endif /* BMI270_HC_H_ */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_maximum_fifo.c b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_maximum_fifo.c new file mode 100644 index 000000000..c97e4bdd8 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_maximum_fifo.c @@ -0,0 +1,212 @@ +/** +* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. +* +* BSD-3-Clause +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* 3. Neither the name of the copyright holder nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING +* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* @file bmi270_maximum_fifo.c +* @date 2020-11-04 +* @version v2.63.1 +* +*/ + +/***************************************************************************/ + +/*! Header files + ****************************************************************************/ +#include "bmi270_maximum_fifo.h" + +/***************************************************************************/ + +/*! Global Variable + ****************************************************************************/ + +/*! @name Global array that stores the configuration file of BMI270 */ +const uint8_t bmi270_maximum_fifo_config_file[] = { + 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x1a, 0x00, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, + 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0x90, 0x32, 0x21, 0x2e, 0x59, 0xf5, + 0x10, 0x30, 0x21, 0x2e, 0x6a, 0xf5, 0x1a, 0x24, 0x22, 0x00, 0x80, 0x2e, 0x3b, 0x00, 0xc8, 0x2e, 0x44, 0x47, 0x22, + 0x00, 0x37, 0x00, 0xa4, 0x00, 0xff, 0x0f, 0xd1, 0x00, 0x07, 0xad, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x11, 0x24, 0xfc, 0xf5, 0x80, 0x30, 0x40, 0x42, 0x50, 0x50, 0x00, 0x30, 0x12, 0x24, 0xeb, + 0x00, 0x03, 0x30, 0x00, 0x2e, 0xc1, 0x86, 0x5a, 0x0e, 0xfb, 0x2f, 0x21, 0x2e, 0xfc, 0xf5, 0x13, 0x24, 0x63, 0xf5, + 0xe0, 0x3c, 0x48, 0x00, 0x22, 0x30, 0xf7, 0x80, 0xc2, 0x42, 0xe1, 0x7f, 0x3a, 0x25, 0xfc, 0x86, 0xf0, 0x7f, 0x41, + 0x33, 0x98, 0x2e, 0xc2, 0xc4, 0xd6, 0x6f, 0xf1, 0x30, 0xf1, 0x08, 0xc4, 0x6f, 0x11, 0x24, 0xff, 0x03, 0x12, 0x24, + 0x00, 0xfc, 0x61, 0x09, 0xa2, 0x08, 0x36, 0xbe, 0x2a, 0xb9, 0x13, 0x24, 0x38, 0x00, 0x64, 0xbb, 0xd1, 0xbe, 0x94, + 0x0a, 0x71, 0x08, 0xd5, 0x42, 0x21, 0xbd, 0x91, 0xbc, 0xd2, 0x42, 0xc1, 0x42, 0x00, 0xb2, 0xfe, 0x82, 0x05, 0x2f, + 0x50, 0x30, 0x21, 0x2e, 0x21, 0xf2, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0xf0, 0x6f, 0x02, 0x30, 0x02, 0x42, 0x20, + 0x26, 0xe0, 0x6f, 0x02, 0x31, 0x03, 0x40, 0x9a, 0x0a, 0x02, 0x42, 0xf0, 0x37, 0x05, 0x2e, 0x5e, 0xf7, 0x10, 0x08, + 0x12, 0x24, 0x1e, 0xf2, 0x80, 0x42, 0x83, 0x84, 0xf1, 0x7f, 0x0a, 0x25, 0x13, 0x30, 0x83, 0x42, 0x3b, 0x82, 0xf0, + 0x6f, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x12, 0x40, 0x52, 0x42, 0x00, 0x2e, 0x12, 0x40, 0x52, 0x42, 0x3e, 0x84, + 0x00, 0x40, 0x40, 0x42, 0x7e, 0x82, 0xe1, 0x7f, 0xf2, 0x7f, 0x98, 0x2e, 0x6a, 0xd6, 0x21, 0x30, 0x23, 0x2e, 0x61, + 0xf5, 0xeb, 0x2c, 0xe1, 0x6f +}; + +// The rest of this is not needed, avoid compiler errors due to pedantic settings +#if false + +/*! @name Global array that stores the feature input configuration of BMI270 */ +const struct bmi2_feature_config bmi270_maximum_fifo_feat_in[BMI270_MAXIMUM_FIFO_MAX_FEAT_IN] = { + +}; + +/*! @name Global array that stores the feature output configuration */ +const struct bmi2_feature_config bmi270_maximum_fifo_feat_out[BMI270_MAXIMUM_FIFO_MAX_FEAT_OUT] = { + +}; + +/******************************************************************************/ + +/*! Local Function Prototypes + ******************************************************************************/ + +/*! + * @brief This internal API is used to validate the device pointer for + * null conditions. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t null_ptr_check(const struct bmi2_dev *dev); + +/***************************************************************************/ + +/*! User Interface Definitions + ****************************************************************************/ + +/*! + * @brief This API: + * 1) updates the device structure with address of the configuration file. + * 2) Initializes BMI270 sensor. + * 3) Writes the configuration file. + * 4) Updates the feature offset parameters in the device structure. + * 5) Updates the maximum number of pages, in the device structure. + */ +int8_t bmi270_maximum_fifo_init(struct bmi2_dev *dev) +{ + /* Variable to define result */ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + + if (rslt == BMI2_OK) + { + /* Assign chip id of BMI270 */ + dev->chip_id = BMI270_MAXIMUM_FIFO_CHIP_ID; + + dev->config_size = sizeof(bmi270_maximum_fifo_config_file); + + /* Enable the variant specific features if any */ + dev->variant_feature = BMI2_GYRO_CROSS_SENS_ENABLE | BMI2_MAXIMUM_FIFO_VARIANT; + + /* An extra dummy byte is read during SPI read */ + if (dev->intf == BMI2_SPI_INTF) + { + dev->dummy_byte = 1; + } + else + { + dev->dummy_byte = 0; + } + + /* If configuration file pointer is not assigned any address */ + if (!dev->config_file_ptr) + { + /* Give the address of the configuration file array to + * the device pointer + */ + dev->config_file_ptr = bmi270_maximum_fifo_config_file; + } + + /* Initialize BMI2 sensor */ + rslt = bmi2_sec_init(dev); + + if (rslt == BMI2_OK) + { + /* Assign the offsets of the feature input + * configuration to the device structure + */ + dev->feat_config = bmi270_maximum_fifo_feat_in; + + /* Assign the offsets of the feature output to + * the device structure + */ + dev->feat_output = bmi270_maximum_fifo_feat_out; + + /* Assign the maximum number of pages to the + * device structure + */ + dev->page_max = BMI270_MAXIMUM_FIFO_MAX_PAGE_NUM; + + /* Assign maximum number of input sensors/ + * features to device structure + */ + dev->input_sens = BMI270_MAXIMUM_FIFO_MAX_FEAT_IN; + + /* Assign maximum number of output sensors/ + * features to device structure + */ + dev->out_sens = BMI270_MAXIMUM_FIFO_MAX_FEAT_OUT; + + /* Get the gyroscope cross axis sensitivity */ + rslt = bmi2_get_gyro_cross_sense(dev); + } + } + + return rslt; +} + +/***************************************************************************/ + +/*! Local Function Definitions + ****************************************************************************/ + +/*! + * @brief This internal API is used to validate the device structure pointer for + * null conditions. + */ +static int8_t null_ptr_check(const struct bmi2_dev *dev) +{ + /* Variable to define result */ + int8_t rslt = BMI2_OK; + + if ((dev == NULL) || (dev->read == NULL) || (dev->write == NULL) || (dev->delay_us == NULL)) + { + /* Device structure pointer is not valid */ + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} +#endif diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_maximum_fifo.h b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_maximum_fifo.h new file mode 100644 index 000000000..e912a96a6 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_maximum_fifo.h @@ -0,0 +1,117 @@ +/** +* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. +* +* BSD-3-Clause +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* 3. Neither the name of the copyright holder nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING +* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* @file bmi270_maximum_fifo.h +* @date 2020-11-04 +* @version v2.63.1 +* +*/ + +/** + * \ingroup bmi2xy + * \defgroup bmi270_maximum_fifo BMI270_MAXIMUM_FIFO + * @brief Sensor driver for BMI270_MAXIMUM_FIFO sensor + */ + +#ifndef BMI270_MAXIMUM_FIFO_H_ +#define BMI270_MAXIMUM_FIFO_H_ + +/*! CPP guard */ +#ifdef __cplusplus +extern "C" { +#endif + +/***************************************************************************/ + +/*! Header files + ****************************************************************************/ +#include "bmi2.h" + +/***************************************************************************/ + +/*! Macro definitions + ****************************************************************************/ + +/*! @name BMI270 Chip identifier */ +#define BMI270_MAXIMUM_FIFO_CHIP_ID UINT8_C(0x24) + +/*! @name Defines maximum number of pages */ +#define BMI270_MAXIMUM_FIFO_MAX_PAGE_NUM UINT8_C(0) + +/*! @name Defines maximum number of feature input configurations */ +#define BMI270_MAXIMUM_FIFO_MAX_FEAT_IN UINT8_C(0) + +/*! @name Defines maximum number of feature outputs */ +#define BMI270_MAXIMUM_FIFO_MAX_FEAT_OUT UINT8_C(0) + +/*! @name Mask definitions for feature interrupt status bits */ + +/***************************************************************************/ + +/*! BMI270 User Interface function prototypes + ****************************************************************************/ + +/** + * \ingroup bmi270_maximum_fifo + * \defgroup bmi270_maximum_fifoApiInit Initialization + * @brief Initialize the sensor and device structure + */ + +/*! + * \ingroup bmi270_maximum_fifoApiInit + * \page bmi270_maximum_fifo_api_bmi270_maximum_fifo_init bmi270_maximum_fifo_init + * \code + * int8_t bmi270_maximum_fifo_init(struct bmi2_dev *dev); + * \endcode + * @details This API: + * 1) updates the device structure with address of the configuration file. + * 2) Initializes BMI270 sensor. + * 3) Writes the configuration file. + * 4) Updates the feature offset parameters in the device structure. + * 5) Updates the maximum number of pages, in the device structure. + * + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_maximum_fifo_init(struct bmi2_dev *dev); + +/******************************************************************************/ +/*! @name C++ Guard Macros */ +/******************************************************************************/ +#ifdef __cplusplus +} +#endif /* End of CPP guard */ + +#endif /* BMI270_MAXIMUM_FIFO_H_ */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_wh.c b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_wh.c new file mode 100644 index 000000000..eda23cc02 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_wh.c @@ -0,0 +1,3431 @@ +/** +* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. +* +* BSD-3-Clause +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* 3. Neither the name of the copyright holder nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING +* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* @file bmi270_wh.c +* @date 2020-11-04 +* @version v2.63.1 +* +*/ + +/***************************************************************************/ + +/*! Header files + ****************************************************************************/ +#include "bmi270_wh.h" + +/***************************************************************************/ + +/*! Global Variable + ****************************************************************************/ + +/*! @name Global array that stores the configuration file of BMI270_WH */ +const uint8_t bmi270_wh_config_file[] = { + 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0xf8, 0x02, 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0xe1, 0x00, 0x80, 0x2e, 0xc0, + 0x02, 0x80, 0x2e, 0xe2, 0x00, 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x2c, 0x02, 0x80, 0x2e, 0xe4, 0x07, 0x59, 0xf5, + 0x10, 0x30, 0x21, 0x2e, 0x6a, 0xf5, 0x80, 0x2e, 0xdd, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3e, 0x31, 0x01, 0x00, 0x22, + 0x00, 0x80, 0x00, 0xfa, 0x07, 0xff, 0x0f, 0xd1, 0x00, 0x65, 0x9d, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0xfd, 0x2d, 0x7b, 0xaf, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x03, 0x00, 0x18, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x01, 0x12, + 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x1a, 0x24, 0x22, 0x00, 0x80, 0x2e, 0x8e, 0x01, 0xc8, 0x2e, 0xc8, 0x2e, 0xbb, 0x52, + 0x00, 0x2e, 0x60, 0x40, 0x41, 0x40, 0x0d, 0xbc, 0x98, 0xbc, 0xc0, 0x2e, 0x01, 0x0a, 0x0f, 0xb8, 0x43, 0x86, 0x25, + 0x40, 0x04, 0x40, 0xd8, 0xbe, 0x2c, 0x0b, 0x22, 0x11, 0x54, 0x42, 0x03, 0x80, 0x4b, 0x0e, 0xf6, 0x2f, 0xb8, 0x2e, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0xfd, 0x2d, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3e, 0x31, 0x00, 0x00, + 0x88, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0xe0, 0xaa, 0x38, 0x05, 0xe0, 0x90, 0x30, 0x04, 0x00, 0xee, + 0x06, 0xf2, 0x05, 0x80, 0x80, 0x16, 0xf1, 0x02, 0x02, 0x2d, 0x01, 0xd4, 0x7b, 0x3b, 0x01, 0xdb, 0x7a, 0x04, 0x00, + 0x3f, 0x7b, 0xcd, 0x6c, 0xc3, 0x04, 0x85, 0x09, 0xc3, 0x04, 0xec, 0xe6, 0x0c, 0x46, 0x01, 0x00, 0x27, 0x00, 0x19, + 0x00, 0x96, 0x00, 0xa0, 0x00, 0x01, 0x00, 0x0c, 0x00, 0xf0, 0x3c, 0x00, 0x01, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, + 0x0e, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x92, 0x00, 0x98, 0xf1, 0xaf, 0x00, 0xae, 0x00, 0x1e, + 0xf2, 0x19, 0xf4, 0x66, 0xf5, 0x00, 0x04, 0xff, 0x00, 0x64, 0xf5, 0x96, 0x00, 0xaa, 0x00, 0x86, 0x00, 0x99, 0x00, + 0x88, 0x00, 0x8a, 0x00, 0xff, 0x3f, 0xff, 0xfb, 0x31, 0x01, 0x00, 0x38, 0x00, 0x30, 0xfd, 0xf5, 0xb2, 0x00, 0xff, + 0x03, 0x00, 0xfc, 0xf0, 0x3f, 0x0b, 0x01, 0x0e, 0x01, 0x10, 0x01, 0xb9, 0x00, 0x2d, 0xf5, 0xca, 0xf5, 0xb0, 0x00, + 0x20, 0xf2, 0xb2, 0x00, 0xff, 0x1f, 0x00, 0x40, 0x80, 0x2e, 0xc0, 0x08, 0x1d, 0x6c, 0xad, 0x00, 0x59, 0x01, 0x31, + 0xd1, 0xff, 0x7f, 0x00, 0x08, 0xee, 0x7a, 0xe4, 0x0c, 0x12, 0x02, 0xb3, 0x00, 0xb4, 0x04, 0x62, 0x03, 0xc0, 0x02, + 0x1f, 0xf8, 0xe1, 0x07, 0xd3, 0x07, 0xd7, 0x00, 0xd3, 0x00, 0xb9, 0x00, 0xc3, 0x00, 0xc5, 0x00, 0xbd, 0x00, 0xbc, + 0x00, 0xbf, 0x00, 0xdd, 0x00, 0x95, 0x00, 0xc2, 0x00, 0xd6, 0x00, 0xf0, 0x00, 0x00, 0xff, 0x00, 0x80, 0x30, 0x50, + 0x98, 0x2e, 0xc3, 0xb0, 0x02, 0x30, 0x00, 0x30, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x01, 0x30, 0x00, 0x2e, 0x41, + 0x82, 0x48, 0xa2, 0xfb, 0x2f, 0x03, 0x2e, 0x95, 0x00, 0x40, 0xb2, 0x1a, 0x2f, 0x05, 0x2e, 0x0a, 0x01, 0x91, 0x52, + 0x98, 0x2e, 0xc7, 0xc1, 0x05, 0x2e, 0x18, 0x00, 0x80, 0xb2, 0x02, 0x30, 0x10, 0x2f, 0xf0, 0x7f, 0x00, 0x2e, 0x91, + 0x50, 0x98, 0x2e, 0x4d, 0xc3, 0x91, 0x50, 0x98, 0x2e, 0x5a, 0xc7, 0x98, 0x2e, 0xbc, 0x03, 0x98, 0x2e, 0x00, 0xb0, + 0x10, 0x30, 0x21, 0x2e, 0x19, 0x00, 0xf0, 0x6f, 0x02, 0x30, 0x03, 0x2e, 0x85, 0x00, 0x40, 0xb2, 0x40, 0x2f, 0x03, + 0x2e, 0x85, 0x00, 0x03, 0x31, 0x4b, 0x08, 0x40, 0xb2, 0x07, 0x2f, 0xf0, 0x7f, 0x98, 0x2e, 0x47, 0xcb, 0x10, 0x30, + 0x21, 0x2e, 0x19, 0x00, 0xf0, 0x6f, 0x02, 0x30, 0x03, 0x2e, 0x85, 0x00, 0x43, 0x30, 0x4b, 0x08, 0x40, 0xb2, 0x2a, + 0x2f, 0x03, 0x2e, 0x90, 0x00, 0xf0, 0x7f, 0x40, 0xb2, 0x0e, 0x2f, 0x41, 0x90, 0x20, 0x2f, 0x05, 0x2e, 0x3f, 0x01, + 0x07, 0x2e, 0x91, 0x00, 0xa1, 0x32, 0x95, 0x58, 0x98, 0x2e, 0x97, 0xb3, 0x02, 0x30, 0x25, 0x2e, 0x90, 0x00, 0x15, + 0x2c, 0xf0, 0x6f, 0x93, 0x58, 0xa3, 0x32, 0x10, 0x41, 0x11, 0x41, 0x18, 0xb9, 0x04, 0x41, 0x98, 0xbc, 0x41, 0x0a, + 0x94, 0x0a, 0x98, 0x2e, 0xf1, 0x03, 0x12, 0x30, 0x21, 0x2e, 0x91, 0x00, 0x21, 0x2e, 0xaf, 0x00, 0x25, 0x2e, 0x90, + 0x00, 0xf0, 0x6f, 0x02, 0x30, 0x11, 0x30, 0x23, 0x2e, 0x19, 0x00, 0x25, 0x2e, 0x85, 0x00, 0x03, 0x2e, 0x19, 0x00, + 0x40, 0xb2, 0x22, 0x2f, 0xf0, 0x7f, 0x98, 0x2e, 0xf5, 0xcb, 0x97, 0x54, 0xbc, 0x84, 0x21, 0x2e, 0xae, 0x00, 0x81, + 0x40, 0x82, 0x86, 0x99, 0x54, 0x18, 0xb8, 0xc3, 0x40, 0x91, 0x42, 0x90, 0x42, 0x33, 0xbc, 0x90, 0x42, 0xe2, 0x7f, + 0xf0, 0x31, 0x82, 0x40, 0x10, 0x08, 0xf2, 0x6f, 0x25, 0xbd, 0x02, 0x0a, 0xd0, 0x7f, 0x98, 0x2e, 0xa8, 0xcf, 0x06, + 0xbc, 0xd2, 0x6f, 0xe1, 0x6f, 0x10, 0x0a, 0x40, 0x42, 0x98, 0x2e, 0x2c, 0x03, 0xf0, 0x6f, 0x02, 0x30, 0x25, 0x2e, + 0x19, 0x00, 0x25, 0x2e, 0x95, 0x00, 0x80, 0x2e, 0x93, 0x01, 0x90, 0x50, 0xf7, 0x7f, 0xe6, 0x7f, 0xd5, 0x7f, 0xc4, + 0x7f, 0xb3, 0x7f, 0xa1, 0x7f, 0x90, 0x7f, 0x82, 0x7f, 0x7b, 0x7f, 0x98, 0x2e, 0xe3, 0x00, 0x00, 0xb2, 0x65, 0x2f, + 0x05, 0x2e, 0x31, 0x01, 0x01, 0x2e, 0x11, 0x01, 0x03, 0x2e, 0x0f, 0x01, 0x8f, 0xb9, 0x23, 0xbe, 0x9f, 0xb8, 0xcb, + 0x0a, 0x24, 0xbc, 0x4f, 0xba, 0x03, 0x2e, 0x12, 0x01, 0x0f, 0xb8, 0x22, 0xbd, 0xdc, 0x0a, 0x2f, 0xb9, 0x9b, 0xbc, + 0x18, 0x0a, 0x9f, 0xb8, 0x82, 0x0a, 0x91, 0x0a, 0x25, 0x2e, 0x18, 0x00, 0x05, 0x2e, 0xc1, 0xf5, 0x2e, 0xbd, 0x2e, + 0xb9, 0x01, 0x2e, 0x18, 0x00, 0x31, 0x30, 0x8a, 0x04, 0x00, 0x90, 0x03, 0x2f, 0x01, 0x2e, 0x83, 0x00, 0x00, 0xb2, + 0x19, 0x2f, 0x9b, 0x50, 0x91, 0x52, 0x98, 0x2e, 0xec, 0x00, 0x05, 0x2e, 0x82, 0x00, 0x25, 0x2e, 0x95, 0x00, 0x05, + 0x2e, 0x82, 0x00, 0x80, 0x90, 0x02, 0x2f, 0x12, 0x30, 0x25, 0x2e, 0x82, 0x00, 0x01, 0x2e, 0x83, 0x00, 0x00, 0xb2, + 0x10, 0x30, 0x05, 0x2e, 0x18, 0x00, 0x01, 0x2f, 0x21, 0x2e, 0x18, 0x00, 0x25, 0x2e, 0x83, 0x00, 0x05, 0x2e, 0x18, + 0x00, 0x80, 0xb2, 0x20, 0x2f, 0x01, 0x2e, 0xc0, 0xf5, 0xf2, 0x30, 0x02, 0x08, 0x07, 0xaa, 0x73, 0x30, 0x03, 0x2e, + 0x84, 0x00, 0x18, 0x22, 0x41, 0x1a, 0x05, 0x2f, 0x03, 0x2e, 0x66, 0xf5, 0x9f, 0xbc, 0x9f, 0xb8, 0x40, 0x90, 0x0c, + 0x2f, 0x9d, 0x52, 0x03, 0x30, 0x53, 0x42, 0x2b, 0x30, 0x90, 0x04, 0x5b, 0x42, 0x21, 0x2e, 0x84, 0x00, 0x24, 0xbd, + 0x7e, 0x80, 0x81, 0x84, 0x43, 0x42, 0x02, 0x42, 0x02, 0x32, 0x25, 0x2e, 0x62, 0xf5, 0x9f, 0x52, 0x05, 0x2e, 0x0a, + 0x01, 0x91, 0x08, 0x00, 0x31, 0x21, 0x2e, 0x60, 0xf5, 0x80, 0xb2, 0x0b, 0x2f, 0xf2, 0x3e, 0x01, 0x2e, 0xca, 0xf5, + 0x82, 0x08, 0x25, 0x2e, 0xca, 0xf5, 0x05, 0x2e, 0x59, 0xf5, 0xe0, 0x3f, 0x90, 0x08, 0x25, 0x2e, 0x59, 0xf5, 0xa1, + 0x6f, 0xb3, 0x6f, 0xc4, 0x6f, 0xd5, 0x6f, 0xe6, 0x6f, 0xf7, 0x6f, 0x7b, 0x6f, 0x82, 0x6f, 0x90, 0x6f, 0x70, 0x5f, + 0xc8, 0x2e, 0xa0, 0x50, 0x80, 0x7f, 0xe7, 0x7f, 0xd5, 0x7f, 0xc4, 0x7f, 0xb3, 0x7f, 0xa2, 0x7f, 0x91, 0x7f, 0xf6, + 0x7f, 0x7b, 0x7f, 0x00, 0x2e, 0x01, 0x2e, 0x60, 0xf5, 0x60, 0x7f, 0x98, 0x2e, 0xe3, 0x00, 0x62, 0x6f, 0x01, 0x32, + 0x91, 0x08, 0x80, 0xb2, 0x0d, 0x2f, 0x00, 0xb2, 0x03, 0x2f, 0x05, 0x2e, 0x18, 0x00, 0x80, 0x90, 0x05, 0x2f, 0xa3, + 0x56, 0x02, 0x30, 0xc1, 0x42, 0xc2, 0x86, 0x00, 0x2e, 0xc2, 0x42, 0x23, 0x2e, 0x60, 0xf5, 0x00, 0x90, 0x00, 0x30, + 0x06, 0x2f, 0x21, 0x2e, 0x82, 0x00, 0xa1, 0x50, 0x21, 0x2e, 0x5a, 0xf2, 0x98, 0x2e, 0x3d, 0x03, 0xf6, 0x6f, 0x80, + 0x6f, 0x91, 0x6f, 0xa2, 0x6f, 0xb3, 0x6f, 0xc4, 0x6f, 0xd5, 0x6f, 0xe7, 0x6f, 0x7b, 0x6f, 0x60, 0x5f, 0xc8, 0x2e, + 0xc0, 0x50, 0xe7, 0x7f, 0xf6, 0x7f, 0x26, 0x30, 0x0f, 0x2e, 0x61, 0xf5, 0x2f, 0x2e, 0x85, 0x00, 0x0f, 0x2e, 0x85, + 0x00, 0xbe, 0x09, 0xd5, 0x7f, 0xc4, 0x7f, 0xb3, 0x7f, 0xa2, 0x7f, 0x90, 0x7f, 0x7b, 0x7f, 0x80, 0xb3, 0x81, 0x7f, + 0x11, 0x2f, 0xa5, 0x50, 0x1a, 0x25, 0x12, 0x40, 0x42, 0x7f, 0x74, 0x82, 0x12, 0x40, 0x52, 0x7f, 0x00, 0x2e, 0x00, + 0x40, 0x60, 0x7f, 0x98, 0x2e, 0x6a, 0xd6, 0x03, 0x2e, 0x61, 0xf7, 0x00, 0x31, 0x48, 0x0a, 0x23, 0x2e, 0x61, 0xf7, + 0xe1, 0x31, 0x23, 0x2e, 0x61, 0xf5, 0xf6, 0x6f, 0xe7, 0x6f, 0x81, 0x6f, 0x90, 0x6f, 0xa2, 0x6f, 0xb3, 0x6f, 0xc4, + 0x6f, 0xd5, 0x6f, 0x7b, 0x6f, 0x40, 0x5f, 0xc8, 0x2e, 0xa7, 0x50, 0x10, 0x50, 0xbd, 0x52, 0x05, 0x2e, 0x8d, 0x00, + 0xfb, 0x7f, 0x00, 0x2e, 0x13, 0x40, 0x93, 0x42, 0x41, 0x0e, 0xfb, 0x2f, 0x98, 0x2e, 0x91, 0x03, 0xfb, 0x6f, 0xf0, + 0x5f, 0x80, 0x2e, 0x87, 0xcf, 0x10, 0x50, 0xfb, 0x7f, 0x98, 0x2e, 0x56, 0xc7, 0x98, 0x2e, 0x49, 0xc3, 0x11, 0x30, + 0x30, 0x30, 0xfb, 0x6f, 0xf0, 0x5f, 0x23, 0x2e, 0x8f, 0x00, 0x21, 0x2e, 0x86, 0x00, 0x23, 0x2e, 0x19, 0x00, 0x21, + 0x2e, 0xac, 0x00, 0xb8, 0x2e, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x9a, 0x01, + 0x34, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x03, 0x2e, 0x8d, 0x00, 0x16, 0xb8, 0x02, 0x34, 0x4a, 0x0c, 0x21, 0x2e, 0x2d, 0xf5, 0xc0, 0x2e, 0x23, + 0x2e, 0x8d, 0x00, 0x40, 0x50, 0xf1, 0x7f, 0x0a, 0x25, 0x3c, 0x86, 0xeb, 0x7f, 0x41, 0x33, 0x22, 0x30, 0x98, 0x2e, + 0xc2, 0xc4, 0xd3, 0x6f, 0xf4, 0x30, 0xdc, 0x09, 0xc1, 0x58, 0xc2, 0x6f, 0x94, 0x09, 0xc3, 0x58, 0x6a, 0xbb, 0xdc, + 0x08, 0xb4, 0xb9, 0xb1, 0xbd, 0xbf, 0x5a, 0x95, 0x08, 0x21, 0xbd, 0xf6, 0xbf, 0x77, 0x0b, 0x51, 0xbe, 0xf1, 0x6f, + 0xeb, 0x6f, 0x52, 0x42, 0x54, 0x42, 0xc0, 0x2e, 0x43, 0x42, 0xc0, 0x5f, 0x03, 0x2e, 0x12, 0x01, 0x00, 0x31, 0x08, + 0x08, 0xf2, 0x30, 0x20, 0x50, 0x4a, 0x08, 0xe1, 0x7f, 0x00, 0xb2, 0xfb, 0x7f, 0x01, 0x30, 0x15, 0x2f, 0x01, 0x2e, + 0x8f, 0x00, 0x01, 0x90, 0x03, 0x2f, 0x23, 0x2e, 0x8f, 0x00, 0x98, 0x2e, 0xe7, 0x03, 0x98, 0x2e, 0xa8, 0xcf, 0x11, + 0x30, 0x41, 0x08, 0x23, 0x2e, 0xd5, 0x00, 0x98, 0x2e, 0x41, 0xb1, 0x10, 0x25, 0xfb, 0x6f, 0xe0, 0x6f, 0xe0, 0x5f, + 0x80, 0x2e, 0x95, 0xcf, 0xe0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, 0x11, 0x30, 0x23, 0x2e, 0x8f, 0x00, 0xfb, 0x6f, 0xe0, + 0x5f, 0xb8, 0x2e, 0xd5, 0x50, 0x01, 0x30, 0x0f, 0x55, 0x11, 0x42, 0x42, 0x0e, 0xfc, 0x2f, 0x10, 0x30, 0xc0, 0x2e, + 0x21, 0x2e, 0xbc, 0x00, 0x40, 0x50, 0x0a, 0x25, 0x3c, 0x80, 0xfb, 0x7f, 0x01, 0x42, 0xd2, 0x7f, 0xe3, 0x7f, 0x32, + 0x30, 0x10, 0x25, 0x98, 0x2e, 0x7a, 0xc1, 0xfb, 0x6f, 0xc0, 0x5f, 0xb8, 0x2e, 0x44, 0x47, 0xb5, 0x50, 0xf0, 0x50, + 0x12, 0x40, 0x06, 0x40, 0x1a, 0x25, 0x6c, 0xbe, 0x76, 0x8a, 0x74, 0x80, 0xfb, 0x7f, 0x68, 0xbf, 0xb7, 0x56, 0x0b, + 0x30, 0xd3, 0x08, 0x4c, 0xba, 0x6c, 0xbb, 0x5b, 0x7f, 0x4b, 0x43, 0x0b, 0x42, 0xc0, 0xb2, 0xc6, 0x7f, 0xb4, 0x7f, + 0xd0, 0x7f, 0xe5, 0x7f, 0xa3, 0x7f, 0x90, 0x2e, 0xaf, 0xb0, 0x01, 0x2e, 0x8c, 0x00, 0x00, 0xb2, 0x0b, 0x2f, 0xab, + 0x52, 0x01, 0x2e, 0x87, 0x00, 0x92, 0x7f, 0x98, 0x2e, 0xbb, 0xcc, 0x01, 0x30, 0x23, 0x2e, 0x8c, 0x00, 0x92, 0x6f, + 0xa3, 0x6f, 0x1a, 0x25, 0x26, 0xbc, 0x86, 0xba, 0x25, 0xbc, 0x0f, 0xb8, 0x54, 0xb1, 0x00, 0xb2, 0x96, 0x7f, 0x0c, + 0x2f, 0xad, 0x50, 0xaf, 0x54, 0x0b, 0x30, 0x0b, 0x2e, 0x31, 0x01, 0xb3, 0x58, 0x1b, 0x42, 0x9b, 0x42, 0x6c, 0x09, + 0x0b, 0x42, 0x2b, 0x2e, 0x31, 0x01, 0x8b, 0x42, 0x71, 0x84, 0xb9, 0x50, 0x58, 0x09, 0xb1, 0x52, 0x91, 0x50, 0x82, + 0x7f, 0x75, 0x7f, 0x98, 0x2e, 0xc2, 0xc0, 0x01, 0x2e, 0x87, 0x00, 0xe5, 0x6f, 0xd4, 0x6f, 0x73, 0x6f, 0x82, 0x6f, + 0xab, 0x52, 0xa9, 0x5c, 0x98, 0x2e, 0x06, 0xcd, 0x65, 0x6f, 0xa0, 0x6f, 0xad, 0x52, 0x40, 0xb3, 0x04, 0xbd, 0x53, + 0x40, 0xaf, 0xba, 0x44, 0x40, 0xe1, 0x7f, 0x02, 0x30, 0x06, 0x2f, 0x40, 0xb3, 0x02, 0x30, 0x03, 0x2f, 0xaf, 0x5c, + 0x12, 0x30, 0x93, 0x43, 0x84, 0x43, 0x03, 0xbf, 0x6f, 0xbb, 0x80, 0xb3, 0x20, 0x2f, 0x46, 0x6f, 0xde, 0x00, 0x56, + 0x6f, 0x26, 0x03, 0x44, 0x42, 0x40, 0x91, 0x27, 0x2e, 0x88, 0x00, 0xaf, 0x52, 0x14, 0x2f, 0xaf, 0x5c, 0x00, 0x2e, + 0x95, 0x41, 0x86, 0x41, 0x5d, 0x05, 0xa6, 0x07, 0x80, 0xab, 0x04, 0x2f, 0x80, 0x91, 0x0a, 0x2f, 0x96, 0x6f, 0x75, + 0x0f, 0x07, 0x2f, 0x95, 0x6f, 0x40, 0xb3, 0x04, 0x2f, 0x53, 0x42, 0x44, 0x42, 0x12, 0x30, 0x04, 0x2c, 0x11, 0x30, + 0x02, 0x2c, 0x11, 0x30, 0x11, 0x30, 0x02, 0xbc, 0x0f, 0xb8, 0xd2, 0x7f, 0x00, 0x90, 0x06, 0x2f, 0x31, 0x30, 0x23, + 0x2e, 0xac, 0x00, 0x23, 0x2e, 0x86, 0x00, 0x0b, 0x2c, 0x01, 0x30, 0x01, 0x2e, 0x86, 0x00, 0x05, 0x2e, 0xac, 0x00, + 0x10, 0x1a, 0x02, 0x2f, 0x21, 0x2e, 0xac, 0x00, 0x01, 0x2d, 0x01, 0x30, 0xc0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, 0xd1, + 0x6f, 0xb0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, 0xe2, 0x6f, 0xa7, 0x52, 0x01, 0x2e, 0x88, 0x00, 0x82, 0x40, 0x50, 0x42, + 0x11, 0x2c, 0x42, 0x42, 0x10, 0x30, 0x31, 0x30, 0x21, 0x2e, 0x8c, 0x00, 0x23, 0x2e, 0xac, 0x00, 0x23, 0x2e, 0x86, + 0x00, 0xc0, 0x6f, 0x01, 0x30, 0x98, 0x2e, 0x95, 0xcf, 0xb0, 0x6f, 0x01, 0x30, 0x98, 0x2e, 0x95, 0xcf, 0x00, 0x2e, + 0xfb, 0x6f, 0x10, 0x5f, 0xb8, 0x2e, 0x50, 0x50, 0xcd, 0x50, 0x51, 0x30, 0x11, 0x42, 0xfb, 0x7f, 0x7b, 0x30, 0x0b, + 0x42, 0x11, 0x30, 0x02, 0x80, 0x23, 0x33, 0x01, 0x42, 0x03, 0x00, 0x07, 0x2e, 0x80, 0x03, 0x05, 0x2e, 0x8d, 0x00, + 0xa5, 0x52, 0xe2, 0x7f, 0xd3, 0x7f, 0xc0, 0x7f, 0x98, 0x2e, 0x9b, 0x03, 0xd1, 0x6f, 0x08, 0x0a, 0x1a, 0x25, 0x7b, + 0x86, 0xd0, 0x7f, 0x01, 0x33, 0x12, 0x30, 0x98, 0x2e, 0xc2, 0xc4, 0xd1, 0x6f, 0x08, 0x0a, 0x00, 0xb2, 0x0d, 0x2f, + 0xe3, 0x6f, 0x01, 0x2e, 0x80, 0x03, 0x51, 0x30, 0xc7, 0x86, 0x23, 0x2e, 0x21, 0xf2, 0x08, 0xbc, 0xc0, 0x42, 0x98, + 0x2e, 0x91, 0x03, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0xb0, 0x6f, 0x0b, 0xb8, 0x03, 0x2e, 0x1b, 0x00, 0x08, 0x1a, + 0xb0, 0x7f, 0x70, 0x30, 0x04, 0x2f, 0x21, 0x2e, 0x21, 0xf2, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x98, 0x2e, 0x6d, + 0xc0, 0x98, 0x2e, 0x5d, 0xc0, 0xc5, 0x50, 0x98, 0x2e, 0x44, 0xcb, 0xc7, 0x50, 0x98, 0x2e, 0x46, 0xc3, 0xc9, 0x50, + 0x98, 0x2e, 0x53, 0xc7, 0x20, 0x26, 0xc0, 0x6f, 0x02, 0x31, 0x12, 0x42, 0x6b, 0x31, 0x0b, 0x42, 0x37, 0x80, 0x0b, + 0x30, 0x0b, 0x42, 0xf3, 0x37, 0xcf, 0x52, 0xd3, 0x50, 0x44, 0x40, 0xa2, 0x0a, 0x42, 0x42, 0x8b, 0x31, 0x09, 0x2e, + 0x5e, 0xf7, 0xd1, 0x54, 0xe3, 0x08, 0x83, 0x42, 0x1b, 0x42, 0x23, 0x33, 0x4b, 0x00, 0xbc, 0x84, 0x0b, 0x40, 0x33, + 0x30, 0x83, 0x42, 0x0b, 0x42, 0xe0, 0x7f, 0xd1, 0x7f, 0x98, 0x2e, 0x2c, 0x03, 0xd1, 0x6f, 0x80, 0x30, 0x40, 0x42, + 0x03, 0x30, 0xe0, 0x6f, 0xcb, 0x54, 0x04, 0x30, 0x00, 0x2e, 0x00, 0x2e, 0x01, 0x89, 0x62, 0x0e, 0xfa, 0x2f, 0x43, + 0x42, 0x11, 0x30, 0xfb, 0x6f, 0xc0, 0x2e, 0x01, 0x42, 0xb0, 0x5f, 0x50, 0x51, 0x91, 0x54, 0xeb, 0x7f, 0x11, 0x30, + 0x0a, 0x25, 0xff, 0x56, 0x11, 0x5d, 0x95, 0x40, 0xc4, 0x40, 0x25, 0x01, 0xd4, 0x42, 0x4d, 0x17, 0xc4, 0x40, 0x65, + 0x03, 0xd5, 0x42, 0x56, 0x0e, 0xf5, 0x2f, 0x15, 0x57, 0x02, 0x30, 0xc6, 0x40, 0xb1, 0x29, 0xe6, 0x42, 0x00, 0x2e, + 0xc3, 0x40, 0xc0, 0xb2, 0x0a, 0x23, 0x4c, 0x14, 0x71, 0x0e, 0xd4, 0x7f, 0x90, 0x2e, 0x93, 0xb3, 0x03, 0x31, 0xe1, + 0x52, 0xe3, 0x54, 0x5c, 0x05, 0x8a, 0x28, 0x2b, 0x82, 0x03, 0x57, 0xdd, 0x5e, 0x2e, 0x80, 0xde, 0x8c, 0x30, 0x89, + 0xff, 0x29, 0x96, 0x7f, 0x60, 0x7f, 0xc5, 0x7f, 0xb3, 0x7f, 0xa4, 0x7f, 0x82, 0x7f, 0x77, 0x7f, 0x51, 0x7f, 0x00, + 0x2e, 0x90, 0x41, 0x92, 0x41, 0xd3, 0x6f, 0xc0, 0xb2, 0x46, 0x7f, 0x31, 0x7f, 0x08, 0x2f, 0xd0, 0xa0, 0x02, 0x2f, + 0xa0, 0x6f, 0x05, 0x2c, 0x10, 0x10, 0xc6, 0x6f, 0x96, 0x14, 0x03, 0x12, 0x02, 0x0a, 0x05, 0x2e, 0xd3, 0x00, 0x80, + 0x90, 0xd7, 0x54, 0x40, 0x42, 0x4a, 0x2f, 0x41, 0x40, 0x98, 0x2e, 0xd9, 0xc0, 0xdb, 0x54, 0xd9, 0x52, 0x20, 0x7f, + 0x98, 0x2e, 0xfe, 0xc9, 0x72, 0x6f, 0x10, 0x25, 0x98, 0x2e, 0xfe, 0xc9, 0x22, 0x6f, 0xe3, 0x30, 0x10, 0x25, 0x98, + 0x2e, 0x0f, 0xca, 0x91, 0x6f, 0x76, 0x86, 0xd9, 0x52, 0xdd, 0x54, 0xd0, 0x42, 0x93, 0x7f, 0x98, 0x2e, 0xfe, 0xc9, + 0x22, 0x6f, 0xe3, 0x30, 0x10, 0x25, 0x98, 0x2e, 0x0f, 0xca, 0x91, 0x6f, 0xdf, 0x54, 0x40, 0x42, 0x79, 0x80, 0xd9, + 0x52, 0x90, 0x7f, 0x98, 0x2e, 0xfe, 0xc9, 0x82, 0x6f, 0x10, 0x25, 0x98, 0x2e, 0xfe, 0xc9, 0x22, 0x6f, 0xe3, 0x30, + 0x10, 0x25, 0x98, 0x2e, 0x0f, 0xca, 0x93, 0x6f, 0xe1, 0x54, 0xd9, 0x52, 0xd0, 0x42, 0x13, 0x7f, 0x98, 0x2e, 0xfe, + 0xc9, 0x22, 0x6f, 0xe3, 0x30, 0x10, 0x25, 0x98, 0x2e, 0x0f, 0xca, 0x13, 0x6f, 0xe5, 0x5e, 0xc0, 0x42, 0xf7, 0x7f, + 0x00, 0x2e, 0x22, 0x6f, 0x91, 0x6f, 0xe1, 0x5a, 0xe3, 0x58, 0xdf, 0x5c, 0xe1, 0x56, 0x98, 0x2e, 0x67, 0xcc, 0xb1, + 0x6f, 0x02, 0x2c, 0x40, 0x42, 0xb1, 0x6f, 0x41, 0x80, 0x32, 0x6f, 0x81, 0x82, 0x62, 0x6f, 0x46, 0x6f, 0x96, 0x7f, + 0x4a, 0x0e, 0xb0, 0x7f, 0x94, 0x2f, 0xf1, 0x3d, 0x01, 0x55, 0x10, 0x30, 0x53, 0x6f, 0x91, 0x00, 0x21, 0x2e, 0xd3, + 0x00, 0x66, 0x6f, 0xd1, 0x40, 0x80, 0x40, 0x01, 0x00, 0x90, 0x42, 0x09, 0x16, 0x85, 0x40, 0x28, 0x02, 0x80, 0x42, + 0x9a, 0x80, 0xd7, 0x54, 0xd6, 0x7f, 0xc3, 0x7f, 0xb0, 0x7f, 0x98, 0x2e, 0xd9, 0xc0, 0x05, 0x30, 0xf5, 0x7f, 0x20, + 0x25, 0xb1, 0x6f, 0xdd, 0x58, 0xdb, 0x5c, 0xdd, 0x56, 0x98, 0x2e, 0x67, 0xcc, 0xd6, 0x6f, 0xc3, 0x6f, 0xb2, 0x6f, + 0x61, 0x6f, 0xa7, 0x84, 0x90, 0x43, 0x59, 0x0e, 0xdf, 0x2f, 0x10, 0x30, 0x81, 0x40, 0x08, 0x28, 0x90, 0x42, 0xd2, + 0x7f, 0x02, 0xa0, 0x27, 0x2f, 0xd5, 0x54, 0x03, 0x53, 0x03, 0x30, 0x96, 0x40, 0x80, 0x40, 0x08, 0x17, 0x15, 0x30, + 0x65, 0x09, 0xb5, 0x01, 0xc3, 0x02, 0x61, 0xb8, 0x94, 0x8c, 0xb6, 0x7f, 0xbf, 0xbd, 0xd7, 0x54, 0xc1, 0x7f, 0x43, + 0x0a, 0x98, 0x2e, 0xd9, 0xc0, 0xe5, 0x54, 0xf2, 0x7f, 0x20, 0x25, 0xb1, 0x6f, 0xe1, 0x5a, 0xe3, 0x58, 0xdf, 0x5c, + 0xe1, 0x56, 0x98, 0x2e, 0x67, 0xcc, 0xb2, 0x6f, 0x03, 0x30, 0xc1, 0x6f, 0xab, 0x84, 0x0b, 0x5d, 0x93, 0x42, 0x50, + 0x42, 0x4e, 0x0e, 0x93, 0x42, 0xdb, 0x2f, 0x83, 0x42, 0x00, 0x2e, 0xd1, 0x6f, 0x98, 0x2e, 0xb3, 0xc0, 0x61, 0x6f, + 0xc0, 0x7f, 0x98, 0x2e, 0xb3, 0xc0, 0x51, 0x6f, 0xb0, 0x7f, 0x98, 0x2e, 0xb3, 0xc0, 0x00, 0xa0, 0xe7, 0x52, 0x08, + 0x22, 0xb2, 0x6f, 0xc1, 0x6f, 0xa0, 0x7f, 0xb3, 0x30, 0x98, 0x2e, 0x0f, 0xca, 0xb2, 0x6f, 0xb0, 0x7f, 0xb3, 0x30, + 0xe9, 0x52, 0x98, 0x2e, 0x5a, 0xca, 0xd6, 0x6f, 0x02, 0x30, 0x61, 0x6f, 0xd2, 0x7f, 0x90, 0x7f, 0x81, 0x7f, 0xb3, + 0x30, 0x42, 0x40, 0x91, 0x41, 0x76, 0x7f, 0x98, 0x2e, 0x0f, 0xca, 0xd2, 0x6f, 0x81, 0x6f, 0x10, 0x28, 0x41, 0x40, + 0x92, 0x6f, 0xd0, 0x7f, 0xb3, 0x30, 0x98, 0x2e, 0x0f, 0xca, 0x81, 0x6f, 0x76, 0x6f, 0x0b, 0x55, 0x50, 0x42, 0x72, + 0x0e, 0xe9, 0x2f, 0xc2, 0x6f, 0xa1, 0x6f, 0x98, 0x2e, 0xfe, 0xc9, 0x10, 0x25, 0xb3, 0x30, 0x21, 0x25, 0x98, 0x2e, + 0x0f, 0xca, 0x20, 0x25, 0x13, 0x51, 0xeb, 0x52, 0x98, 0x2e, 0xcb, 0xb3, 0xc5, 0x6f, 0x13, 0x51, 0xed, 0x5c, 0x12, + 0x40, 0x75, 0x05, 0x31, 0x30, 0x11, 0x86, 0x05, 0x5d, 0xc1, 0x7f, 0x15, 0x0f, 0x00, 0x40, 0x08, 0x2f, 0x3f, 0x80, + 0x00, 0xa8, 0x21, 0x2e, 0xc3, 0x00, 0x00, 0x30, 0x0a, 0x2f, 0x90, 0x43, 0x09, 0x2c, 0x80, 0x43, 0x01, 0x80, 0x03, + 0xa0, 0x21, 0x2e, 0xc3, 0x00, 0x10, 0x30, 0x01, 0x2f, 0x91, 0x43, 0x80, 0x43, 0x00, 0x2e, 0xef, 0x54, 0x00, 0x6f, + 0x82, 0x0f, 0x03, 0x2f, 0xf0, 0x6e, 0xf1, 0x54, 0x02, 0x0f, 0x14, 0x2f, 0xe1, 0x6e, 0xc3, 0x7f, 0x98, 0x2e, 0x74, + 0xc0, 0xc3, 0x6f, 0xf3, 0x54, 0x01, 0x30, 0xc1, 0x7f, 0xc2, 0x0f, 0x0a, 0x2f, 0xf0, 0x6e, 0xf5, 0x52, 0x81, 0x0e, + 0x11, 0x30, 0x04, 0x2f, 0x00, 0x6f, 0x00, 0xa4, 0x11, 0x30, 0x00, 0x2f, 0x21, 0x30, 0xc1, 0x7f, 0xf1, 0x80, 0xc2, + 0x40, 0x80, 0x90, 0x07, 0x2f, 0x07, 0x55, 0xb8, 0x86, 0x12, 0x30, 0xc1, 0x42, 0xd7, 0x86, 0x23, 0x2e, 0xc5, 0x00, + 0xc2, 0x42, 0x38, 0x8a, 0x02, 0x40, 0x0a, 0x1a, 0x07, 0x55, 0x00, 0x30, 0x02, 0x2f, 0x91, 0x42, 0x0e, 0x2c, 0x80, + 0x42, 0x03, 0x2e, 0xc6, 0x00, 0x12, 0x30, 0x4a, 0x28, 0x23, 0x2e, 0xc6, 0x00, 0x50, 0xa0, 0x04, 0x2f, 0x09, 0x55, + 0x89, 0x82, 0xc3, 0x6f, 0x83, 0x42, 0x40, 0x42, 0x00, 0x2e, 0x05, 0x2e, 0x8e, 0x00, 0x84, 0x8c, 0x47, 0x41, 0xa1, + 0x41, 0xa1, 0x56, 0xc0, 0xb3, 0x0b, 0x09, 0xc4, 0x05, 0x44, 0x89, 0x85, 0x41, 0xf3, 0xbf, 0x82, 0x8d, 0xa7, 0x7f, + 0x94, 0x7f, 0x0a, 0x2f, 0x09, 0x2e, 0xc4, 0x00, 0x00, 0x91, 0x06, 0x2f, 0x81, 0x80, 0xf9, 0x58, 0x0b, 0x40, 0xfb, + 0x5a, 0x84, 0x7f, 0x0c, 0x2c, 0xfb, 0x50, 0x2b, 0x09, 0x98, 0xb9, 0x44, 0x04, 0xd8, 0xba, 0x82, 0x84, 0x53, 0xbc, + 0xf7, 0x5e, 0xb3, 0xbe, 0x8b, 0x40, 0x13, 0xbe, 0x87, 0x7f, 0xb3, 0x30, 0x86, 0x41, 0xfd, 0x52, 0xb2, 0x6f, 0x76, + 0x7f, 0x60, 0x7f, 0x55, 0x7f, 0x4b, 0x7f, 0x34, 0x7f, 0x98, 0x2e, 0x0f, 0xca, 0xd1, 0x6f, 0x88, 0x0e, 0x03, 0x2f, + 0x01, 0x2e, 0xbc, 0x00, 0x00, 0xb2, 0x03, 0x2f, 0x00, 0x30, 0x21, 0x2e, 0xbe, 0x00, 0x10, 0x2d, 0x03, 0x2e, 0xbe, + 0x00, 0x10, 0x30, 0x48, 0x28, 0x72, 0x6f, 0xa8, 0xb9, 0x23, 0x2e, 0xbe, 0x00, 0x0b, 0x0e, 0xc1, 0x6f, 0x0b, 0x55, + 0x03, 0x2f, 0x90, 0x42, 0x91, 0x42, 0x00, 0x30, 0x80, 0x42, 0xb3, 0x30, 0xb2, 0x6f, 0x41, 0x6f, 0x98, 0x2e, 0x0f, + 0xca, 0xd1, 0x6f, 0x08, 0x0f, 0x03, 0x2f, 0x01, 0x2e, 0xbc, 0x00, 0x00, 0x90, 0x03, 0x2f, 0x00, 0x30, 0x21, 0x2e, + 0xc0, 0x00, 0x12, 0x2d, 0x01, 0x2e, 0xc0, 0x00, 0x71, 0x6f, 0xa1, 0x54, 0x01, 0x80, 0x4a, 0x08, 0x21, 0x2e, 0xc0, + 0x00, 0x01, 0x0e, 0x07, 0x2f, 0x0d, 0x51, 0x02, 0x80, 0x01, 0x30, 0x21, 0x42, 0x12, 0x30, 0x01, 0x42, 0x25, 0x2e, + 0xbf, 0x00, 0x91, 0x6f, 0x7e, 0x84, 0x43, 0x40, 0x82, 0x40, 0x7c, 0x80, 0x9a, 0x29, 0x03, 0x40, 0x46, 0x42, 0xc3, + 0xb2, 0x06, 0x30, 0x00, 0x30, 0x2b, 0x2f, 0xc0, 0x6f, 0x00, 0xb2, 0x00, 0x30, 0x27, 0x2f, 0x01, 0x2e, 0xc4, 0x00, + 0x00, 0x90, 0x00, 0x30, 0x05, 0x2f, 0xc2, 0x90, 0x03, 0x2f, 0xc0, 0x6f, 0x03, 0x90, 0x00, 0x30, 0x1c, 0x2f, 0x00, + 0x6f, 0x83, 0x6f, 0x83, 0x0e, 0x00, 0x30, 0x17, 0x2f, 0xf3, 0x6e, 0x50, 0x6f, 0x98, 0x0f, 0x00, 0x30, 0x12, 0x2f, + 0xa0, 0x6f, 0x98, 0x0e, 0x00, 0x30, 0x0e, 0x2f, 0xe3, 0x6e, 0x60, 0x6f, 0x98, 0x0f, 0x00, 0x30, 0x09, 0x2f, 0x30, + 0x6f, 0x98, 0x0e, 0x00, 0x30, 0x05, 0x2f, 0x80, 0xb2, 0x00, 0x30, 0x02, 0x2f, 0x2d, 0x2e, 0xbc, 0x00, 0x10, 0x30, + 0x42, 0x40, 0x82, 0xac, 0x56, 0x82, 0x01, 0x2f, 0x00, 0xb2, 0x05, 0x2f, 0x0d, 0x55, 0x82, 0x84, 0x2d, 0x2e, 0xbf, + 0x00, 0x86, 0x42, 0x00, 0x2e, 0x0f, 0x55, 0x56, 0x42, 0x56, 0x42, 0x4a, 0x0e, 0xfb, 0x2f, 0x2d, 0x2e, 0xd6, 0x00, + 0x01, 0x2d, 0x00, 0x30, 0xeb, 0x6f, 0xb0, 0x5e, 0xb8, 0x2e, 0x70, 0x50, 0x0a, 0x25, 0x39, 0x80, 0xf3, 0x7f, 0x03, + 0x42, 0xa1, 0x7f, 0xc2, 0x7f, 0xd1, 0x7f, 0x03, 0x30, 0x03, 0x43, 0xe4, 0x7f, 0xbb, 0x7f, 0x22, 0x30, 0x10, 0x25, + 0x98, 0x2e, 0x7a, 0xc1, 0xd2, 0x6f, 0x02, 0x17, 0x04, 0x08, 0xc1, 0x6f, 0x0c, 0x09, 0x04, 0x1a, 0x10, 0x30, 0x04, + 0x30, 0x20, 0x22, 0x01, 0xb2, 0x14, 0x2f, 0x17, 0x59, 0x14, 0x09, 0xf3, 0x30, 0x93, 0x08, 0x24, 0xbd, 0x44, 0xba, + 0x94, 0x0a, 0x02, 0x17, 0xf3, 0x6f, 0x4c, 0x08, 0x9a, 0x08, 0x8a, 0x0a, 0x19, 0x53, 0x51, 0x08, 0xa1, 0x58, 0x94, + 0x08, 0x28, 0xbd, 0x98, 0xb8, 0xe4, 0x6f, 0x51, 0x0a, 0x01, 0x43, 0x00, 0x2e, 0xbb, 0x6f, 0x90, 0x5f, 0xb8, 0x2e, + 0x1b, 0x57, 0x05, 0x40, 0x69, 0x18, 0x0d, 0x17, 0xe1, 0x18, 0x19, 0x05, 0x16, 0x25, 0x37, 0x25, 0x4a, 0x17, 0x54, + 0x18, 0xec, 0x18, 0x04, 0x30, 0x64, 0x07, 0xea, 0x18, 0x8e, 0x00, 0x5f, 0x02, 0xd9, 0x56, 0x93, 0x00, 0x4c, 0x02, + 0x2f, 0xb9, 0x91, 0xbc, 0x91, 0x0a, 0x02, 0x42, 0xb8, 0x2e, 0x00, 0x2e, 0x10, 0x24, 0xfa, 0x07, 0x11, 0x24, 0x00, + 0x10, 0x12, 0x24, 0x80, 0x2e, 0x13, 0x24, 0x00, 0xc1, 0x12, 0x42, 0x13, 0x42, 0x41, 0x1a, 0xfb, 0x2f, 0x10, 0x24, + 0x50, 0x32, 0x11, 0x24, 0x21, 0x2e, 0x21, 0x2e, 0x10, 0x00, 0x23, 0x2e, 0x11, 0x00, 0x80, 0x2e, 0x10, 0x00 +}; + +/*! @name Global array that stores the feature input configuration of BMI270_WH */ +const struct bmi2_feature_config bmi270_wh_feat_in[BMI270_WH_MAX_FEAT_IN] = { + { .type = BMI2_CONFIG_ID, .page = BMI2_PAGE_1, .start_addr = BMI270_WH_CONFIG_ID_STRT_ADDR }, + { .type = BMI2_MAX_BURST_LEN, .page = BMI2_PAGE_1, .start_addr = BMI270_WH_MAX_BURST_LEN_STRT_ADDR }, + { .type = BMI2_AXIS_MAP, .page = BMI2_PAGE_1, .start_addr = BMI270_WH_AXIS_MAP_STRT_ADDR }, + { .type = BMI2_NVM_PROG_PREP, .page = BMI2_PAGE_1, .start_addr = BMI270_WH_NVM_PROG_PREP_STRT_ADDR }, + { .type = BMI2_GYRO_GAIN_UPDATE, .page = BMI2_PAGE_1, .start_addr = BMI270_WH_GYRO_GAIN_UPDATE_STRT_ADDR }, + { .type = BMI2_ANY_MOTION, .page = BMI2_PAGE_1, .start_addr = BMI270_WH_ANY_MOT_STRT_ADDR }, + { .type = BMI2_NO_MOTION, .page = BMI2_PAGE_2, .start_addr = BMI270_WH_NO_MOT_STRT_ADDR }, + { .type = BMI2_STEP_COUNTER_PARAMS, .page = BMI2_PAGE_3, .start_addr = BMI270_WH_STEP_CNT_1_STRT_ADDR }, + { .type = BMI2_STEP_DETECTOR, .page = BMI2_PAGE_6, .start_addr = BMI270_WH_STEP_CNT_4_STRT_ADDR }, + { .type = BMI2_STEP_COUNTER, .page = BMI2_PAGE_6, .start_addr = BMI270_WH_STEP_CNT_4_STRT_ADDR }, + { .type = BMI2_STEP_ACTIVITY, .page = BMI2_PAGE_6, .start_addr = BMI270_WH_STEP_CNT_4_STRT_ADDR }, + { .type = BMI2_WRIST_WEAR_WAKE_UP_WH, .page = BMI2_PAGE_2, .start_addr = BMI270_WH_WRIST_WEAR_WAKE_UP_STRT_ADDR }, +}; + +/*! @name Global array that stores the feature output configuration */ +const struct bmi2_feature_config bmi270_wh_feat_out[BMI270_WH_MAX_FEAT_OUT] = { + { .type = BMI2_STEP_COUNTER, .page = BMI2_PAGE_0, .start_addr = BMI270_WH_STEP_CNT_OUT_STRT_ADDR }, + { .type = BMI2_STEP_ACTIVITY, .page = BMI2_PAGE_0, .start_addr = BMI270_WH_STEP_ACT_OUT_STRT_ADDR }, + { .type = BMI2_GYRO_GAIN_UPDATE, .page = BMI2_PAGE_0, .start_addr = BMI270_WH_GYR_USER_GAIN_OUT_STRT_ADDR }, + { .type = BMI2_GYRO_CROSS_SENSE, .page = BMI2_PAGE_0, .start_addr = BMI270_WH_GYRO_CROSS_SENSE_STRT_ADDR }, + { .type = BMI2_NVM_STATUS, .page = BMI2_PAGE_0, .start_addr = BMI270_WH_NVM_VFRM_OUT_STRT_ADDR }, + { .type = BMI2_VFRM_STATUS, .page = BMI2_PAGE_0, .start_addr = BMI270_WH_NVM_VFRM_OUT_STRT_ADDR } +}; + +/*! @name Global array that stores the feature interrupts of BMI270_WH */ +struct bmi2_map_int bmi270wh_map_int[BMI270_WH_MAX_INT_MAP] = { + { .type = BMI2_STEP_COUNTER, .sens_map_int = BMI270_WH_INT_STEP_COUNTER_MASK }, + { .type = BMI2_STEP_DETECTOR, .sens_map_int = BMI270_WH_INT_STEP_DETECTOR_MASK }, + { .type = BMI2_STEP_ACTIVITY, .sens_map_int = BMI270_WH_INT_STEP_ACT_MASK }, + { .type = BMI2_WRIST_WEAR_WAKE_UP_WH, .sens_map_int = BMI270_WH_INT_WRIST_WEAR_WAKEUP_WH_MASK }, + { .type = BMI2_ANY_MOTION, .sens_map_int = BMI270_WH_INT_ANY_MOT_MASK }, + { .type = BMI2_NO_MOTION, .sens_map_int = BMI270_WH_INT_NO_MOT_MASK }, +}; + +/******************************************************************************/ + +/*! Local Function Prototypes + ******************************************************************************/ + +/*! + * @brief This internal API is used to validate the device pointer for + * null conditions. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t null_ptr_check(const struct bmi2_dev *dev); + +/*! + * @brief This internal API enables the selected sensor/features. + * + * @param[in] sensor_sel : Selects the desired sensor. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t sensor_enable(uint64_t sensor_sel, struct bmi2_dev *dev); + +/*! + * @brief This internal API disables the selected sensor/features. + * + * @param[in] sensor_sel : Selects the desired sensor. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t sensor_disable(uint64_t sensor_sel, struct bmi2_dev *dev); + +/*! + * @brief This internal API selects the sensors/features to be enabled or + * disabled. + * + * @param[in] sens_list : Pointer to select the sensor. + * @param[in] n_sens : Number of sensors selected. + * @param[out] sensor_sel : Gets the selected sensor. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t select_sensor(const uint8_t *sens_list, uint8_t n_sens, uint64_t *sensor_sel); + +/*! + * @brief This internal API is used to enable/disable any-motion feature. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables any-motion. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables any-motion. + * BMI2_ENABLE | Enables any-motion. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_any_motion(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to enable/disable no-motion feature. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables no-motion. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables no-motion. + * BMI2_ENABLE | Enables no-motion. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_no_motion(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to enable/disable step detector feature. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables step-detector. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables step detector + * BMI2_ENABLE | Enables step detector + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_step_detector(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to enable/disable step counter feature. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables step counter. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables step counter + * BMI2_ENABLE | Enables step counter + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_step_counter(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to enable/disable step activity detection. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables step activity. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables step activity + * BMI2_ENABLE | Enables step activity + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_step_activity(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to enable/disable gyroscope user gain + * feature. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables gyroscope user gain. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables gyroscope user gain + * BMI2_ENABLE | Enables gyroscope user gain + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_gyro_user_gain(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API enables the wrist wear wake up feature for wearable variant. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables wrist wear wake up. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables wrist wear wake up + * BMI2_ENABLE | Enables wrist wear wake up + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_wrist_wear_wake_up_wh(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets any-motion configurations like axes select, + * duration, threshold and output-configuration. + * + * @param[in] config : Structure instance of bmi2_any_motion_config. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @verbatim + *---------------------------------------------------------------------------- + * bmi2_any_motion_config | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * | Defines the number of consecutive data points for + * | which the threshold condition must be respected, + * | for interrupt assertion. It is expressed in 50 Hz + * duration | samples (20 msec). + * | Range is 0 to 163sec. + * | Default value is 5 = 100ms. + * -------------------------|--------------------------------------------------- + * | Slope threshold value for in 5.11g format. + * threshold | Range is 0 to 1g. + * | Default value is 0xAA = 83mg. + * -------------------------|--------------------------------------------------- + * x_sel, y_sel, z_sel | Selects the feature on a per-axis basis + * -------------------------|--------------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_any_motion_config(const struct bmi2_any_motion_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets no-motion configurations like axes select, + * duration, threshold and output-configuration. + * + * @param[in] config : Structure instance of bmi2_no_motion_config. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @verbatim + *---------------------------------------------------------------------------- + * bmi2_no_motion_config | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * | Defines the number of consecutive data points for + * | which the threshold condition must be respected, + * | for interrupt assertion. It is expressed in 50 Hz + * duration | samples (20 msec). + * | Range is 0 to 163sec. + * | Default value is 5 = 100ms. + * -------------------------|--------------------------------------------------- + * | Slope threshold value for in 5.11g format. + * threshold | Range is 0 to 1g. + * | Default value is 0xAA = 83mg. + * -------------------------|--------------------------------------------------- + * x_sel, y_sel, z_sel | Selects the feature on a per-axis basis + * -------------------------|--------------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_no_motion_config(const struct bmi2_no_motion_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets step counter parameter configurations. + * + * @param[in] step_count_params : Array that stores parameters 1 to 25. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_step_count_params_config(const uint16_t *step_count_params, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets step counter/detector/activity configurations. + * + * @param[in] config : Structure instance of bmi2_step_config. + * @param[in] dev : Structure instance of bmi2_dev. + * + *--------------------------------------------------------------------------- + * bmi2_step_config | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * | The Step-counter will trigger output every time + * | the number of steps are counted. Holds implicitly + * water-mark level | a 20x factor, so the range is 0 to 10230, + * | with resolution of 20 steps. + * -------------------------|--------------------------------------------------- + * reset counter | Flag to reset the counted steps. + * -------------------------|--------------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_step_config(const struct bmi2_step_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets wrist wear wake-up configurations like + * output-configuration for wearable variant. + * + * @param[in] config : Structure instance of + * bmi2_wrist_wear_wake_up_config. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @verbatim + *-------------------------------------|---------------------------------------- + * bmi2_wrist_wear_wake_up_wh_config | + * Structure parameters | Description + *-------------------------------------|------------------------------------------- + * | To set the wrist wear wake-up parameters like + * | min_angle_focus, min_angle_nonfocus, + * wrist_wear_wake_wh_params | angle_landscape_left, angle_landscape_right, + * | angle_potrait_up and down. + * ------------------------------------|------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_wrist_wear_wake_up_wh_config(const struct bmi2_wrist_wear_wake_up_wh_config *config, + struct bmi2_dev *dev); + +/*! + * @brief This internal API gets any-motion configurations like axes select, + * duration, threshold and output-configuration. + * + * @param[out] config : Structure instance of bmi2_any_motion_config. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @verbatim + *---------------------------------------------------------------------------- + * bmi2_any_motion_config | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * | Defines the number of consecutive data points for + * | which the threshold condition must be respected, + * | for interrupt assertion. It is expressed in 50 Hz + * duration | samples (20 msec). + * | Range is 0 to 163sec. + * | Default value is 5 = 100ms. + * -------------------------|--------------------------------------------------- + * | Slope threshold value for in 5.11g format. + * threshold | Range is 0 to 1g. + * | Default value is 0xAA = 83mg. + * -------------------------|--------------------------------------------------- + * x_sel, y_sel, z_sel | Selects the feature on a per-axis basis + * -------------------------|--------------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_any_motion_config(struct bmi2_any_motion_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets no-motion configurations like axes select, + * duration, threshold and output-configuration. + * + * @param[out] config : Structure instance of bmi2_no_motion_config. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @verbatim + *---------------------------------------------------------------------------- + * bmi2_no_motion_config | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * | Defines the number of consecutive data points for + * | which the threshold condition must be respected, + * | for interrupt assertion. It is expressed in 50 Hz + * duration | samples (20 msec). + * | Range is 0 to 163sec. + * | Default value is 5 = 100ms. + * -------------------------|--------------------------------------------------- + * | Slope threshold value for in 5.11g format. + * threshold | Range is 0 to 1g. + * | Default value is 0xAA = 83mg. + * -------------------------|--------------------------------------------------- + * x_sel, y_sel, z_sel | Selects the feature on a per-axis basis + * -------------------------|--------------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_no_motion_config(struct bmi2_no_motion_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets step counter parameter configurations. + * + * @param[in] step_count_params : Array that stores parameters 1 to 25. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_step_count_params_config(uint16_t *step_count_params, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets step counter/detector/activity configurations. + * + * @param[out] config : Structure instance of bmi2_step_config. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @verbatim + *---------------------------------------------------------------------------- + * bmi2_step_config | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * | The Step-counter will trigger output every time + * | the number of steps are counted. Holds implicitly + * water-mark level | a 20x factor, so the range is 0 to 10230, + * | with resolution of 20 steps. + * -------------------------|--------------------------------------------------- + * reset counter | Flag to reset the counted steps. + * -------------------------|--------------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_step_config(struct bmi2_step_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets wrist wear wake-up configurations like + * output-configuration for wearable variant. + * + * @param[out] config : Structure instance of + * bmi2_wrist_wear_wake_up_config. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @verbatim + *----------------------------------------------------------------------------- + * bmi2_wrist_wear_wake_up_config | + * Structure parameters | Description + *----------------------------------|------------------------------------------- + * | To get the wrist wear wake-up parameters like + * | min_angle_focus, min_angle_nonfocus, + * wrist_wear_wake_params | angle_landscape_left, angle_landscape_right, + * | angle_potrait_up and down. + * ---------------------------------|------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_wrist_wear_wake_up_wh_config(struct bmi2_wrist_wear_wake_up_wh_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets the output values of step activity. + * + * @param[out] step_act : Pointer to the stored step activity data. + * @param[in] dev : Structure instance of bmi2_dev. + * + * *step_act | Output + * -----------|------------ + * 0x00 | STILL + * 0x01 | WALKING + * 0x02 | RUNNING + * 0x03 | UNKNOWN + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_step_activity_output(uint8_t *step_act, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets the output values of step counter. + * + * @param[out] step_count : Pointer to the stored step counter data. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_step_counter_output(uint32_t *step_count, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets the error status related to NVM. + * + * @param[out] nvm_err_stat : Stores the NVM error status. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_nvm_error_status(struct bmi2_nvm_err_status *nvm_err_stat, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets the error status related to virtual frames. + * + * @param[out] vfrm_err_stat : Stores the VFRM related error status. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_vfrm_error_status(struct bmi2_vfrm_err_status *vfrm_err_stat, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to get enable status of gyroscope user gain + * update. + * + * @param[out] status : Stores status of gyroscope user gain update. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_user_gain_upd_status(uint8_t *status, struct bmi2_dev *dev); + +/*! + * @brief This internal API enables/disables compensation of the gain defined + * in the GAIN register. + * + * @param[in] enable : Enables/Disables gain compensation + * @param[in] dev : Structure instance of bmi2_dev. + * + * enable | Description + * -------------|--------------- + * BMI2_ENABLE | Enable gain compensation. + * BMI2_DISABLE | Disable gain compensation. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t enable_gyro_gain(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to extract the output feature configuration + * details like page and start address from the look-up table. + * + * @param[out] feat_output : Structure that stores output feature + * configurations. + * @param[in] type : Type of feature or sensor. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Returns the feature found flag. + * + * @retval BMI2_FALSE : Feature not found + * BMI2_TRUE : Feature found + */ +static uint8_t extract_output_feat_config(struct bmi2_feature_config *feat_output, + uint8_t type, + const struct bmi2_dev *dev); + +/***************************************************************************/ + +/*! User Interface Definitions + ****************************************************************************/ + +/*! + * @brief This API: + * 1) updates the device structure with address of the configuration file. + * 2) Initializes BMI270_WH sensor. + * 3) Writes the configuration file. + * 4) Updates the feature offset parameters in the device structure. + * 5) Updates the maximum number of pages, in the device structure. + */ +int8_t bmi270_wh_init(struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + /* Assign chip id of BMI270_WH */ + dev->chip_id = BMI270_WH_CHIP_ID; + + /* get the size of config array */ + dev->config_size = sizeof(bmi270_wh_config_file); + + /* Enable the variant specific features if any */ + dev->variant_feature = BMI2_GYRO_CROSS_SENS_ENABLE | BMI2_CRT_RTOSK_ENABLE; + + /* An extra dummy byte is read during SPI read */ + if (dev->intf == BMI2_SPI_INTF) + { + dev->dummy_byte = 1; + } + else + { + dev->dummy_byte = 0; + } + + /* If configuration file pointer is not assigned any address */ + if (!dev->config_file_ptr) + { + /* Give the address of the configuration file array to + * the device pointer + */ + dev->config_file_ptr = bmi270_wh_config_file; + } + + /* Initialize BMI2 sensor */ + rslt = bmi2_sec_init(dev); + if (rslt == BMI2_OK) + { + /* Assign the offsets of the feature input + * configuration to the device structure + */ + dev->feat_config = bmi270_wh_feat_in; + + /* Assign the offsets of the feature output to + * the device structure + */ + dev->feat_output = bmi270_wh_feat_out; + + /* Assign the maximum number of pages to the + * device structure + */ + dev->page_max = BMI270_WH_MAX_PAGE_NUM; + + /* Assign maximum number of input sensors/ + * features to device structure + */ + dev->input_sens = BMI270_WH_MAX_FEAT_IN; + + /* Assign maximum number of output sensors/ + * features to device structure + */ + dev->out_sens = BMI270_WH_MAX_FEAT_OUT; + + /* Assign the offsets of the feature interrupt + * to the device structure + */ + dev->map_int = bmi270wh_map_int; + + /* Assign maximum number of feature interrupts + * to device structure + */ + dev->sens_int_map = BMI270_WH_MAX_INT_MAP; + + /* Get the gyroscope cross axis sensitivity */ + rslt = bmi2_get_gyro_cross_sense(dev); + } + } + + return rslt; +} + +/*! + * @brief This API selects the sensors/features to be enabled. + */ +int8_t bmi270_wh_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to select sensor */ + uint64_t sensor_sel = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_list != NULL)) + { + /* Get the selected sensors */ + rslt = select_sensor(sens_list, n_sens, &sensor_sel); + if (rslt == BMI2_OK) + { + /* Enable the selected sensors */ + rslt = sensor_enable(sensor_sel, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API selects the sensors/features to be disabled. + */ +int8_t bmi270_wh_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to select sensor */ + uint64_t sensor_sel = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_list != NULL)) + { + /* Get the selected sensors */ + rslt = select_sensor(sens_list, n_sens, &sensor_sel); + if (rslt == BMI2_OK) + { + /* Disable the selected sensors */ + rslt = sensor_disable(sensor_sel, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API sets the sensor/feature configuration. + */ +int8_t bmi270_wh_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_cfg != NULL)) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + + for (loop = 0; loop < n_sens; loop++) + { + if ((sens_cfg[loop].type == BMI2_ACCEL) || (sens_cfg[loop].type == BMI2_GYRO) || + (sens_cfg[loop].type == BMI2_AUX) || (sens_cfg[loop].type == BMI2_GYRO_GAIN_UPDATE)) + { + rslt = bmi2_set_sensor_config(&sens_cfg[loop], 1, dev); + } + else + { + /* Disable Advance power save if enabled for auxiliary + * and feature configurations + */ + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if + * enabled + */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + switch (sens_cfg[loop].type) + { + /* Set any motion configuration */ + case BMI2_ANY_MOTION: + rslt = set_any_motion_config(&sens_cfg[loop].cfg.any_motion, dev); + break; + + /* Set no motion configuration */ + case BMI2_NO_MOTION: + rslt = set_no_motion_config(&sens_cfg[loop].cfg.no_motion, dev); + break; + + /* Set the step counter parameters */ + case BMI2_STEP_COUNTER_PARAMS: + rslt = set_step_count_params_config(sens_cfg[loop].cfg.step_counter_params, dev); + break; + + /* Set step counter/detector/activity configuration */ + case BMI2_STEP_DETECTOR: + case BMI2_STEP_COUNTER: + case BMI2_STEP_ACTIVITY: + rslt = set_step_config(&sens_cfg[loop].cfg.step_counter, dev); + break; + + /* Set the wrist wear wake-up configuration for wearable variant */ + case BMI2_WRIST_WEAR_WAKE_UP_WH: + rslt = set_wrist_wear_wake_up_wh_config(&sens_cfg[loop].cfg.wrist_wear_wake_up_wh, dev); + break; + + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + } + + /* Return error if any of the set configurations fail */ + if (rslt != BMI2_OK) + { + break; + } + } + } + + /* Enable Advance power save if disabled while configuring and + * not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API gets the sensor/feature configuration. + */ +int8_t bmi270_wh_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_cfg != NULL)) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + for (loop = 0; loop < n_sens; loop++) + { + if ((sens_cfg[loop].type == BMI2_ACCEL) || (sens_cfg[loop].type == BMI2_GYRO) || + (sens_cfg[loop].type == BMI2_AUX) || (sens_cfg[loop].type == BMI2_GYRO_GAIN_UPDATE)) + { + rslt = bmi2_get_sensor_config(&sens_cfg[loop], 1, dev); + } + else + { + /* Disable Advance power save if enabled for auxiliary + * and feature configurations + */ + if ((sens_cfg[loop].type >= BMI2_MAIN_SENS_MAX_NUM) || (sens_cfg[loop].type == BMI2_AUX)) + { + + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if + * enabled + */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + } + + if (rslt == BMI2_OK) + { + switch (sens_cfg[loop].type) + { + /* Get any motion configuration */ + case BMI2_ANY_MOTION: + rslt = get_any_motion_config(&sens_cfg[loop].cfg.any_motion, dev); + break; + + /* Get no motion configuration */ + case BMI2_NO_MOTION: + rslt = get_no_motion_config(&sens_cfg[loop].cfg.no_motion, dev); + break; + + /* Set the step counter parameters */ + case BMI2_STEP_COUNTER_PARAMS: + rslt = get_step_count_params_config(sens_cfg[loop].cfg.step_counter_params, dev); + break; + + /* Get step counter/detector/activity configuration */ + case BMI2_STEP_DETECTOR: + case BMI2_STEP_COUNTER: + case BMI2_STEP_ACTIVITY: + rslt = get_step_config(&sens_cfg[loop].cfg.step_counter, dev); + break; + + /* Get the wrist wear wake-up configuration for wearable variant */ + case BMI2_WRIST_WEAR_WAKE_UP_WH: + rslt = get_wrist_wear_wake_up_wh_config(&sens_cfg[loop].cfg.wrist_wear_wake_up_wh, dev); + break; + + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + } + + /* Return error if any of the get configurations fail */ + if (rslt != BMI2_OK) + { + break; + } + } + } + + /* Enable Advance power save if disabled while configuring and + * not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API gets the sensor/feature data for accelerometer, gyroscope, + * auxiliary sensor, step counter, high-g, gyroscope user-gain update, + * orientation, gyroscope cross sensitivity and error status for NVM and VFRM. + */ +int8_t bmi270_wh_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sensor_data != NULL)) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + for (loop = 0; loop < n_sens; loop++) + { + if ((sensor_data[loop].type == BMI2_ACCEL) || (sensor_data[loop].type == BMI2_GYRO) || + (sensor_data[loop].type == BMI2_AUX) || (sensor_data[loop].type == BMI2_GYRO_GAIN_UPDATE) || + (sensor_data[loop].type == BMI2_GYRO_CROSS_SENSE)) + { + rslt = bmi2_get_sensor_data(&sensor_data[loop], 1, dev); + } + else + { + /* Disable Advance power save if enabled for feature + * configurations + */ + if (sensor_data[loop].type >= BMI2_MAIN_SENS_MAX_NUM) + { + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if + * enabled + */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + } + + if (rslt == BMI2_OK) + { + switch (sensor_data[loop].type) + { + case BMI2_STEP_COUNTER: + + /* Get step counter output */ + rslt = get_step_counter_output(&sensor_data[loop].sens_data.step_counter_output, dev); + break; + case BMI2_STEP_ACTIVITY: + + /* Get step activity output */ + rslt = get_step_activity_output(&sensor_data[loop].sens_data.activity_output, dev); + break; + case BMI2_NVM_STATUS: + + /* Get NVM error status */ + rslt = get_nvm_error_status(&sensor_data[loop].sens_data.nvm_status, dev); + break; + case BMI2_VFRM_STATUS: + + /* Get VFRM error status */ + rslt = get_vfrm_error_status(&sensor_data[loop].sens_data.vfrm_status, dev); + break; + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + + /* Return error if any of the get sensor data fails */ + if (rslt != BMI2_OK) + { + break; + } + } + } + + /* Enable Advance power save if disabled while + * configuring and not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API updates the gyroscope user-gain. + */ +int8_t bmi270_wh_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to select sensor */ + uint8_t sens_sel[2] = { BMI2_GYRO, BMI2_GYRO_GAIN_UPDATE }; + + /* Structure to define sensor configurations */ + struct bmi2_sens_config sens_cfg; + + /* Variable to store status of user-gain update module */ + uint8_t status = 0; + + /* Variable to define count */ + uint8_t count = 100; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (user_gain != NULL)) + { + /* Select type of feature */ + sens_cfg.type = BMI2_GYRO_GAIN_UPDATE; + + /* Get the user gain configurations */ + rslt = bmi270_wh_get_sensor_config(&sens_cfg, 1, dev); + if (rslt == BMI2_OK) + { + /* Get the user-defined ratio */ + sens_cfg.cfg.gyro_gain_update = *user_gain; + + /* Set rate ratio for all axes */ + rslt = bmi270_wh_set_sensor_config(&sens_cfg, 1, dev); + } + + /* Disable gyroscope */ + if (rslt == BMI2_OK) + { + rslt = bmi270_wh_sensor_disable(&sens_sel[0], 1, dev); + } + + /* Enable gyroscope user-gain update module */ + if (rslt == BMI2_OK) + { + rslt = bmi270_wh_sensor_enable(&sens_sel[1], 1, dev); + } + + /* Set the command to trigger the computation */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_command_register(BMI2_USR_GAIN_CMD, dev); + } + + if (rslt == BMI2_OK) + { + /* Poll until enable bit of user-gain update is 0 */ + while (count--) + { + rslt = get_user_gain_upd_status(&status, dev); + if ((rslt == BMI2_OK) && (status == 0)) + { + /* Enable compensation of gain defined + * in the GAIN register + */ + rslt = enable_gyro_gain(BMI2_ENABLE, dev); + + /* Enable gyroscope */ + if (rslt == BMI2_OK) + { + rslt = bmi270_wh_sensor_enable(&sens_sel[0], 1, dev); + } + + break; + } + + dev->delay_us(10000, dev->intf_ptr); + } + + /* Return error if user-gain update is failed */ + if ((rslt == BMI2_OK) && (status != 0)) + { + rslt = BMI2_E_GYR_USER_GAIN_UPD_FAIL; + } + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API reads the compensated gyroscope user-gain values. + */ +int8_t bmi270_wh_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define register data */ + uint8_t reg_data[3] = { 0 }; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (gyr_usr_gain != NULL)) + { + /* Get the gyroscope compensated gain values */ + rslt = bmi2_get_regs(BMI2_GYR_USR_GAIN_0_ADDR, reg_data, 3, dev); + if (rslt == BMI2_OK) + { + /* Gyroscope user gain correction X-axis */ + gyr_usr_gain->x = (int8_t)BMI2_GET_BIT_POS0(reg_data[0], BMI2_GYR_USR_GAIN_X); + + /* Gyroscope user gain correction Y-axis */ + gyr_usr_gain->y = (int8_t)BMI2_GET_BIT_POS0(reg_data[1], BMI2_GYR_USR_GAIN_Y); + + /* Gyroscope user gain correction z-axis */ + gyr_usr_gain->z = (int8_t)BMI2_GET_BIT_POS0(reg_data[2], BMI2_GYR_USR_GAIN_Z); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API maps/unmaps feature interrupts to that of interrupt pins. + */ +int8_t bmi270_wh_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_int != NULL)) + { + for (loop = 0; loop < n_sens; loop++) + { + switch (sens_int[loop].type) + { + case BMI2_ANY_MOTION: + case BMI2_NO_MOTION: + case BMI2_STEP_COUNTER: + case BMI2_STEP_DETECTOR: + case BMI2_STEP_ACTIVITY: + case BMI2_WRIST_WEAR_WAKE_UP_WH: + + rslt = bmi2_map_feat_int(sens_int[loop].type, sens_int[loop].hw_int_pin, dev); + break; + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + + /* Return error if interrupt mapping fails */ + if (rslt != BMI2_OK) + { + break; + } + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/***************************************************************************/ + +/*! Local Function Definitions + ****************************************************************************/ + +/*! + * @brief This internal API is used to validate the device structure pointer for + * null conditions. + */ +static int8_t null_ptr_check(const struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + if ((dev == NULL) || (dev->read == NULL) || (dev->write == NULL) || (dev->delay_us == NULL)) + { + /* Device structure pointer is not valid */ + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This internal API selects the sensor/features to be enabled or + * disabled. + */ +static int8_t select_sensor(const uint8_t *sens_list, uint8_t n_sens, uint64_t *sensor_sel) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Variable to define loop */ + uint8_t count; + + for (count = 0; count < n_sens; count++) + { + switch (sens_list[count]) + { + case BMI2_ACCEL: + *sensor_sel |= BMI2_ACCEL_SENS_SEL; + break; + case BMI2_GYRO: + *sensor_sel |= BMI2_GYRO_SENS_SEL; + break; + case BMI2_AUX: + *sensor_sel |= BMI2_AUX_SENS_SEL; + break; + case BMI2_TEMP: + *sensor_sel |= BMI2_TEMP_SENS_SEL; + break; + case BMI2_ANY_MOTION: + *sensor_sel |= BMI2_ANY_MOT_SEL; + break; + case BMI2_NO_MOTION: + *sensor_sel |= BMI2_NO_MOT_SEL; + break; + case BMI2_STEP_DETECTOR: + *sensor_sel |= BMI2_STEP_DETECT_SEL; + break; + case BMI2_STEP_COUNTER: + *sensor_sel |= BMI2_STEP_COUNT_SEL; + break; + case BMI2_STEP_ACTIVITY: + *sensor_sel |= BMI2_STEP_ACT_SEL; + break; + case BMI2_GYRO_GAIN_UPDATE: + *sensor_sel |= BMI2_GYRO_GAIN_UPDATE_SEL; + break; + case BMI2_WRIST_WEAR_WAKE_UP_WH: + *sensor_sel |= BMI2_WRIST_WEAR_WAKE_UP_WH_SEL; + break; + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + } + + return rslt; +} + +/*! + * @brief This internal API enables the selected sensor/features. + */ +static int8_t sensor_enable(uint64_t sensor_sel, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store register values */ + uint8_t reg_data = 0; + + /* Variable to define loop */ + uint8_t loop = 1; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + /* Enable accelerometer */ + if (sensor_sel & BMI2_ACCEL_SENS_SEL) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_ACC_EN, BMI2_ENABLE); + } + + /* Enable gyroscope */ + if (sensor_sel & BMI2_GYRO_SENS_SEL) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_EN, BMI2_ENABLE); + } + + /* Enable auxiliary sensor */ + if (sensor_sel & BMI2_AUX_SENS_SEL) + { + reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_AUX_EN, BMI2_ENABLE); + } + + /* Enable temperature sensor */ + if (sensor_sel & BMI2_TEMP_SENS_SEL) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_TEMP_EN, BMI2_ENABLE); + } + + /* Enable the sensors that are set in the power control register */ + if (sensor_sel & BMI2_MAIN_SENSORS) + { + rslt = bmi2_set_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + } + } + + if ((rslt == BMI2_OK) && (sensor_sel & ~(BMI2_MAIN_SENSORS))) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if enabled */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + while (loop--) + { + /* Enable any motion feature */ + if (sensor_sel & BMI2_ANY_MOT_SEL) + { + rslt = set_any_motion(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_ANY_MOT_SEL; + } + else + { + break; + } + } + + /* Enable no motion feature */ + if (sensor_sel & BMI2_NO_MOT_SEL) + { + rslt = set_no_motion(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_NO_MOT_SEL; + } + else + { + break; + } + } + + /* Enable step detector feature */ + if (sensor_sel & BMI2_STEP_DETECT_SEL) + { + rslt = set_step_detector(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_STEP_DETECT_SEL; + } + else + { + break; + } + } + + /* Enable step counter feature */ + if (sensor_sel & BMI2_STEP_COUNT_SEL) + { + rslt = set_step_counter(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_STEP_COUNT_SEL; + } + else + { + break; + } + } + + /* Enable step activity feature */ + if (sensor_sel & BMI2_STEP_ACT_SEL) + { + rslt = set_step_activity(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_STEP_ACT_SEL; + } + else + { + break; + } + } + + /* Enable gyroscope user gain */ + if (sensor_sel & BMI2_GYRO_GAIN_UPDATE_SEL) + { + rslt = set_gyro_user_gain(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_GYRO_GAIN_UPDATE_SEL; + } + else + { + break; + } + } + + /* Enable wrist wear wake-up feature for wearable variant */ + if (sensor_sel & BMI2_WRIST_WEAR_WAKE_UP_WH_SEL) + { + rslt = set_wrist_wear_wake_up_wh(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_WRIST_WEAR_WAKE_UP_WH_SEL; + } + else + { + break; + } + } + } + + /* Enable Advance power save if disabled while + * configuring and not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + } + + return rslt; +} + +/*! + * @brief This internal API disables the selected sensors/features. + */ +static int8_t sensor_disable(uint64_t sensor_sel, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store register values */ + uint8_t reg_data = 0; + + /* Variable to define loop */ + uint8_t loop = 1; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + /* Disable accelerometer */ + if (sensor_sel & BMI2_ACCEL_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_ACC_EN); + } + + /* Disable gyroscope */ + if (sensor_sel & BMI2_GYRO_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_GYR_EN); + } + + /* Disable auxiliary sensor */ + if (sensor_sel & BMI2_AUX_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_AUX_EN); + } + + /* Disable temperature sensor */ + if (sensor_sel & BMI2_TEMP_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_TEMP_EN); + } + + /* Disable the sensors that are set in the power control register */ + if (sensor_sel & BMI2_MAIN_SENSORS) + { + rslt = bmi2_set_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + } + } + + if ((rslt == BMI2_OK) && (sensor_sel & ~(BMI2_MAIN_SENSORS))) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if enabled */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + while (loop--) + { + /* Disable any-motion feature */ + if (sensor_sel & BMI2_ANY_MOT_SEL) + { + rslt = set_any_motion(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_ANY_MOT_SEL; + } + else + { + break; + } + } + + /* Disable no-motion feature */ + if (sensor_sel & BMI2_NO_MOT_SEL) + { + rslt = set_no_motion(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_NO_MOT_SEL; + } + else + { + break; + } + } + + /* Disable step detector feature */ + if (sensor_sel & BMI2_STEP_DETECT_SEL) + { + rslt = set_step_detector(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_STEP_DETECT_SEL; + } + else + { + break; + } + } + + /* Disable step counter feature */ + if (sensor_sel & BMI2_STEP_COUNT_SEL) + { + rslt = set_step_counter(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_STEP_COUNT_SEL; + } + else + { + break; + } + } + + /* Disable step activity feature */ + if (sensor_sel & BMI2_STEP_ACT_SEL) + { + rslt = set_step_activity(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_STEP_ACT_SEL; + } + else + { + break; + } + } + + /* Disable gyroscope user gain */ + if (sensor_sel & BMI2_GYRO_GAIN_UPDATE_SEL) + { + rslt = set_gyro_user_gain(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_GYRO_GAIN_UPDATE_SEL; + } + else + { + break; + } + } + + /* Disable wrist wear wake-up feature for wearable variant */ + if (sensor_sel & BMI2_WRIST_WEAR_WAKE_UP_WH_SEL) + { + rslt = set_wrist_wear_wake_up_wh(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_WRIST_WEAR_WAKE_UP_WH_SEL; + } + else + { + break; + } + } + + /* Enable Advance power save if disabled while + * configuring and not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + } + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable any motion feature. + */ +static int8_t set_any_motion(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for any-motion */ + struct bmi2_feature_config any_mot_config = { 0, 0, 0 }; + + /* Search for any-motion feature and extract its configurations details */ + feat_found = bmi2_extract_input_feat_config(&any_mot_config, BMI2_ANY_MOTION, dev); + if (feat_found) + { + /* Get the configuration from the page where any-motion feature resides */ + rslt = bmi2_get_feat_config(any_mot_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of any-motion axes */ + idx = any_mot_config.start_addr + BMI2_ANY_MOT_FEAT_EN_OFFSET; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_ANY_NO_MOT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable no-motion feature. + */ +static int8_t set_no_motion(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for no-motion */ + struct bmi2_feature_config no_mot_config = { 0, 0, 0 }; + + /* Search for no-motion feature and extract its configurations details */ + feat_found = bmi2_extract_input_feat_config(&no_mot_config, BMI2_NO_MOTION, dev); + if (feat_found) + { + /* Get the configuration from the page where any/no-motion feature resides */ + rslt = bmi2_get_feat_config(no_mot_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of no-motion axes */ + idx = no_mot_config.start_addr + BMI2_NO_MOT_FEAT_EN_OFFSET; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_ANY_NO_MOT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable step detector feature. + */ +static int8_t set_step_detector(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step detector */ + struct bmi2_feature_config step_det_config = { 0, 0, 0 }; + + /* Search for step detector feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_det_config, BMI2_STEP_DETECTOR, dev); + if (feat_found) + { + /* Get the configuration from the page where step detector feature resides */ + rslt = bmi2_get_feat_config(step_det_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of step detector */ + idx = step_det_config.start_addr + BMI2_STEP_COUNT_FEAT_EN_OFFSET; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_STEP_DET_FEAT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable step counter feature. + */ +static int8_t set_step_counter(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step counter */ + struct bmi2_feature_config step_count_config = { 0, 0, 0 }; + + /* Search for step counter feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); + if (feat_found) + { + /* Get the configuration from the page where step-counter feature resides */ + rslt = bmi2_get_feat_config(step_count_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of step counter */ + idx = step_count_config.start_addr + BMI2_STEP_COUNT_FEAT_EN_OFFSET; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_STEP_COUNT_FEAT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable step activity detection. + */ +static int8_t set_step_activity(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step activity */ + struct bmi2_feature_config step_act_config = { 0, 0, 0 }; + + /* Search for step activity feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_act_config, BMI2_STEP_ACTIVITY, dev); + if (feat_found) + { + /* Get the configuration from the page where step-activity + * feature resides + */ + rslt = bmi2_get_feat_config(step_act_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of step activity */ + idx = step_act_config.start_addr + BMI2_STEP_COUNT_FEAT_EN_OFFSET; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_STEP_ACT_FEAT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API enables the wrist wear wake up feature. + */ +static int8_t set_wrist_wear_wake_up_wh(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for wrist wear wake up */ + struct bmi2_feature_config wrist_wake_up_cfg = { 0, 0, 0 }; + + /* Search for wrist wear wake up and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&wrist_wake_up_cfg, BMI2_WRIST_WEAR_WAKE_UP_WH, dev); + if (feat_found) + { + /* Get the configuration from the page where wrist wear wake up + * feature resides + */ + rslt = bmi2_get_feat_config(wrist_wake_up_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of wrist wear wake up */ + idx = wrist_wake_up_cfg.start_addr; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_WRIST_WEAR_WAKE_UP_FEAT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable gyroscope user gain + * feature. + */ +static int8_t set_gyro_user_gain(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for gyroscope user gain */ + struct bmi2_feature_config gyr_user_gain_cfg = { 0, 0, 0 }; + + /* Search for user gain feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&gyr_user_gain_cfg, BMI2_GYRO_GAIN_UPDATE, dev); + if (feat_found) + { + /* Get the configuration from the page where user gain feature resides */ + rslt = bmi2_get_feat_config(gyr_user_gain_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of user gain */ + idx = gyr_user_gain_cfg.start_addr + BMI2_GYR_USER_GAIN_FEAT_EN_OFFSET; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_GYR_USER_GAIN_FEAT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API sets any-motion configurations like axes select, + * duration, threshold and output-configuration. + */ +static int8_t set_any_motion_config(const struct bmi2_any_motion_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define count */ + uint8_t index = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for any motion */ + struct bmi2_feature_config any_mot_config = { 0, 0, 0 }; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for any-motion feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&any_mot_config, BMI2_ANY_MOTION, dev); + if (feat_found) + { + /* Get the configuration from the page where any-motion feature resides */ + rslt = bmi2_get_feat_config(any_mot_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for any-motion select */ + idx = any_mot_config.start_addr; + + /* Get offset in words since all the features are set in words length */ + idx = idx / 2; + + /* Set duration */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_ANY_NO_MOT_DUR, config->duration); + + /* Set x-select */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_ANY_NO_MOT_X_SEL, config->select_x); + + /* Set y-select */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_ANY_NO_MOT_Y_SEL, config->select_y); + + /* Set z-select */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_ANY_NO_MOT_Z_SEL, config->select_z); + + /* Increment offset by 1 word to set threshold and output configuration */ + idx++; + + /* Set threshold */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_ANY_NO_MOT_THRES, config->threshold); + + /* Increment offset by 1 more word to get the total length in words */ + idx++; + + /* Get total length in bytes to copy from local pointer to the array */ + idx = (uint8_t)(idx * 2) - any_mot_config.start_addr; + + /* Copy the bytes to be set back to the array */ + for (index = 0; index < idx; index++) + { + feat_config[any_mot_config.start_addr + + index] = *((uint8_t *) data_p + any_mot_config.start_addr + index); + } + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API sets no-motion configurations like axes select, + * duration, threshold and output-configuration. + */ +static int8_t set_no_motion_config(const struct bmi2_no_motion_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define count */ + uint8_t index = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for no-motion */ + struct bmi2_feature_config no_mot_config = { 0, 0, 0 }; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for no-motion feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&no_mot_config, BMI2_NO_MOTION, dev); + if (feat_found) + { + /* Get the configuration from the page where no-motion feature resides */ + rslt = bmi2_get_feat_config(no_mot_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for no-motion select */ + idx = no_mot_config.start_addr; + + /* Get offset in words since all the features are set in words length */ + idx = idx / 2; + + /* Set duration */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_ANY_NO_MOT_DUR, config->duration); + + /* Set x-select */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_ANY_NO_MOT_X_SEL, config->select_x); + + /* Set y-select */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_ANY_NO_MOT_Y_SEL, config->select_y); + + /* Set z-select */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_ANY_NO_MOT_Z_SEL, config->select_z); + + /* Increment offset by 1 word to set threshold and output configuration */ + idx++; + + /* Set threshold */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_ANY_NO_MOT_THRES, config->threshold); + + /* Increment offset by 1 more word to get the total length in words */ + idx++; + + /* Get total length in bytes to copy from local pointer to the array */ + idx = (uint8_t)(idx * 2) - no_mot_config.start_addr; + + /* Copy the bytes to be set back to the array */ + for (index = 0; index < idx; index++) + { + feat_config[no_mot_config.start_addr + + index] = *((uint8_t *) data_p + no_mot_config.start_addr + index); + } + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API sets step counter parameter configurations. + */ +static int8_t set_step_count_params_config(const uint16_t *step_count_params, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define index */ + uint8_t index = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step counter parameters */ + struct bmi2_feature_config step_params_config = { 0, 0, 0 }; + + /* Variable to index the page number */ + uint8_t page_idx; + + /* Variable to define the start page */ + uint8_t start_page; + + /* Variable to define start address of the parameters */ + uint8_t start_addr; + + /* Variable to define number of bytes */ + uint8_t n_bytes = (BMI2_STEP_CNT_N_PARAMS * 2); + + /* Variable to store number of pages */ + uint8_t n_pages = (n_bytes / 16); + + /* Variable to define the end page */ + uint8_t end_page; + + /* Variable to define the remaining bytes to be read */ + uint8_t remain_len; + + /* Variable to define the maximum words(16 bytes or 8 words) to be read in a page */ + uint8_t max_len = 8; + + /* Variable index bytes in a page */ + uint8_t page_byte_idx; + + /* Variable to index the parameters */ + uint8_t param_idx = 0; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for step counter parameter feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_params_config, BMI2_STEP_COUNTER_PARAMS, dev); + if (feat_found) + { + /* Get the start page for the step counter parameters */ + start_page = step_params_config.page; + + /* Get the end page for the step counter parameters */ + end_page = start_page + n_pages; + + /* Get the start address for the step counter parameters */ + start_addr = step_params_config.start_addr; + + /* Get the remaining length of bytes to be read */ + remain_len = (uint8_t)((n_bytes - (n_pages * 16)) + start_addr); + for (page_idx = start_page; page_idx <= end_page; page_idx++) + { + /* Get the configuration from the respective page */ + rslt = bmi2_get_feat_config(page_idx, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Start from address 0x00 when switched to next page */ + if (page_idx > start_page) + { + start_addr = 0; + } + + /* Remaining number of words to be read in the page */ + if (page_idx == end_page) + { + max_len = (remain_len / 2); + } + + /* Get offset in words since all the features are set in words length */ + page_byte_idx = start_addr / 2; + for (; page_byte_idx < max_len;) + { + /* Set parameters 1 to 25 */ + *(data_p + page_byte_idx) = BMI2_SET_BIT_POS0(*(data_p + page_byte_idx), + BMI2_STEP_COUNT_PARAMS, + step_count_params[param_idx]); + + /* Increment offset by 1 word to set to the next parameter */ + page_byte_idx++; + + /* Increment to next parameter */ + param_idx++; + } + + /* Get total length in bytes to copy from local pointer to the array */ + page_byte_idx = (uint8_t)(page_byte_idx * 2) - step_params_config.start_addr; + + /* Copy the bytes to be set back to the array */ + for (index = 0; index < page_byte_idx; index++) + { + feat_config[step_params_config.start_addr + + index] = *((uint8_t *) data_p + step_params_config.start_addr + index); + } + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/* @brief This internal API sets step counter configurations like water-mark + * level, reset-counter and output-configuration step detector and activity. + */ +static int8_t set_step_config(const struct bmi2_step_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define index */ + uint8_t index = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step counter 4 */ + struct bmi2_feature_config step_count_config = { 0, 0, 0 }; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for step counter feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); + if (feat_found) + { + /* Get the configuration from the page where step counter resides */ + rslt = bmi2_get_feat_config(step_count_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes */ + idx = step_count_config.start_addr; + + /* Get offset in words since all the features are set in words length */ + idx = idx / 2; + + /* Set water-mark level */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_STEP_COUNT_WM_LEVEL, config->watermark_level); + + /* Set reset-counter */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_STEP_COUNT_RST_CNT, config->reset_counter); + + /* Increment offset by 1 word to set output + * configuration of step detector and step activity + */ + idx++; + + /* Set step buffer size */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_STEP_BUFFER_SIZE, config->step_buffer_size); + + /* Increment offset by 1 more word to get the total length in words */ + idx++; + + /* Get total length in bytes to copy from local pointer to the array */ + idx = (uint8_t)(idx * 2) - step_count_config.start_addr; + + /* Copy the bytes to be set back to the array */ + for (index = 0; index < idx; index++) + { + feat_config[step_count_config.start_addr + + index] = *((uint8_t *) data_p + step_count_config.start_addr + index); + } + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API sets wrist wear wake-up configurations like + * output-configuration for wearable variant. + */ +static int8_t set_wrist_wear_wake_up_wh_config(const struct bmi2_wrist_wear_wake_up_wh_config *config, + struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define index */ + uint8_t index = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for wrist wear wake-up */ + struct bmi2_feature_config wrist_wake_up_config = { 0, 0, 0 }; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for wrist wear wake-up feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&wrist_wake_up_config, BMI2_WRIST_WEAR_WAKE_UP_WH, dev); + if (feat_found) + { + /* Get the configuration from the page where wrist wear wake-up feature resides */ + rslt = bmi2_get_feat_config(wrist_wake_up_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for wrist wear wake-up select */ + idx = wrist_wake_up_config.start_addr; + + /* Get offset in words since all the features are set in words length */ + idx = idx / 2; + + *(data_p + idx) = config->min_angle_focus; + + /* Increment offset by 1 more word to set min_angle_nonfocus */ + idx++; + *(data_p + idx) = config->min_angle_nonfocus; + + /* Increment offset by 1 more word to set angle landscape right and angle landscape left */ + idx++; + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_WRIST_WAKE_UP_ANGLE_LR, config->angle_lr); + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_WRIST_WAKE_UP_ANGLE_LL, config->angle_ll); + + /* Increment offset by 1 more word to set angle portrait down and angle portrait left */ + idx++; + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_WRIST_WAKE_UP_ANGLE_PD, config->angle_pd); + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_WRIST_WAKE_UP_ANGLE_PU, config->angle_pu); + + /* Increment offset by 1 more word to set min duration moved and min duration quite */ + idx++; + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_WRIST_WAKE_UP_MIN_DUR_MOVED, config->min_dur_mov); + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_WRIST_WAKE_UP_MIN_DUR_QUITE, config->min_dur_quite); + + /* Increment offset by 1 more word to get the total length in words */ + idx++; + + /* Get total length in bytes to copy from local pointer to the array */ + idx = (uint8_t)(idx * 2) - wrist_wake_up_config.start_addr; + + /* Copy the bytes to be set back to the array */ + for (index = 0; index < idx; index++) + { + feat_config[wrist_wake_up_config.start_addr + + index] = *((uint8_t *) data_p + wrist_wake_up_config.start_addr + index); + } + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets any-motion configurations like axes select, + * duration, threshold and output-configuration. + */ +static int8_t get_any_motion_config(struct bmi2_any_motion_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define LSB */ + uint16_t lsb; + + /* Variable to define MSB */ + uint16_t msb; + + /* Variable to define a word */ + uint16_t lsb_msb; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for any-motion */ + struct bmi2_feature_config any_mot_config = { 0, 0, 0 }; + + /* Search for any-motion feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&any_mot_config, BMI2_ANY_MOTION, dev); + if (feat_found) + { + /* Get the configuration from the page where any-motion feature resides */ + rslt = bmi2_get_feat_config(any_mot_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for feature enable for any-motion */ + idx = any_mot_config.start_addr; + + /* Get word to calculate duration, x, y and z select */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get duration */ + config->duration = lsb_msb & BMI2_ANY_NO_MOT_DUR_MASK; + + /* Get x-select */ + config->select_x = (lsb_msb & BMI2_ANY_NO_MOT_X_SEL_MASK) >> BMI2_ANY_NO_MOT_X_SEL_POS; + + /* Get y-select */ + config->select_y = (lsb_msb & BMI2_ANY_NO_MOT_Y_SEL_MASK) >> BMI2_ANY_NO_MOT_Y_SEL_POS; + + /* Get z-select */ + config->select_z = (lsb_msb & BMI2_ANY_NO_MOT_Z_SEL_MASK) >> BMI2_ANY_NO_MOT_Z_SEL_POS; + + /* Get word to calculate threshold, output configuration from the same word */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get threshold */ + config->threshold = lsb_msb & BMI2_ANY_NO_MOT_THRES_MASK; + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets no-motion configurations like axes select, + * duration, threshold and output-configuration. + */ +static int8_t get_no_motion_config(struct bmi2_no_motion_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define LSB */ + uint16_t lsb = 0; + + /* Variable to define MSB */ + uint16_t msb = 0; + + /* Variable to define a word */ + uint16_t lsb_msb = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for no-motion */ + struct bmi2_feature_config no_mot_config = { 0, 0, 0 }; + + /* Search for no-motion feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&no_mot_config, BMI2_NO_MOTION, dev); + if (feat_found) + { + /* Get the configuration from the page where no-motion feature resides */ + rslt = bmi2_get_feat_config(no_mot_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for feature enable for no-motion */ + idx = no_mot_config.start_addr; + + /* Get word to calculate duration, x, y and z select */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get duration */ + config->duration = lsb_msb & BMI2_ANY_NO_MOT_DUR_MASK; + + /* Get x-select */ + config->select_x = (lsb_msb & BMI2_ANY_NO_MOT_X_SEL_MASK) >> BMI2_ANY_NO_MOT_X_SEL_POS; + + /* Get y-select */ + config->select_y = (lsb_msb & BMI2_ANY_NO_MOT_Y_SEL_MASK) >> BMI2_ANY_NO_MOT_Y_SEL_POS; + + /* Get z-select */ + config->select_z = (lsb_msb & BMI2_ANY_NO_MOT_Z_SEL_MASK) >> BMI2_ANY_NO_MOT_Z_SEL_POS; + + /* Get word to calculate threshold, output configuration from the same word */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get threshold */ + config->threshold = lsb_msb & BMI2_ANY_NO_MOT_THRES_MASK; + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets step counter parameter configurations. + */ +static int8_t get_step_count_params_config(uint16_t *step_count_params, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Variable to define LSB */ + uint16_t lsb = 0; + + /* Variable to define MSB */ + uint16_t msb = 0; + + /* Variable to define a word */ + uint16_t lsb_msb = 0; + + /* Initialize feature configuration for step counter 1 */ + struct bmi2_feature_config step_params_config = { 0, 0, 0 }; + + /* Variable to index the page number */ + uint8_t page_idx; + + /* Variable to define the start page */ + uint8_t start_page; + + /* Variable to define start address of the parameters */ + uint8_t start_addr; + + /* Variable to define number of bytes */ + uint8_t n_bytes = (BMI2_STEP_CNT_N_PARAMS * 2); + + /* Variable to store number of pages */ + uint8_t n_pages = (n_bytes / 16); + + /* Variable to define the end page */ + uint8_t end_page; + + /* Variable to define the remaining bytes to be read */ + uint8_t remain_len; + + /* Variable to define the maximum words to be read in a page */ + uint8_t max_len = BMI2_FEAT_SIZE_IN_BYTES; + + /* Variable index bytes in a page */ + uint8_t page_byte_idx; + + /* Variable to index the parameters */ + uint8_t param_idx = 0; + + /* Search for step counter parameter feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_params_config, BMI2_STEP_COUNTER_PARAMS, dev); + if (feat_found) + { + /* Get the start page for the step counter parameters */ + start_page = step_params_config.page; + + /* Get the end page for the step counter parameters */ + end_page = start_page + n_pages; + + /* Get the start address for the step counter parameters */ + start_addr = step_params_config.start_addr; + + /* Get the remaining length of bytes to be read */ + remain_len = (uint8_t)((n_bytes - (n_pages * 16)) + start_addr); + for (page_idx = start_page; page_idx <= end_page; page_idx++) + { + /* Get the configuration from the respective page */ + rslt = bmi2_get_feat_config(page_idx, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Start from address 0x00 when switched to next page */ + if (page_idx > start_page) + { + start_addr = 0; + } + + /* Remaining number of bytes to be read in the page */ + if (page_idx == end_page) + { + max_len = remain_len; + } + + /* Get the offset */ + page_byte_idx = start_addr; + while (page_byte_idx < max_len) + { + /* Get word to calculate the parameter*/ + lsb = (uint16_t) feat_config[page_byte_idx++]; + if (page_byte_idx < max_len) + { + msb = ((uint16_t) feat_config[page_byte_idx++] << 8); + } + + lsb_msb = lsb | msb; + + /* Get parameters 1 to 25 */ + step_count_params[param_idx] = lsb_msb & BMI2_STEP_COUNT_PARAMS_MASK; + + /* Increment to next parameter */ + param_idx++; + } + } + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets step counter/detector/activity configurations. + */ +static int8_t get_step_config(struct bmi2_step_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define LSB */ + uint16_t lsb = 0; + + /* Variable to define MSB */ + uint16_t msb = 0; + + /* Variable to define a word */ + uint16_t lsb_msb = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step counter */ + struct bmi2_feature_config step_count_config = { 0, 0, 0 }; + + /* Search for step counter 4 feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); + if (feat_found) + { + /* Get the configuration from the page where step counter 4 parameter resides */ + rslt = bmi2_get_feat_config(step_count_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for feature enable for step counter/detector/activity */ + idx = step_count_config.start_addr; + + /* Get word to calculate water-mark level and reset counter */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get water-mark level */ + config->watermark_level = lsb_msb & BMI2_STEP_COUNT_WM_LEVEL_MASK; + + /* Get reset counter */ + config->reset_counter = (lsb_msb & BMI2_STEP_COUNT_RST_CNT_MASK) >> BMI2_STEP_COUNT_RST_CNT_POS; + + /* Get word to calculate output configuration of step detector and activity */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + config->step_buffer_size = (lsb_msb & BMI2_STEP_BUFFER_SIZE_MASK) >> BMI2_STEP_BUFFER_SIZE_POS; + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets wrist wear wake-up configurations like min_angle_focus, + * min_angle_nonfocus for wearable variant. + */ +static int8_t get_wrist_wear_wake_up_wh_config(struct bmi2_wrist_wear_wake_up_wh_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for wrist wear wake-up */ + struct bmi2_feature_config wrist_wake_up_config = { 0, 0, 0 }; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for wrist wear wake-up feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&wrist_wake_up_config, BMI2_WRIST_WEAR_WAKE_UP_WH, dev); + if (feat_found) + { + /* Get the configuration from the page where wrist wear wake-up feature resides */ + rslt = bmi2_get_feat_config(wrist_wake_up_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for wrist wear wake-up select */ + idx = wrist_wake_up_config.start_addr; + + /* Get offset in words since all the features are set in words length */ + idx = idx / 2; + + config->min_angle_focus = *(data_p + idx); + + /* Increment the offset value by 1 word to get min_angle_nonfocus */ + idx++; + config->min_angle_nonfocus = *(data_p + idx); + + /* Increment the offset value by 1 word to get angle landscape right and angle landscape left */ + idx++; + config->angle_lr = BMI2_GET_BIT_POS0(*(data_p + idx), BMI2_WRIST_WAKE_UP_ANGLE_LR); + config->angle_ll = BMI2_GET_BITS(*(data_p + idx), BMI2_WRIST_WAKE_UP_ANGLE_LL); + + /* Increment the offset value by 1 word to get angle portrait down and angle portrait up */ + idx++; + config->angle_pd = BMI2_GET_BIT_POS0(*(data_p + idx), BMI2_WRIST_WAKE_UP_ANGLE_PD); + config->angle_pu = BMI2_GET_BITS(*(data_p + idx), BMI2_WRIST_WAKE_UP_ANGLE_PU); + + /* Increment the offset value by 1 word to get min duration quite and min duration moved */ + idx++; + config->min_dur_mov = BMI2_GET_BIT_POS0(*(data_p + idx), BMI2_WRIST_WAKE_UP_MIN_DUR_MOVED); + config->min_dur_quite = BMI2_GET_BITS(*(data_p + idx), BMI2_WRIST_WAKE_UP_MIN_DUR_QUITE); + + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets the output values of step counter. + */ +static int8_t get_step_counter_output(uint32_t *step_count, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variables to define index */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature output for step counter */ + struct bmi2_feature_config step_cnt_out_config = { 0, 0, 0 }; + + /* Search for step counter output feature and extract its configuration details */ + feat_found = extract_output_feat_config(&step_cnt_out_config, BMI2_STEP_COUNTER, dev); + if (feat_found) + { + /* Get the feature output configuration for step-counter */ + rslt = bmi2_get_feat_config(step_cnt_out_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for step counter output */ + idx = step_cnt_out_config.start_addr; + + /* Get the step counter output in 4 bytes */ + *step_count = (uint32_t) feat_config[idx++]; + *step_count |= ((uint32_t) feat_config[idx++] << 8); + *step_count |= ((uint32_t) feat_config[idx++] << 16); + *step_count |= ((uint32_t) feat_config[idx++] << 24); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets the error status related to NVM. + */ +static int8_t get_nvm_error_status(struct bmi2_nvm_err_status *nvm_err_stat, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variables to define index */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature output for NVM error status */ + struct bmi2_feature_config nvm_err_cfg = { 0, 0, 0 }; + + /* Search for NVM error status feature and extract its configuration details */ + feat_found = extract_output_feat_config(&nvm_err_cfg, BMI2_NVM_STATUS, dev); + if (feat_found) + { + /* Get the feature output configuration for NVM error status */ + rslt = bmi2_get_feat_config(nvm_err_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for NVM error status */ + idx = nvm_err_cfg.start_addr; + + /* Increment index to get the error status */ + idx++; + + /* Error when NVM load action fails */ + nvm_err_stat->load_error = BMI2_GET_BIT_POS0(feat_config[idx], BMI2_NVM_LOAD_ERR_STATUS); + + /* Error when NVM program action fails */ + nvm_err_stat->prog_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_PROG_ERR_STATUS); + + /* Error when NVM erase action fails */ + nvm_err_stat->erase_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_ERASE_ERR_STATUS); + + /* Error when NVM program limit is exceeded */ + nvm_err_stat->exceed_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_END_EXCEED_STATUS); + + /* Error when NVM privilege mode is not acquired */ + nvm_err_stat->privil_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_PRIV_ERR_STATUS); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to get enable status of gyroscope user gain + * update. + */ +static int8_t get_user_gain_upd_status(uint8_t *status, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Variable to check APS status */ + uint8_t aps_stat = 0; + + /* Initialize feature configuration for gyroscope user gain */ + struct bmi2_feature_config gyr_user_gain_cfg = { 0, 0, 0 }; + + /* Search for user gain feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&gyr_user_gain_cfg, BMI2_GYRO_GAIN_UPDATE, dev); + if (feat_found) + { + /* Disable advance power save */ + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + /* Get the configuration from the page where user gain feature resides */ + rslt = bmi2_get_feat_config(gyr_user_gain_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of user gain */ + idx = gyr_user_gain_cfg.start_addr + BMI2_GYR_USER_GAIN_FEAT_EN_OFFSET; + + /* Set the feature enable status */ + *status = BMI2_GET_BITS(feat_config[idx], BMI2_GYR_USER_GAIN_FEAT_EN); + } + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + /* Enable Advance power save if disabled while configuring and not when already disabled */ + if ((rslt == BMI2_OK) && (aps_stat == BMI2_ENABLE)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + + return rslt; +} + +/*! + * @brief This internal API gets the output values of step activity. + */ +static int8_t get_step_activity_output(uint8_t *step_act, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variables to define index */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature output for step activity */ + struct bmi2_feature_config step_act_out_config = { 0, 0, 0 }; + + /* Search for step activity output feature and extract its configuration details */ + feat_found = extract_output_feat_config(&step_act_out_config, BMI2_STEP_ACTIVITY, dev); + if (feat_found) + { + /* Get the feature output configuration for step-activity */ + rslt = bmi2_get_feat_config(step_act_out_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for step activity output */ + idx = step_act_out_config.start_addr; + + /* Get the step activity output */ + *step_act = feat_config[idx]; + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets the error status related to virtual frames. + */ +static int8_t get_vfrm_error_status(struct bmi2_vfrm_err_status *vfrm_err_stat, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variables to define index */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature output for VFRM error status */ + struct bmi2_feature_config vfrm_err_cfg = { 0, 0, 0 }; + + /* Search for VFRM error status feature and extract its configuration details */ + feat_found = extract_output_feat_config(&vfrm_err_cfg, BMI2_VFRM_STATUS, dev); + if (feat_found) + { + /* Get the feature output configuration for VFRM error status */ + rslt = bmi2_get_feat_config(vfrm_err_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for VFRM error status */ + idx = vfrm_err_cfg.start_addr; + + /* Increment index to get the error status */ + idx++; + + /* Internal error while acquiring lock for FIFO */ + vfrm_err_stat->lock_error = BMI2_GET_BITS(feat_config[idx], BMI2_VFRM_LOCK_ERR_STATUS); + + /* Internal error while writing byte into FIFO */ + vfrm_err_stat->write_error = BMI2_GET_BITS(feat_config[idx], BMI2_VFRM_WRITE_ERR_STATUS); + + /* Internal error while writing into FIFO */ + vfrm_err_stat->fatal_error = BMI2_GET_BITS(feat_config[idx], BMI2_VFRM_FATAL_ERR_STATUS); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API enables/disables compensation of the gain defined + * in the GAIN register. + */ +static int8_t enable_gyro_gain(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define register data */ + uint8_t reg_data = 0; + + rslt = bmi2_get_regs(BMI2_GYR_OFF_COMP_6_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_GAIN_EN, enable); + rslt = bmi2_set_regs(BMI2_GYR_OFF_COMP_6_ADDR, ®_data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This internal API is used to extract the output feature configuration + * details from the look-up table. + */ +static uint8_t extract_output_feat_config(struct bmi2_feature_config *feat_output, + uint8_t type, + const struct bmi2_dev *dev) +{ + /* Variable to define loop */ + uint8_t loop = 0; + + /* Variable to set flag */ + uint8_t feat_found = BMI2_FALSE; + + /* Search for the output feature from the output configuration array */ + while (loop < dev->out_sens) + { + if (dev->feat_output[loop].type == type) + { + *feat_output = dev->feat_output[loop]; + feat_found = BMI2_TRUE; + break; + } + + loop++; + } + + /* Return flag */ + return feat_found; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_wh.h b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_wh.h new file mode 100644 index 000000000..d46251bbd --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_wh.h @@ -0,0 +1,402 @@ +/** +* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. +* +* BSD-3-Clause +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* 3. Neither the name of the copyright holder nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING +* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* @file bmi270_wh.h +* @date 2020-11-04 +* @version v2.63.1 +* +*/ + +/** + * \ingroup bmi2xy + * \defgroup bmi270_wh BMI270_WH + * @brief Sensor driver for BMI270_WH sensor + */ + +#ifndef BMI270_WH_H_ +#define BMI270_WH_H_ + +/*! CPP guard */ +#ifdef __cplusplus +extern "C" { +#endif + +/***************************************************************************/ + +/*! Header files + ****************************************************************************/ +#include "bmi2.h" + +/***************************************************************************/ + +/*! Macro definitions + ****************************************************************************/ + +/*! @name BMI270_WH Chip identifier */ +#define BMI270_WH_CHIP_ID UINT8_C(0x24) + +/*! @name BMI270_WH feature input start addresses */ +#define BMI270_WH_CONFIG_ID_STRT_ADDR UINT8_C(0x00) +#define BMI270_WH_MAX_BURST_LEN_STRT_ADDR UINT8_C(0x02) +#define BMI270_WH_AXIS_MAP_STRT_ADDR UINT8_C(0x04) +#define BMI270_WH_NVM_PROG_PREP_STRT_ADDR UINT8_C(0x05) +#define BMI270_WH_GYRO_GAIN_UPDATE_STRT_ADDR UINT8_C(0x06) +#define BMI270_WH_ANY_MOT_STRT_ADDR UINT8_C(0x0C) +#define BMI270_WH_NO_MOT_STRT_ADDR UINT8_C(0x00) +#define BMI270_WH_STEP_CNT_1_STRT_ADDR UINT8_C(0x00) +#define BMI270_WH_STEP_CNT_4_STRT_ADDR UINT8_C(0x02) +#define BMI270_WH_WRIST_WEAR_WAKE_UP_STRT_ADDR UINT8_C(0x04) + +/*! @name BMI270_WH feature output start addresses */ +#define BMI270_WH_STEP_CNT_OUT_STRT_ADDR UINT8_C(0x00) +#define BMI270_WH_STEP_ACT_OUT_STRT_ADDR UINT8_C(0x04) +#define BMI270_WH_WRIST_GEST_OUT_STRT_ADDR UINT8_C(0x06) +#define BMI270_WH_GYR_USER_GAIN_OUT_STRT_ADDR UINT8_C(0x08) +#define BMI270_WH_GYRO_CROSS_SENSE_STRT_ADDR UINT8_C(0x0C) +#define BMI270_WH_NVM_VFRM_OUT_STRT_ADDR UINT8_C(0x0E) + +/*! @name Defines maximum number of pages */ +#define BMI270_WH_MAX_PAGE_NUM UINT8_C(8) + +/*! @name Defines maximum number of feature input configurations */ +#define BMI270_WH_MAX_FEAT_IN UINT8_C(12) + +/*! @name Defines maximum number of feature outputs */ +#define BMI270_WH_MAX_FEAT_OUT UINT8_C(6) + +/*! @name Mask definitions for feature interrupt status bits */ +#define BMI270_WH_STEP_CNT_STATUS_MASK UINT8_C(0x02) +#define BMI270_WH_STEP_ACT_STATUS_MASK UINT8_C(0x04) +#define BMI270_WH_WRIST_WEAR_WAKEUP_WH_STATUS_MASK UINT8_C(0x08) +#define BMI270_WH_NO_MOT_STATUS_MASK UINT8_C(0x20) +#define BMI270_WH_ANY_MOT_STATUS_MASK UINT8_C(0x40) + +/*! @name Mask definitions for feature interrupt mapping bits */ +#define BMI270_WH_INT_STEP_COUNTER_MASK UINT8_C(0x02) +#define BMI270_WH_INT_STEP_DETECTOR_MASK UINT8_C(0x02) +#define BMI270_WH_INT_STEP_ACT_MASK UINT8_C(0x04) +#define BMI270_WH_INT_WRIST_WEAR_WAKEUP_WH_MASK UINT8_C(0x08) +#define BMI270_WH_INT_NO_MOT_MASK UINT8_C(0x20) +#define BMI270_WH_INT_ANY_MOT_MASK UINT8_C(0x40) + +/*! @name Defines maximum number of feature interrupts */ +#define BMI270_WH_MAX_INT_MAP UINT8_C(6) + +/***************************************************************************/ + +/*! BMI270_WH User Interface function prototypes + ****************************************************************************/ + +/** + * \ingroup bmi270_wh + * \defgroup bmi270_whApiInit Initialization + * @brief Initialize the sensor and device structure + */ + +/*! + * \ingroup bmi270_whApiInit + * \page bmi270_wh_api_bmi270_wh_init bmi270_wh_init + * \code + * int8_t bmi270_wh_init(struct bmi2_dev *dev); + * \endcode + * @details This API: + * 1) updates the device structure with address of the configuration file. + * 2) Initializes BMI270_WH sensor. + * 3) Writes the configuration file. + * 4) Updates the feature offset parameters in the device structure. + * 5) Updates the maximum number of pages, in the device structure. + * + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_wh_init(struct bmi2_dev *dev); + +/** + * \ingroup bmi270_wh + * \defgroup bmi270_whApiSensor Feature Set + * @brief Enable / Disable features of the sensor + */ + +/*! + * \ingroup bmi270_whApiSensor + * \page bmi270_wh_api_bmi270_wh_sensor_enable bmi270_wh_sensor_enable + * \code + * int8_t bmi270_wh_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API selects the sensors/features to be enabled. + * + * @param[in] sens_list : Pointer to select the sensor/feature. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features that can be enabled. + * + *@verbatim + * sens_list | Values + * ----------------------------|----------- + * BMI2_ACCEL | 0 + * BMI2_GYRO | 1 + * BMI2_AUX | 2 + * BMI2_ANY_MOTION | 4 + * BMI2_NO_MOTION | 5 + * BMI2_STEP_DETECTOR | 6 + * BMI2_STEP_COUNTER | 7 + * BMI2_STEP_ACTIVITY | 8 + * BMI2_GYRO_GAIN_UPDATE | 9 + * BMI2_WRIST_WEAR_WAKE_UP_WH | 21 + *@endverbatim + * + * @note : + * example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO}; + * uint8_t n_sens = 2; + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_wh_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + +/*! + * \ingroup bmi270_whApiSensor + * \page bmi270_wh_api_bmi270_wh_sensor_disable bmi270_wh_sensor_disable + * \code + * int8_t bmi270_wh_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API selects the sensors/features to be disabled. + * + * @param[in] sens_list : Pointer to select the sensor/feature. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features that can be disabled. + * + *@verbatim + * sens_list | Values + * ----------------------------|----------- + * BMI2_ACCEL | 0 + * BMI2_GYRO | 1 + * BMI2_AUX | 2 + * BMI2_ANY_MOTION | 4 + * BMI2_NO_MOTION | 5 + * BMI2_STEP_DETECTOR | 6 + * BMI2_STEP_COUNTER | 7 + * BMI2_STEP_ACTIVITY | 8 + * BMI2_GYRO_GAIN_UPDATE | 9 + * BMI2_WRIST_WEAR_WAKE_UP_WH | 21 + *@endverbatim + * + * @note : + * example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO}; + * uint8_t n_sens = 2; + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_wh_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + +/** + * \ingroup bmi270_wh + * \defgroup bmi270_whApiSensorC Sensor Configuration + * @brief Enable / Disable feature configuration of the sensor + */ + +/*! + * \ingroup bmi270_whApiSensorC + * \page bmi270_wh_api_bmi270_wh_set_sensor_config bmi270_wh_set_sensor_config + * \code + * int8_t bmi270_wh_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API sets the sensor/feature configuration. + * + * @param[in] sens_cfg : Structure instance of bmi2_sens_config. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features that can be configured + * + *@verbatim + * sens_list | Values + * ----------------------------|----------- + * BMI2_ANY_MOTION | 4 + * BMI2_NO_MOTION | 5 + * BMI2_STEP_DETECTOR | 6 + * BMI2_STEP_COUNTER | 7 + * BMI2_STEP_ACTIVITY | 8 + * BMI2_WRIST_WEAR_WAKE_UP_WH | 21 + * BMI2_STEP_COUNTER_PARAMS | 28 + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_wh_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + +/*! + * \ingroup bmi270_whApiSensorC + * \page bmi270_wh_api_bmi270_wh_get_sensor_config bmi270_wh_get_sensor_config + * \code + * int8_t bmi270_wh_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API gets the sensor/feature configuration. + * + * @param[in] sens_cfg : Structure instance of bmi2_sens_config. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features whose configurations can be read. + * + *@verbatim + * sens_list | Values + * ----------------------------|----------- + * BMI2_ANY_MOTION | 4 + * BMI2_NO_MOTION | 5 + * BMI2_STEP_DETECTOR | 6 + * BMI2_STEP_COUNTER | 7 + * BMI2_STEP_ACTIVITY | 8 + * BMI2_WRIST_WEAR_WAKE_UP_WH | 21 + * BMI2_STEP_COUNTER_PARAMS | 28 + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_wh_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + +/** + * \ingroup bmi270_wh + * \defgroup bmi270_whApiSensorD Sensor Data + * @brief Get sensor data + */ + +/*! + * \ingroup bmi270_whApiSensorD + * \page bmi270_wh_api_bmi270_wh_get_sensor_data bmi270_wh_get_sensor_data + * \code + * int8_t bmi270_wh_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API gets the sensor/feature data for accelerometer, gyroscope, + * auxiliary sensor, step counter, high-g, gyroscope user-gain update, + * orientation, gyroscope cross sensitivity and error status for NVM and VFRM. + * + * @param[out] sensor_data : Structure instance of bmi2_sensor_data. + * @param[in] n_sens : Number of sensors selected. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features whose data can be read + * + *@verbatim + * sens_list | Values + * ---------------------|----------- + * BMI2_STEP_COUNTER | 7 + * BMI2_STEP_ACTIVITY | 8 + * BMI2_NVM_STATUS | 38 + * BMI2_VFRM_STATUS | 39 + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_wh_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev); + +/** + * \ingroup bmi270_wh + * \defgroup bmi270_whApiGyroUG Gyro User Gain + * @brief Set / Get Gyro User Gain of the sensor + */ + +/*! + * \ingroup bmi270_whApiGyroUG + * \page bmi270_wh_api_bmi270_wh_update_gyro_user_gain bmi270_wh_update_gyro_user_gain + * \code + * int8_t bmi270_wh_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev); + * \endcode + * @details This API updates the gyroscope user-gain. + * + * @param[in] user_gain : Structure that stores user-gain configurations. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_wh_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev); + +/*! + * \ingroup bmi270_whApiGyroUG + * \page bmi270_wh_api_bmi270_wh_read_gyro_user_gain bmi270_wh_read_gyro_user_gain + * \code + * int8_t bmi270_wh_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, const struct bmi2_dev *dev); + * \endcode + * @details This API reads the compensated gyroscope user-gain values. + * + * @param[out] gyr_usr_gain : Structure that stores gain values. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_wh_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, struct bmi2_dev *dev); + +/*! + * \ingroup bmi270_whApiInt + * \page bmi270_wh_api_bmi270_wh_map_feat_int bmi270_wh_map_feat_int + * \code + * int8_t bmi270_wh_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev) + * \endcode + * @details This API maps/unmaps feature interrupts to that of interrupt pins. + * + * @param[in] sens_int : Structure instance of bmi2_sens_int_config. + * @param[in] n_sens : Number of interrupts to be mapped. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_wh_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev); + +/******************************************************************************/ +/*! @name C++ Guard Macros */ +/******************************************************************************/ +#ifdef __cplusplus +} +#endif /* End of CPP guard */ + +#endif /* BMI270_WH_H_ */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_defs.h b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_defs.h index 64bd472f3..889bf8256 100644 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_defs.h +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_defs.h @@ -30,11 +30,13 @@ * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * -* @file bmi2_defs.h -* @date 2020-01-10 -* @version v2.46.1 +* @file bmi2_defs.h +* @date 2020-11-04 +* @version v2.63.1 * -*/#ifndef BMI2_DEFS_H_ +*/ + +#ifndef BMI2_DEFS_H_ #define BMI2_DEFS_H_ /******************************************************************************/ @@ -53,32 +55,32 @@ /******************************************************************************/ #ifdef __KERNEL__ #if !defined(UINT8_C) && !defined(INT8_C) -#define INT8_C(x) S8_C(x) -#define UINT8_C(x) U8_C(x) +#define INT8_C(x) S8_C(x) +#define UINT8_C(x) U8_C(x) #endif #if !defined(UINT16_C) && !defined(INT16_C) -#define INT16_C(x) S16_C(x) -#define UINT16_C(x) U16_C(x) +#define INT16_C(x) S16_C(x) +#define UINT16_C(x) U16_C(x) #endif #if !defined(INT32_C) && !defined(UINT32_C) -#define INT32_C(x) S32_C(x) -#define UINT32_C(x) U32_C(x) +#define INT32_C(x) S32_C(x) +#define UINT32_C(x) U32_C(x) #endif #if !defined(INT64_C) && !defined(UINT64_C) -#define INT64_C(x) S64_C(x) -#define UINT64_C(x) U64_C(x) +#define INT64_C(x) S64_C(x) +#define UINT64_C(x) U64_C(x) #endif #endif /*! @name C standard macros */ #ifndef NULL #ifdef __cplusplus -#define NULL 0 +#define NULL 0 #else -#define NULL ((void *) 0) +#define NULL ((void *) 0) #endif #endif @@ -98,348 +100,715 @@ ((reg_data & ~(bitname##_MASK)) | \ (data & bitname##_MASK)) -#define BMI2_GET_BIT_POS0(reg_data, bitname) (reg_data & (bitname##_MASK)) -#define BMI2_SET_BIT_VAL0(reg_data, bitname) (reg_data & ~(bitname##_MASK)) +#define BMI2_GET_BIT_POS0(reg_data, bitname) (reg_data & (bitname##_MASK)) +#define BMI2_SET_BIT_VAL0(reg_data, bitname) (reg_data & ~(bitname##_MASK)) /*! @name For getting LSB and MSB */ -#define BMI2_GET_LSB(var) (uint8_t)(var & BMI2_SET_LOW_BYTE) -#define BMI2_GET_MSB(var) (uint8_t)((var & BMI2_SET_HIGH_BYTE) >> 8) +#define BMI2_GET_LSB(var) (uint8_t)(var & BMI2_SET_LOW_BYTE) +#define BMI2_GET_MSB(var) (uint8_t)((var & BMI2_SET_HIGH_BYTE) >> 8) + +#ifndef BMI2_INTF_RETURN_TYPE +#define BMI2_INTF_RETURN_TYPE int8_t +#endif /*! @name For defining absolute values */ -#define BMI2_ABS(a) ((a) > 0 ? (a) : -(a)) +#define BMI2_ABS(a) ((a) > 0 ? (a) : -(a)) /*! @name LSB and MSB mask definitions */ -#define BMI2_SET_LOW_BYTE UINT16_C(0x00FF) -#define BMI2_SET_HIGH_BYTE UINT16_C(0xFF00) -#define BMI2_SET_LOW_NIBBLE UINT8_C(0x0F) +#define BMI2_SET_LOW_BYTE UINT16_C(0x00FF) +#define BMI2_SET_HIGH_BYTE UINT16_C(0xFF00) +#define BMI2_SET_LOW_NIBBLE UINT8_C(0x0F) /*! @name For enable and disable */ -#define BMI2_ENABLE UINT8_C(1) -#define BMI2_DISABLE UINT8_C(0) +#define BMI2_ENABLE UINT8_C(1) +#define BMI2_DISABLE UINT8_C(0) /*! @name To define TRUE or FALSE */ -#define BMI2_TRUE UINT8_C(1) -#define BMI2_FALSE UINT8_C(0) +#define BMI2_TRUE UINT8_C(1) +#define BMI2_FALSE UINT8_C(0) + +/*! @name To define sensor interface success code */ +#define BMI2_INTF_RET_SUCCESS INT8_C(0) /*! @name To define success code */ -#define BMI2_OK INT8_C(0) +#define BMI2_OK INT8_C(0) /*! @name To define error codes */ -#define BMI2_E_NULL_PTR INT8_C(-1) -#define BMI2_E_COM_FAIL INT8_C(-2) -#define BMI2_E_DEV_NOT_FOUND INT8_C(-3) -#define BMI2_E_OUT_OF_RANGE INT8_C(-4) -#define BMI2_E_ACC_INVALID_CFG INT8_C(-5) -#define BMI2_E_GYRO_INVALID_CFG INT8_C(-6) -#define BMI2_E_ACC_GYR_INVALID_CFG INT8_C(-7) -#define BMI2_E_INVALID_SENSOR INT8_C(-8) -#define BMI2_E_CONFIG_LOAD INT8_C(-9) -#define BMI2_E_INVALID_PAGE INT8_C(-10) -#define BMI2_E_INVALID_FEAT_BIT INT8_C(-11) -#define BMI2_E_INVALID_INT_PIN INT8_C(-12) -#define BMI2_E_SET_APS_FAIL INT8_C(-13) -#define BMI2_E_AUX_INVALID_CFG INT8_C(-14) -#define BMI2_E_AUX_BUSY INT8_C(-15) -#define BMI2_E_SELF_TEST_FAIL INT8_C(-16) -#define BMI2_E_REMAP_ERROR INT8_C(-17) -#define BMI2_E_GYR_USER_GAIN_UPD_FAIL INT8_C(-18) -#define BMI2_E_SELF_TEST_NOT_DONE INT8_C(-19) -#define BMI2_E_INVALID_INPUT INT8_C(-20) -#define BMI2_E_INVALID_STATUS INT8_C(-21) -#define BMI2_E_CRT_ERROR INT8_C(-22) -#define BMI2_E_ST_ALREADY_RUNNING INT8_C(-23) -#define BMI2_E_CRT_READY_FOR_DL_FAIL_ABORT INT8_C(-24) -#define BMI2_E_DL_ERROR INT8_C(-25) -#define BMI2_E_PRECON_ERROR INT8_C(-26) -#define BMI2_E_ABORT_ERROR INT8_C(-27) -#define BMI2_E_GYRO_SELF_TEST_ERROR INT8_C(-28) -#define BMI2_E_GYRO_SELF_TEST_TIMEOUT INT8_C(-29) -#define BMI2_E_WRITE_CYCLE_ONGOING INT8_C(-30) -#define BMI2_E_WRITE_CYCLE_TIMEOUT INT8_C(-31) -#define BMI2_E_ST_NOT_RUNING INT8_C(-32) -#define BMI2_E_DATA_RDY_INT_FAILED INT8_C(-33) -#define BMI2_E_INVALID_FOC_POSITION INT8_C(-34) +#define BMI2_E_NULL_PTR INT8_C(-1) +#define BMI2_E_COM_FAIL INT8_C(-2) +#define BMI2_E_DEV_NOT_FOUND INT8_C(-3) +#define BMI2_E_OUT_OF_RANGE INT8_C(-4) +#define BMI2_E_ACC_INVALID_CFG INT8_C(-5) +#define BMI2_E_GYRO_INVALID_CFG INT8_C(-6) +#define BMI2_E_ACC_GYR_INVALID_CFG INT8_C(-7) +#define BMI2_E_INVALID_SENSOR INT8_C(-8) +#define BMI2_E_CONFIG_LOAD INT8_C(-9) +#define BMI2_E_INVALID_PAGE INT8_C(-10) +#define BMI2_E_INVALID_FEAT_BIT INT8_C(-11) +#define BMI2_E_INVALID_INT_PIN INT8_C(-12) +#define BMI2_E_SET_APS_FAIL INT8_C(-13) +#define BMI2_E_AUX_INVALID_CFG INT8_C(-14) +#define BMI2_E_AUX_BUSY INT8_C(-15) +#define BMI2_E_SELF_TEST_FAIL INT8_C(-16) +#define BMI2_E_REMAP_ERROR INT8_C(-17) +#define BMI2_E_GYR_USER_GAIN_UPD_FAIL INT8_C(-18) +#define BMI2_E_SELF_TEST_NOT_DONE INT8_C(-19) +#define BMI2_E_INVALID_INPUT INT8_C(-20) +#define BMI2_E_INVALID_STATUS INT8_C(-21) +#define BMI2_E_CRT_ERROR INT8_C(-22) +#define BMI2_E_ST_ALREADY_RUNNING INT8_C(-23) +#define BMI2_E_CRT_READY_FOR_DL_FAIL_ABORT INT8_C(-24) +#define BMI2_E_DL_ERROR INT8_C(-25) +#define BMI2_E_PRECON_ERROR INT8_C(-26) +#define BMI2_E_ABORT_ERROR INT8_C(-27) +#define BMI2_E_GYRO_SELF_TEST_ERROR INT8_C(-28) +#define BMI2_E_GYRO_SELF_TEST_TIMEOUT INT8_C(-29) +#define BMI2_E_WRITE_CYCLE_ONGOING INT8_C(-30) +#define BMI2_E_WRITE_CYCLE_TIMEOUT INT8_C(-31) +#define BMI2_E_ST_NOT_RUNING INT8_C(-32) +#define BMI2_E_DATA_RDY_INT_FAILED INT8_C(-33) +#define BMI2_E_INVALID_FOC_POSITION INT8_C(-34) /*! @name To define warnings for FIFO activity */ -#define BMI2_W_FIFO_EMPTY INT8_C(1) -#define BMI2_W_PARTIAL_READ INT8_C(2) +#define BMI2_W_FIFO_EMPTY INT8_C(1) +#define BMI2_W_PARTIAL_READ INT8_C(2) /*! @name Bit wise to define information */ -#define BMI2_I_MIN_VALUE UINT8_C(1) -#define BMI2_I_MAX_VALUE UINT8_C(2) +#define BMI2_I_MIN_VALUE UINT8_C(1) +#define BMI2_I_MAX_VALUE UINT8_C(2) /*! @name BMI2 register addresses */ -#define BMI2_CHIP_ID_ADDR UINT8_C(0x00) -#define BMI2_STATUS_ADDR UINT8_C(0x03) -#define BMI2_AUX_X_LSB_ADDR UINT8_C(0x04) -#define BMI2_ACC_X_LSB_ADDR UINT8_C(0x0C) -#define BMI2_GYR_X_LSB_ADDR UINT8_C(0x12) -#define BMI2_EVENT_ADDR UINT8_C(0x1B) -#define BMI2_INT_STATUS_0_ADDR UINT8_C(0x1C) -#define BMI2_INT_STATUS_1_ADDR UINT8_C(0x1D) -#define BMI2_SC_OUT_0_ADDR UINT8_C(0x1E) -#define BMI2_SYNC_COMMAND_ADDR UINT8_C(0x1E) -#define BMI2_GYR_CAS_GPIO0_ADDR UINT8_C(0x1E) -#define BMI2_INTERNAL_STATUS_ADDR UINT8_C(0x21) -#define BMI2_FIFO_LENGTH_0_ADDR UINT8_C(0X24) -#define BMI2_FIFO_DATA_ADDR UINT8_C(0X26) -#define BMI2_FEAT_PAGE_ADDR UINT8_C(0x2F) -#define BMI2_FEATURES_REG_ADDR UINT8_C(0x30) -#define BMI2_ACC_CONF_ADDR UINT8_C(0x40) -#define BMI2_GYR_CONF_ADDR UINT8_C(0x42) -#define BMI2_AUX_CONF_ADDR UINT8_C(0x44) -#define BMI2_FIFO_DOWNS_ADDR UINT8_C(0X45) -#define BMI2_FIFO_WTM_0_ADDR UINT8_C(0X46) -#define BMI2_FIFO_WTM_1_ADDR UINT8_C(0X47) -#define BMI2_FIFO_CONFIG_0_ADDR UINT8_C(0X48) -#define BMI2_FIFO_CONFIG_1_ADDR UINT8_C(0X49) -#define BMI2_AUX_DEV_ID_ADDR UINT8_C(0x4B) -#define BMI2_AUX_IF_CONF_ADDR UINT8_C(0x4C) -#define BMI2_AUX_RD_ADDR UINT8_C(0x4D) -#define BMI2_AUX_WR_ADDR UINT8_C(0x4E) -#define BMI2_AUX_WR_DATA_ADDR UINT8_C(0x4F) -#define BMI2_INT1_IO_CTRL_ADDR UINT8_C(0x53) -#define BMI2_INT2_IO_CTRL_ADDR UINT8_C(0x54) -#define BMI2_INT1_MAP_FEAT_ADDR UINT8_C(0x56) -#define BMI2_INT2_MAP_FEAT_ADDR UINT8_C(0x57) -#define BMI2_INT_MAP_DATA_ADDR UINT8_C(0x58) -#define BMI2_INIT_CTRL_ADDR UINT8_C(0x59) -#define BMI2_INIT_ADDR_0 UINT8_C(0x5B) -#define BMI2_INIT_ADDR_1 UINT8_C(0x5C) -#define BMI2_INIT_DATA_ADDR UINT8_C(0x5E) -#define BMI2_GYR_CRT_CONF_ADDR UINT8_C(0X69) -#define BMI2_NVM_CONF_ADDR UINT8_C(0x6A) -#define BMI2_IF_CONF_ADDR UINT8_C(0X6B) -#define BMI2_ACC_SELF_TEST_ADDR UINT8_C(0X6D) -#define BMI2_GYR_SELF_TEST_AXES_ADDR UINT8_C(0x6E) -#define BMI2_SELF_TEST_MEMS_ADDR UINT8_C(0X6F) -#define BMI2_NV_CONF_ADDR UINT8_C(0x70) -#define BMI2_ACC_OFF_COMP_0_ADDR UINT8_C(0X71) -#define BMI2_GYR_OFF_COMP_3_ADDR UINT8_C(0X74) -#define BMI2_GYR_OFF_COMP_6_ADDR UINT8_C(0X77) -#define BMI2_GYR_USR_GAIN_0_ADDR UINT8_C(0X78) -#define BMI2_PWR_CONF_ADDR UINT8_C(0x7C) -#define BMI2_PWR_CTRL_ADDR UINT8_C(0x7D) -#define BMI2_CMD_REG_ADDR UINT8_C(0x7E) +#define BMI2_CHIP_ID_ADDR UINT8_C(0x00) +#define BMI2_STATUS_ADDR UINT8_C(0x03) +#define BMI2_AUX_X_LSB_ADDR UINT8_C(0x04) +#define BMI2_ACC_X_LSB_ADDR UINT8_C(0x0C) +#define BMI2_GYR_X_LSB_ADDR UINT8_C(0x12) +#define BMI2_EVENT_ADDR UINT8_C(0x1B) +#define BMI2_INT_STATUS_0_ADDR UINT8_C(0x1C) +#define BMI2_INT_STATUS_1_ADDR UINT8_C(0x1D) +#define BMI2_SC_OUT_0_ADDR UINT8_C(0x1E) +#define BMI2_SYNC_COMMAND_ADDR UINT8_C(0x1E) +#define BMI2_GYR_CAS_GPIO0_ADDR UINT8_C(0x1E) +#define BMI2_INTERNAL_STATUS_ADDR UINT8_C(0x21) +#define BMI2_FIFO_LENGTH_0_ADDR UINT8_C(0X24) +#define BMI2_FIFO_DATA_ADDR UINT8_C(0X26) +#define BMI2_FEAT_PAGE_ADDR UINT8_C(0x2F) +#define BMI2_FEATURES_REG_ADDR UINT8_C(0x30) +#define BMI2_ACC_CONF_ADDR UINT8_C(0x40) +#define BMI2_GYR_CONF_ADDR UINT8_C(0x42) +#define BMI2_AUX_CONF_ADDR UINT8_C(0x44) +#define BMI2_FIFO_DOWNS_ADDR UINT8_C(0X45) +#define BMI2_FIFO_WTM_0_ADDR UINT8_C(0X46) +#define BMI2_FIFO_WTM_1_ADDR UINT8_C(0X47) +#define BMI2_FIFO_CONFIG_0_ADDR UINT8_C(0X48) +#define BMI2_FIFO_CONFIG_1_ADDR UINT8_C(0X49) +#define BMI2_AUX_DEV_ID_ADDR UINT8_C(0x4B) +#define BMI2_AUX_IF_CONF_ADDR UINT8_C(0x4C) +#define BMI2_AUX_RD_ADDR UINT8_C(0x4D) +#define BMI2_AUX_WR_ADDR UINT8_C(0x4E) +#define BMI2_AUX_WR_DATA_ADDR UINT8_C(0x4F) +#define BMI2_INT1_IO_CTRL_ADDR UINT8_C(0x53) +#define BMI2_INT2_IO_CTRL_ADDR UINT8_C(0x54) +#define BMI2_INT_LATCH_ADDR UINT8_C(0x55) +#define BMI2_INT1_MAP_FEAT_ADDR UINT8_C(0x56) +#define BMI2_INT2_MAP_FEAT_ADDR UINT8_C(0x57) +#define BMI2_INT_MAP_DATA_ADDR UINT8_C(0x58) +#define BMI2_INIT_CTRL_ADDR UINT8_C(0x59) +#define BMI2_INIT_ADDR_0 UINT8_C(0x5B) +#define BMI2_INIT_ADDR_1 UINT8_C(0x5C) +#define BMI2_INIT_DATA_ADDR UINT8_C(0x5E) +#define BMI2_AUX_IF_TRIM UINT8_C(0x68) +#define BMI2_GYR_CRT_CONF_ADDR UINT8_C(0X69) +#define BMI2_NVM_CONF_ADDR UINT8_C(0x6A) +#define BMI2_IF_CONF_ADDR UINT8_C(0X6B) +#define BMI2_ACC_SELF_TEST_ADDR UINT8_C(0X6D) +#define BMI2_GYR_SELF_TEST_AXES_ADDR UINT8_C(0x6E) +#define BMI2_SELF_TEST_MEMS_ADDR UINT8_C(0X6F) +#define BMI2_NV_CONF_ADDR UINT8_C(0x70) +#define BMI2_ACC_OFF_COMP_0_ADDR UINT8_C(0X71) +#define BMI2_GYR_OFF_COMP_3_ADDR UINT8_C(0X74) +#define BMI2_GYR_OFF_COMP_6_ADDR UINT8_C(0X77) +#define BMI2_GYR_USR_GAIN_0_ADDR UINT8_C(0X78) +#define BMI2_PWR_CONF_ADDR UINT8_C(0x7C) +#define BMI2_PWR_CTRL_ADDR UINT8_C(0x7D) +#define BMI2_CMD_REG_ADDR UINT8_C(0x7E) /*! @name BMI2 I2C address */ -#define BMI2_I2C_PRIM_ADDR UINT8_C(0x68) -#define BMI2_I2C_SEC_ADDR UINT8_C(0x69) +#define BMI2_I2C_PRIM_ADDR UINT8_C(0x68) +#define BMI2_I2C_SEC_ADDR UINT8_C(0x69) /*! @name BMI2 Commands */ -#define BMI2_G_TRIGGER_CMD UINT8_C(0x02) -#define BMI2_USR_GAIN_CMD UINT8_C(0x03) -#define BMI2_NVM_PROG_CMD UINT8_C(0xA0) -#define BMI2_SOFT_RESET_CMD UINT8_C(0xB6) -#define BMI2_FIFO_FLUSH_CMD UINT8_C(0xB0) - -/*! @name Mask definitions for ..... */ -#define BMI2_GYR_RDY_FOR_DL_MASK UINT8_C(0x08) -#define BMI2_GYR_CRT_RUNNING_MASK UINT8_C(0x04) - -/*! @name Bit position definitions..... */ -#define BMI2_GYR_RDY_FOR_DL_POS UINT8_C(0x03) -#define BMI2_GYR_CRT_RUNNING_POS UINT8_C(0x02) +#define BMI2_G_TRIGGER_CMD UINT8_C(0x02) +#define BMI2_USR_GAIN_CMD UINT8_C(0x03) +#define BMI2_NVM_PROG_CMD UINT8_C(0xA0) +#define BMI2_SOFT_RESET_CMD UINT8_C(0xB6) +#define BMI2_FIFO_FLUSH_CMD UINT8_C(0xB0) /*! @name BMI2 sensor data bytes */ -#define BMI2_ACC_GYR_NUM_BYTES UINT8_C(6) -#define BMI2_AUX_NUM_BYTES UINT8_C(8) -#define BMI2_CRT_CONFIG_FILE_SIZE UINT16_C(2048) -#define BMI2_FEAT_SIZE_IN_BYTES UINT8_C(16) -#define BMI2_ACC_CONFIG_LENGTH UINT8_C(2) +#define BMI2_ACC_GYR_NUM_BYTES UINT8_C(6) +#define BMI2_AUX_NUM_BYTES UINT8_C(8) +#define BMI2_CRT_CONFIG_FILE_SIZE UINT16_C(2048) +#define BMI2_FEAT_SIZE_IN_BYTES UINT8_C(16) +#define BMI2_ACC_CONFIG_LENGTH UINT8_C(2) /*! @name BMI2 configuration load status */ -#define BMI2_CONFIG_LOAD_SUCCESS UINT8_C(1) +#define BMI2_CONFIG_LOAD_SUCCESS UINT8_C(1) /*! @name To define BMI2 pages */ -#define BMI2_PAGE_0 UINT8_C(0) -#define BMI2_PAGE_1 UINT8_C(1) -#define BMI2_PAGE_2 UINT8_C(2) -#define BMI2_PAGE_3 UINT8_C(3) -#define BMI2_PAGE_4 UINT8_C(4) -#define BMI2_PAGE_5 UINT8_C(5) -#define BMI2_PAGE_6 UINT8_C(6) -#define BMI2_PAGE_7 UINT8_C(7) +#define BMI2_PAGE_0 UINT8_C(0) +#define BMI2_PAGE_1 UINT8_C(1) +#define BMI2_PAGE_2 UINT8_C(2) +#define BMI2_PAGE_3 UINT8_C(3) +#define BMI2_PAGE_4 UINT8_C(4) +#define BMI2_PAGE_5 UINT8_C(5) +#define BMI2_PAGE_6 UINT8_C(6) +#define BMI2_PAGE_7 UINT8_C(7) /*! @name Array Parameter DefinItions */ -#define BMI2_SENSOR_TIME_LSB_BYTE UINT8_C(0) -#define BMI2_SENSOR_TIME_XLSB_BYTE UINT8_C(1) -#define BMI2_SENSOR_TIME_MSB_BYTE UINT8_C(2) +#define BMI2_SENSOR_TIME_LSB_BYTE UINT8_C(0) +#define BMI2_SENSOR_TIME_XLSB_BYTE UINT8_C(1) +#define BMI2_SENSOR_TIME_MSB_BYTE UINT8_C(2) + +/*! @name Mask definitions for Gyro CRT */ +#define BMI2_GYR_RDY_FOR_DL_MASK UINT8_C(0x08) +#define BMI2_GYR_CRT_RUNNING_MASK UINT8_C(0x04) /*! @name mask definition for status register */ -#define BMI2_AUX_BUSY_MASK UINT8_C(0x04) -#define BMI2_CMD_RDY_MASK UINT8_C(0x10) -#define BMI2_DRDY_AUX_MASK UINT8_C(0x20) -#define BMI2_DRDY_GYR_MASK UINT8_C(0x40) -#define BMI2_DRDY_ACC_MASK UINT8_C(0x80) - -/*! @name Bit position for status register*/ -#define BMI2_AUX_BUSY_POS UINT8_C(0x02) -#define BMI2_CMD_RDY_POS UINT8_C(0x04) -#define BMI2_DRDY_AUX_POS UINT8_C(0x05) -#define BMI2_DRDY_GYR_POS UINT8_C(0x06) -#define BMI2_DRDY_ACC_POS UINT8_C(0x07) +#define BMI2_AUX_BUSY_MASK UINT8_C(0x04) +#define BMI2_CMD_RDY_MASK UINT8_C(0x10) +#define BMI2_DRDY_AUX_MASK UINT8_C(0x20) +#define BMI2_DRDY_GYR_MASK UINT8_C(0x40) +#define BMI2_DRDY_ACC_MASK UINT8_C(0x80) /*! @name Mask definitions for SPI read/write address */ -#define BMI2_SPI_RD_MASK UINT8_C(0x80) -#define BMI2_SPI_WR_MASK UINT8_C(0x7F) +#define BMI2_SPI_RD_MASK UINT8_C(0x80) +#define BMI2_SPI_WR_MASK UINT8_C(0x7F) /*! @name Mask definitions for power configuration register */ -#define BMI2_ADV_POW_EN_MASK UINT8_C(0x01) +#define BMI2_ADV_POW_EN_MASK UINT8_C(0x01) /*! @name Mask definitions for initialization control register */ -#define BMI2_CONF_LOAD_EN_MASK UINT8_C(0x01) +#define BMI2_CONF_LOAD_EN_MASK UINT8_C(0x01) /*! @name Mask definitions for power control register */ -#define BMI2_AUX_EN_MASK UINT8_C(0x01) -#define BMI2_GYR_EN_MASK UINT8_C(0x02) -#define BMI2_ACC_EN_MASK UINT8_C(0x04) -#define BMI2_TEMP_EN_MASK UINT8_C(0x08) - -/*! @name Bit position definitions for power control register */ -#define BMI2_GYR_EN_POS UINT8_C(0x01) -#define BMI2_ACC_EN_POS UINT8_C(0x02) -#define BMI2_TEMP_EN_POS UINT8_C(0x03) +#define BMI2_AUX_EN_MASK UINT8_C(0x01) +#define BMI2_GYR_EN_MASK UINT8_C(0x02) +#define BMI2_ACC_EN_MASK UINT8_C(0x04) +#define BMI2_TEMP_EN_MASK UINT8_C(0x08) /*! @name Mask definitions for sensor event flags */ -#define BMI2_EVENT_FLAG_MASK UINT8_C(0x1C) - -/*! @name Bit position definitions for sensor event flags */ -#define BMI2_EVENT_FLAG_POS UINT8_C(0x02) +#define BMI2_EVENT_FLAG_MASK UINT8_C(0x1C) /*! @name Mask definitions to switch page */ -#define BMI2_SWITCH_PAGE_EN_MASK UINT8_C(0x07) +#define BMI2_SWITCH_PAGE_EN_MASK UINT8_C(0x07) + +/*! @name Mask definitions of NVM register */ +#define BMI2_NV_ACC_OFFSET_MASK UINT8_C(0x08) + +/*! @name Mask definition for config version */ +#define BMI2_CONFIG_MAJOR_MASK UINT16_C(0x3C0) +#define BMI2_CONFIG_MINOR_MASK UINT8_C(0x3F) + +/*! @name mask and bit position for activity recognition settings */ +#define BMI2_ACT_RECG_POST_PROS_EN_DIS_MASK UINT8_C(0x01) +#define BMI2_ACT_RECG_BUFF_SIZE_MASK UINT8_C(0x0F) +#define BMI2_ACT_RECG_MIN_SEG_CONF_MASK UINT8_C(0x0F) + +/*! @name mask and bit position for activity recognition hc settings */ +#define BMI2_HC_ACT_RECG_SEGMENT_SIZE_MASK UINT8_C(0x03) +#define BMI2_HC_ACT_RECG_PP_EN_MASK UINT8_C(0x01) +#define BMI2_HC_ACT_RECG_MIN_GDI_THRES_MASK UINT16_C(0xFFFF) +#define BMI2_HC_ACT_RECG_MAX_GDI_THRES_MASK UINT16_C(0xFFFF) +#define BMI2_HC_ACT_RECG_BUF_SIZE_MASK UINT16_C(0xFFFF) +#define BMI2_HC_ACT_RECG_MIN_SEG_CONF_MASK UINT16_C(0xFFFF) + +#define BMI2_GYRO_CROSS_AXES_SENSE_MASK UINT8_C(0x7F) +#define BMI2_GYRO_CROSS_AXES_SENSE_SIGN_BIT_MASK UINT8_C(0x40) + +/*! @name Bit position definitions for Gyro CRT */ +#define BMI2_GYR_RDY_FOR_DL_POS UINT8_C(0x03) +#define BMI2_GYR_CRT_RUNNING_POS UINT8_C(0x02) + +/*! @name Bit position for status register*/ +#define BMI2_AUX_BUSY_POS UINT8_C(0x02) +#define BMI2_CMD_RDY_POS UINT8_C(0x04) +#define BMI2_DRDY_AUX_POS UINT8_C(0x05) +#define BMI2_DRDY_GYR_POS UINT8_C(0x06) +#define BMI2_DRDY_ACC_POS UINT8_C(0x07) + +/*! @name Bit position definitions for power control register */ +#define BMI2_GYR_EN_POS UINT8_C(0x01) +#define BMI2_ACC_EN_POS UINT8_C(0x02) +#define BMI2_TEMP_EN_POS UINT8_C(0x03) + +/*! @name Bit position definitions for sensor event flags */ +#define BMI2_EVENT_FLAG_POS UINT8_C(0x02) + +/*! @name Bit position definitions of NVM register */ +#define BMI2_NV_ACC_OFFSET_POS UINT8_C(0x03) + +/*! @name Bit position for major version from config */ +#define BMI2_CONFIG_MAJOR_POS UINT8_C(0x06) /*! @name Accelerometer and Gyroscope Filter/Noise performance modes */ /* Power optimized mode */ -#define BMI2_POWER_OPT_MODE UINT8_C(0) +#define BMI2_POWER_OPT_MODE UINT8_C(0) /* Performance optimized */ -#define BMI2_PERF_OPT_MODE UINT8_C(1) - -/*! @name Mask definitions of NVM register */ -#define BMI2_NV_ACC_OFFSET_MASK UINT8_C(0x08) - -/*! @name Bit position definitions of NVM register */ -#define BMI2_NV_ACC_OFFSET_POS UINT8_C(0x03) - -/*! @name Mask definition for config version */ -#define BMI2_CONFIG_MAJOR_MASK UINT16_C(0x3C0) -#define BMI2_CONFIG_MINOR_MASK UINT8_C(0x3F) - -/*! @name Bit position for major version from config */ -#define BMI2_CONFIG_MAJOR_POS UINT8_C(0x06) - -#define BMI2_GYRO_CROSS_AXES_SENSE_MASK UINT8_C(0x7F) -#define BMI2_GYRO_CROSS_AXES_SENSE_SIGN_BIT_MASK UINT8_C(0x40) +#define BMI2_PERF_OPT_MODE UINT8_C(1) /*! @name index for config major minor information */ -#define BMI2_CONFIG_INFO_LOWER UINT8_C(52) -#define BMI2_CONFIG_INFO_HIGHER UINT8_C(53) +#define BMI2_CONFIG_INFO_LOWER UINT8_C(52) +#define BMI2_CONFIG_INFO_HIGHER UINT8_C(53) /*! @name Sensor status */ -#define BMI2_DRDY_ACC UINT8_C(0x80) -#define BMI2_DRDY_GYR UINT8_C(0x40) -#define BMI2_DRDY_AUX UINT8_C(0x20) -#define BMI2_CMD_RDY UINT8_C(0x10) -#define BMI2_AUX_BUSY UINT8_C(0x04) +#define BMI2_DRDY_ACC UINT8_C(0x80) +#define BMI2_DRDY_GYR UINT8_C(0x40) +#define BMI2_DRDY_AUX UINT8_C(0x20) +#define BMI2_CMD_RDY UINT8_C(0x10) +#define BMI2_AUX_BUSY UINT8_C(0x04) /*! @name Macro to define accelerometer configuration value for FOC */ -#define BMI2_FOC_ACC_CONF_VAL UINT8_C(0xB7) +#define BMI2_FOC_ACC_CONF_VAL UINT8_C(0xB7) /*! @name Macro to define gyroscope configuration value for FOC */ -#define BMI2_FOC_GYR_CONF_VAL UINT8_C(0xB6) +#define BMI2_FOC_GYR_CONF_VAL UINT8_C(0xB6) /*! @name Macro to define X Y and Z axis for an array */ -#define BMI2_X_AXIS UINT8_C(0) -#define BMI2_Y_AXIS UINT8_C(1) -#define BMI2_Z_AXIS UINT8_C(2) - -/*! @name mask and bit position for activity recognition settings */ -#define BMI2_ACT_RECG_POST_PROS_EN_DIS_MASK UINT8_C(0x01) -#define BMI2_ACT_RECG_BUFF_SIZE_MASK UINT8_C(0x0F) -#define BMI2_ACT_RECG_MIN_SEG_CONF_MASK UINT8_C(0x0F) +#define BMI2_X_AXIS UINT8_C(0) +#define BMI2_Y_AXIS UINT8_C(1) +#define BMI2_Z_AXIS UINT8_C(2) /******************************************************************************/ /*! @name Sensor Macro Definitions */ /******************************************************************************/ /*! @name Macros to define BMI2 sensor/feature types */ -#define BMI2_ACCEL UINT8_C(0) -#define BMI2_GYRO UINT8_C(1) -#define BMI2_AUX UINT8_C(2) -#define BMI2_SIG_MOTION UINT8_C(3) -#define BMI2_ANY_MOTION UINT8_C(4) -#define BMI2_NO_MOTION UINT8_C(5) -#define BMI2_STEP_DETECTOR UINT8_C(6) -#define BMI2_STEP_COUNTER UINT8_C(7) -#define BMI2_STEP_ACTIVITY UINT8_C(8) -#define BMI2_GYRO_GAIN_UPDATE UINT8_C(9) -#define BMI2_TILT UINT8_C(10) -#define BMI2_UP_HOLD_TO_WAKE UINT8_C(11) -#define BMI2_GLANCE_DETECTOR UINT8_C(12) -#define BMI2_WAKE_UP UINT8_C(13) -#define BMI2_ORIENTATION UINT8_C(14) -#define BMI2_HIGH_G UINT8_C(15) -#define BMI2_LOW_G UINT8_C(16) -#define BMI2_FLAT UINT8_C(17) -#define BMI2_EXT_SENS_SYNC UINT8_C(18) -#define BMI2_WRIST_GESTURE UINT8_C(19) -#define BMI2_WRIST_WEAR_WAKE_UP UINT8_C(20) -#define BMI2_WRIST_WEAR_WAKE_UP_WH UINT8_C(21) -#define BMI2_WRIST_GESTURE_WH UINT8_C(22) -#define BMI2_PRIMARY_OIS UINT8_C(23) -#define BMI2_FREE_FALL_DET UINT8_C(24) +#define BMI2_ACCEL UINT8_C(0) +#define BMI2_GYRO UINT8_C(1) +#define BMI2_AUX UINT8_C(2) +#define BMI2_SIG_MOTION UINT8_C(3) +#define BMI2_ANY_MOTION UINT8_C(4) +#define BMI2_NO_MOTION UINT8_C(5) +#define BMI2_STEP_DETECTOR UINT8_C(6) +#define BMI2_STEP_COUNTER UINT8_C(7) +#define BMI2_STEP_ACTIVITY UINT8_C(8) +#define BMI2_GYRO_GAIN_UPDATE UINT8_C(9) +#define BMI2_TILT UINT8_C(10) +#define BMI2_UP_HOLD_TO_WAKE UINT8_C(11) +#define BMI2_GLANCE_DETECTOR UINT8_C(12) +#define BMI2_WAKE_UP UINT8_C(13) +#define BMI2_ORIENTATION UINT8_C(14) +#define BMI2_HIGH_G UINT8_C(15) +#define BMI2_LOW_G UINT8_C(16) +#define BMI2_FLAT UINT8_C(17) +#define BMI2_EXT_SENS_SYNC UINT8_C(18) +#define BMI2_WRIST_GESTURE UINT8_C(19) +#define BMI2_WRIST_WEAR_WAKE_UP UINT8_C(20) +#define BMI2_WRIST_WEAR_WAKE_UP_WH UINT8_C(21) +#define BMI2_WRIST_GESTURE_WH UINT8_C(22) +#define BMI2_PRIMARY_OIS UINT8_C(23) +#define BMI2_FREE_FALL_DET UINT8_C(24) +#define BMI2_SINGLE_TAP UINT8_C(25) +#define BMI2_DOUBLE_TAP UINT8_C(26) +#define BMI2_TRIPLE_TAP UINT8_C(27) +#define BMI2_TAP UINT8_C(28) /* Non virtual sensor features */ -#define BMI2_STEP_COUNTER_PARAMS UINT8_C(25) -#define BMI2_TEMP UINT8_C(26) -#define BMI2_ACCEL_SELF_TEST UINT8_C(27) -#define BMI2_GYRO_SELF_OFF UINT8_C(28) -#define BMI2_ACTIVITY_RECOGNITION UINT8_C(29) -#define BMI2_MAX_BURST_LEN UINT8_C(30) -#define BMI2_SENS_MAX_NUM UINT8_C(31) -#define BMI2_AXIS_MAP UINT8_C(32) -#define BMI2_NVM_STATUS UINT8_C(33) -#define BMI2_VFRM_STATUS UINT8_C(34) -#define BMI2_GYRO_CROSS_SENSE UINT8_C(35) -#define BMI2_CRT_GYRO_SELF_TEST UINT8_C(36) -#define BMI2_ABORT_CRT_GYRO_SELF_TEST UINT8_C(37) -#define BMI2_NVM_PROG_PREP UINT8_C(38) -#define BMI2_ACTIVITY_RECOGNITION_SETTINGS UINT8_C(39) -#define BMI2_OIS_OUTPUT UINT8_C(40) -#define BMI2_CONFIG_ID UINT8_C(41) +#define BMI2_STEP_COUNTER_PARAMS UINT8_C(29) +#define BMI2_TAP_DETECTOR_1 UINT8_C(30) +#define BMI2_TAP_DETECTOR_2 UINT8_C(31) +#define BMI2_TEMP UINT8_C(32) +#define BMI2_ACCEL_SELF_TEST UINT8_C(33) +#define BMI2_GYRO_SELF_OFF UINT8_C(34) +#define BMI2_ACTIVITY_RECOGNITION UINT8_C(35) +#define BMI2_MAX_BURST_LEN UINT8_C(36) +#define BMI2_SENS_MAX_NUM UINT8_C(37) +#define BMI2_AXIS_MAP UINT8_C(38) +#define BMI2_NVM_STATUS UINT8_C(39) +#define BMI2_VFRM_STATUS UINT8_C(40) +#define BMI2_GYRO_CROSS_SENSE UINT8_C(41) +#define BMI2_CRT_GYRO_SELF_TEST UINT8_C(42) +#define BMI2_ABORT_CRT_GYRO_SELF_TEST UINT8_C(43) +#define BMI2_NVM_PROG_PREP UINT8_C(44) +#define BMI2_ACTIVITY_RECOGNITION_SETTINGS UINT8_C(45) +#define BMI2_OIS_OUTPUT UINT8_C(46) +#define BMI2_CONFIG_ID UINT8_C(47) /*! @name Bit wise for selecting BMI2 sensors/features */ -#define BMI2_ACCEL_SENS_SEL (1) -#define BMI2_GYRO_SENS_SEL (1 << BMI2_GYRO) -#define BMI2_AUX_SENS_SEL (1 << BMI2_AUX) -#define BMI2_TEMP_SENS_SEL (1 << BMI2_TEMP) -#define BMI2_ANY_MOT_SEL (1 << BMI2_ANY_MOTION) -#define BMI2_NO_MOT_SEL (1 << BMI2_NO_MOTION) -#define BMI2_TILT_SEL (1 << BMI2_TILT) -#define BMI2_ORIENT_SEL (1 << BMI2_ORIENTATION) -#define BMI2_SIG_MOTION_SEL (1 << BMI2_SIG_MOTION) -#define BMI2_STEP_DETECT_SEL (1 << BMI2_STEP_DETECTOR) -#define BMI2_STEP_COUNT_SEL (1 << BMI2_STEP_COUNTER) -#define BMI2_STEP_ACT_SEL (1 << BMI2_STEP_ACTIVITY) -#define BMI2_GYRO_GAIN_UPDATE_SEL (1 << BMI2_GYRO_GAIN_UPDATE) -#define BMI2_UP_HOLD_TO_WAKE_SEL (1 << BMI2_UP_HOLD_TO_WAKE) -#define BMI2_GLANCE_DET_SEL (1 << BMI2_GLANCE_DETECTOR) -#define BMI2_WAKE_UP_SEL (1 << BMI2_WAKE_UP) -#define BMI2_HIGH_G_SEL (1 << BMI2_HIGH_G) -#define BMI2_LOW_G_SEL (1 << BMI2_LOW_G) -#define BMI2_FLAT_SEL (1 << BMI2_FLAT) -#define BMI2_EXT_SENS_SEL (1 << BMI2_EXT_SENS_SYNC) -#define BMI2_GYRO_SELF_OFF_SEL (1 << BMI2_GYRO_SELF_OFF) -#define BMI2_WRIST_GEST_SEL (1 << BMI2_WRIST_GESTURE) -#define BMI2_WRIST_WEAR_WAKE_UP_SEL (1 << BMI2_WRIST_WEAR_WAKE_UP) -#define BMI2_ACTIVITY_RECOGNITION_SEL (1 << BMI2_ACTIVITY_RECOGNITION) -#define BMI2_ACCEL_SELF_TEST_SEL (1 << BMI2_ACCEL_SELF_TEST) -#define BMI2_WRIST_GEST_W_SEL (1 << BMI2_WRIST_GESTURE_WH) -#define BMI2_WRIST_WEAR_WAKE_UP_WH_SEL (1 << BMI2_WRIST_WEAR_WAKE_UP_WH) -#define BMI2_PRIMARY_OIS_SEL (1 << BMI2_PRIMARY_OIS) -#define BMI2_FREE_FALL_DET_SEL (1 << BMI2_FREE_FALL_DET) +#define BMI2_ACCEL_SENS_SEL (1) +#define BMI2_GYRO_SENS_SEL (1 << BMI2_GYRO) +#define BMI2_AUX_SENS_SEL (1 << BMI2_AUX) +#define BMI2_TEMP_SENS_SEL ((uint64_t)1 << BMI2_TEMP) +#define BMI2_ANY_MOT_SEL (1 << BMI2_ANY_MOTION) +#define BMI2_NO_MOT_SEL (1 << BMI2_NO_MOTION) +#define BMI2_TILT_SEL (1 << BMI2_TILT) +#define BMI2_ORIENT_SEL (1 << BMI2_ORIENTATION) +#define BMI2_SIG_MOTION_SEL (1 << BMI2_SIG_MOTION) +#define BMI2_STEP_DETECT_SEL (1 << BMI2_STEP_DETECTOR) +#define BMI2_STEP_COUNT_SEL (1 << BMI2_STEP_COUNTER) +#define BMI2_STEP_ACT_SEL (1 << BMI2_STEP_ACTIVITY) +#define BMI2_GYRO_GAIN_UPDATE_SEL (1 << BMI2_GYRO_GAIN_UPDATE) +#define BMI2_UP_HOLD_TO_WAKE_SEL (1 << BMI2_UP_HOLD_TO_WAKE) +#define BMI2_GLANCE_DET_SEL (1 << BMI2_GLANCE_DETECTOR) +#define BMI2_WAKE_UP_SEL (1 << BMI2_WAKE_UP) +#define BMI2_TAP_SEL (1 << BMI2_TAP) +#define BMI2_HIGH_G_SEL (1 << BMI2_HIGH_G) +#define BMI2_LOW_G_SEL (1 << BMI2_LOW_G) +#define BMI2_FLAT_SEL (1 << BMI2_FLAT) +#define BMI2_EXT_SENS_SEL (1 << BMI2_EXT_SENS_SYNC) +#define BMI2_SINGLE_TAP_SEL (1 << BMI2_SINGLE_TAP) +#define BMI2_DOUBLE_TAP_SEL (1 << BMI2_DOUBLE_TAP) +#define BMI2_TRIPLE_TAP_SEL (1 << BMI2_TRIPLE_TAP) +#define BMI2_GYRO_SELF_OFF_SEL ((uint64_t)1 << BMI2_GYRO_SELF_OFF) +#define BMI2_WRIST_GEST_SEL (1 << BMI2_WRIST_GESTURE) +#define BMI2_WRIST_WEAR_WAKE_UP_SEL (1 << BMI2_WRIST_WEAR_WAKE_UP) +#define BMI2_ACTIVITY_RECOGNITION_SEL ((uint64_t)1 << BMI2_ACTIVITY_RECOGNITION) +#define BMI2_ACCEL_SELF_TEST_SEL ((uint64_t)1 << BMI2_ACCEL_SELF_TEST) +#define BMI2_WRIST_GEST_W_SEL (1 << BMI2_WRIST_GESTURE_WH) +#define BMI2_WRIST_WEAR_WAKE_UP_WH_SEL (1 << BMI2_WRIST_WEAR_WAKE_UP_WH) +#define BMI2_PRIMARY_OIS_SEL (1 << BMI2_PRIMARY_OIS) +#define BMI2_FREE_FALL_DET_SEL (1 << BMI2_FREE_FALL_DET) + +/*! @name Mask definitions for BMI2 wake-up feature configuration for bmi260 */ +#define BMI2_WAKEUP_SENSITIVITY_MASK UINT8_C(0x0E) +#define BMI2_WAKEUP_SINGLE_TAP_EN_MASK UINT8_C(0x01) +#define BMI2_WAKEUP_DOUBLE_TAP_EN_MASK UINT8_C(0x02) +#define BMI2_WAKEUP_TRIPLE_TAP_EN_MASK UINT8_C(0x04) +#define BMI2_WAKEUP_DATA_REG_EN_MASK UINT8_C(0x08) +#define BMI2_WAKEUP_AXIS_SEL_MASK UINT8_C(0x03) + +/*! @name Bit position definitions for BMI2 wake-up feature configuration for bmi260 */ +#define BMI2_WAKEUP_SENSITIVITY_POS UINT8_C(0x01) +#define BMI2_WAKEUP_DOUBLE_TAP_EN_POS UINT8_C(0x01) +#define BMI2_WAKEUP_TRIPLE_TAP_EN_POS UINT8_C(0x02) +#define BMI2_WAKEUP_DATA_REG_EN_POS UINT8_C(0x03) + +/*! @name Mask definitions for BMI2 tap feature configuration for bmi260t */ +#define BMI2_TAP_SENSITIVITY_MASK UINT8_C(0x0E) +#define BMI2_TAP_SINGLE_TAP_EN_MASK UINT8_C(0x01) +#define BMI2_TAP_DOUBLE_TAP_EN_MASK UINT8_C(0x02) +#define BMI2_TAP_TRIPLE_TAP_EN_MASK UINT8_C(0x04) +#define BMI2_TAP_DATA_REG_EN_MASK UINT8_C(0x08) +#define BMI2_TAP_AXIS_SEL_MASK UINT8_C(0x03) + +/*! @name Bit position definitions for BMI2 tap feature configuration for bmi260t */ +#define BMI2_TAP_SENSITIVITY_POS UINT8_C(0x01) +#define BMI2_TAP_DOUBLE_TAP_EN_POS UINT8_C(0x01) +#define BMI2_TAP_TRIPLE_TAP_EN_POS UINT8_C(0x02) +#define BMI2_TAP_DATA_REG_EN_POS UINT8_C(0x03) + +/*! @name Mask definitions for BMI2 wake-up feature configuration for other than bmi261 */ +#define BMI2_WAKE_UP_SENSITIVITY_MASK UINT16_C(0x000E) +#define BMI2_WAKE_UP_SINGLE_TAP_EN_MASK UINT16_C(0x0010) + +/*! @name Bit position definitions for BMI2 wake-up feature configuration for other than bmi261 */ +#define BMI2_WAKE_UP_SENSITIVITY_POS UINT8_C(0x01) +#define BMI2_WAKE_UP_SINGLE_TAP_EN_POS UINT8_C(0x04) + +/*! @name Offsets from feature start address for BMI2 feature enable/disable */ +#define BMI2_ANY_MOT_FEAT_EN_OFFSET UINT8_C(0x03) +#define BMI2_NO_MOT_FEAT_EN_OFFSET UINT8_C(0x03) +#define BMI2_SIG_MOT_FEAT_EN_OFFSET UINT8_C(0x0A) +#define BMI2_STEP_COUNT_FEAT_EN_OFFSET UINT8_C(0x01) +#define BMI2_GYR_USER_GAIN_FEAT_EN_OFFSET UINT8_C(0x05) +#define BMI2_HIGH_G_FEAT_EN_OFFSET UINT8_C(0x03) +#define BMI2_LOW_G_FEAT_EN_OFFSET UINT8_C(0x03) +#define BMI2_TILT_FEAT_EN_OFFSET UINT8_C(0x00) + +/*! @name Mask definitions for BMI2 feature enable/disable */ +#define BMI2_ANY_NO_MOT_EN_MASK UINT8_C(0x80) +#define BMI2_TILT_FEAT_EN_MASK UINT8_C(0x01) +#define BMI2_ORIENT_FEAT_EN_MASK UINT8_C(0x01) +#define BMI2_SIG_MOT_FEAT_EN_MASK UINT8_C(0x01) +#define BMI2_STEP_DET_FEAT_EN_MASK UINT8_C(0x08) +#define BMI2_STEP_COUNT_FEAT_EN_MASK UINT8_C(0x10) +#define BMI2_STEP_ACT_FEAT_EN_MASK UINT8_C(0x20) +#define BMI2_GYR_USER_GAIN_FEAT_EN_MASK UINT8_C(0x08) +#define BMI2_UP_HOLD_TO_WAKE_FEAT_EN_MASK UINT8_C(0x01) +#define BMI2_GLANCE_FEAT_EN_MASK UINT8_C(0x01) +#define BMI2_WAKE_UP_FEAT_EN_MASK UINT8_C(0x01) +#define BMI2_TAP_FEAT_EN_MASK UINT8_C(0x01) +#define BMI2_HIGH_G_FEAT_EN_MASK UINT8_C(0x80) +#define BMI2_LOW_G_FEAT_EN_MASK UINT8_C(0x10) +#define BMI2_FLAT_FEAT_EN_MASK UINT8_C(0x01) +#define BMI2_EXT_SENS_SYNC_FEAT_EN_MASK UINT8_C(0x01) +#define BMI2_GYR_SELF_OFF_CORR_FEAT_EN_MASK UINT8_C(0x02) +#define BMI2_WRIST_GEST_FEAT_EN_MASK UINT8_C(0x20) +#define BMI2_WRIST_WEAR_WAKE_UP_FEAT_EN_MASK UINT8_C(0x10) +#define BMI2_ACTIVITY_RECOG_EN_MASK UINT8_C(0x01) +#define BMI2_ACC_SELF_TEST_FEAT_EN_MASK UINT8_C(0x02) +#define BMI2_GYRO_SELF_TEST_CRT_EN_MASK UINT8_C(0x01) +#define BMI2_ABORT_FEATURE_EN_MASK UINT8_C(0x02) +#define BMI2_NVM_PREP_FEATURE_EN_MASK UINT8_C(0x04) +#define BMI2_FREE_FALL_DET_FEAT_EN_MASK UINT8_C(0x01) +#define BMI2_WRIST_GEST_WH_FEAT_EN_MASK UINT8_C(0x02) + +/*! @name Bit position definitions for BMI2 feature enable/disable */ +#define BMI2_ANY_NO_MOT_EN_POS UINT8_C(0x07) +#define BMI2_STEP_DET_FEAT_EN_POS UINT8_C(0x03) +#define BMI2_STEP_COUNT_FEAT_EN_POS UINT8_C(0x04) +#define BMI2_STEP_ACT_FEAT_EN_POS UINT8_C(0x05) +#define BMI2_GYR_USER_GAIN_FEAT_EN_POS UINT8_C(0x03) +#define BMI2_HIGH_G_FEAT_EN_POS UINT8_C(0x07) +#define BMI2_LOW_G_FEAT_EN_POS UINT8_C(0x04) +#define BMI2_GYR_SELF_OFF_CORR_FEAT_EN_POS UINT8_C(0x01) +#define BMI2_WRIST_GEST_FEAT_EN_POS UINT8_C(0x05) +#define BMI2_WRIST_WEAR_WAKE_UP_FEAT_EN_POS UINT8_C(0x04) +#define BMI2_ACC_SELF_TEST_FEAT_EN_POS UINT8_C(0x01) +#define BMI2_ABORT_FEATURE_EN_POS UINT8_C(0x1) +#define BMI2_NVM_PREP_FEATURE_EN_POS UINT8_C(0x02) +#define BMI2_WRIST_GEST_WH_FEAT_EN_POS UINT8_C(0x01) + +/*! Primary OIS low pass filter configuration position and mask */ +#define BMI2_LP_FILTER_EN_MASK UINT8_C(0x01) + +#define BMI2_LP_FILTER_CONFIG_POS UINT8_C(0x01) +#define BMI2_LP_FILTER_CONFIG_MASK UINT8_C(0x06) + +#define BMI2_PRIMARY_OIS_GYR_EN_POS UINT8_C(0x06) +#define BMI2_PRIMARY_OIS_GYR_EN_MASK UINT8_C(0x40) + +#define BMI2_PRIMARY_OIS_ACC_EN_POS UINT8_C(0x07) +#define BMI2_PRIMARY_OIS_ACC_EN_MASK UINT8_C(0x80) + +/*! @name Mask definitions for BMI2 any and no-motion feature configuration */ +#define BMI2_ANY_NO_MOT_DUR_MASK UINT16_C(0x1FFF) +#define BMI2_ANY_NO_MOT_X_SEL_MASK UINT16_C(0x2000) +#define BMI2_ANY_NO_MOT_Y_SEL_MASK UINT16_C(0x4000) +#define BMI2_ANY_NO_MOT_Z_SEL_MASK UINT16_C(0x8000) +#define BMI2_ANY_NO_MOT_THRES_MASK UINT16_C(0x07FF) +#define BMI2_ANY_MOT_INT_MASK UINT8_C(0x40) + +/*! @name Mask definitions for BMI2 no-motion interrupt mapping */ +#define BMI2_NO_MOT_INT_MASK UINT8_C(0x20) + +/*! @name Bit position definitions for BMI2 any and no-motion feature + * configuration + */ +#define BMI2_ANY_NO_MOT_X_SEL_POS UINT8_C(0x0D) +#define BMI2_ANY_NO_MOT_Y_SEL_POS UINT8_C(0x0E) +#define BMI2_ANY_NO_MOT_Z_SEL_POS UINT8_C(0x0F) + +/*! @name Mask definitions for BMI2 orientation feature configuration */ +#define BMI2_ORIENT_UP_DOWN_MASK UINT16_C(0x0002) +#define BMI2_ORIENT_SYMM_MODE_MASK UINT16_C(0x000C) +#define BMI2_ORIENT_BLOCK_MODE_MASK UINT16_C(0x0030) +#define BMI2_ORIENT_THETA_MASK UINT16_C(0x0FC0) +#define BMI2_ORIENT_HYST_MASK UINT16_C(0x07FF) + +/*! @name Bit position definitions for BMI2 orientation feature configuration */ +#define BMI2_ORIENT_UP_DOWN_POS UINT8_C(0x01) +#define BMI2_ORIENT_SYMM_MODE_POS UINT8_C(0x02) +#define BMI2_ORIENT_BLOCK_MODE_POS UINT8_C(0x04) +#define BMI2_ORIENT_THETA_POS UINT8_C(0x06) + +/*! @name Mask definitions for BMI2 sig-motion feature configuration */ +#define BMI2_SIG_MOT_PARAM_1_MASK UINT16_C(0xFFFF) +#define BMI2_SIG_MOT_PARAM_2_MASK UINT16_C(0xFFFF) +#define BMI2_SIG_MOT_PARAM_3_MASK UINT16_C(0xFFFF) +#define BMI2_SIG_MOT_PARAM_4_MASK UINT16_C(0xFFFF) +#define BMI2_SIG_MOT_PARAM_5_MASK UINT16_C(0xFFFF) + +/*! @name Mask definitions for BMI2 parameter configurations */ +#define BMI2_STEP_COUNT_PARAMS_MASK UINT16_C(0xFFFF) + +/*! @name Mask definitions for BMI2 step-counter/detector feature configuration */ +#define BMI2_STEP_COUNT_WM_LEVEL_MASK UINT16_C(0x03FF) +#define BMI2_STEP_COUNT_RST_CNT_MASK UINT16_C(0x0400) +#define BMI2_STEP_BUFFER_SIZE_MASK UINT16_C(0XFF00) +#define BMI2_STEP_COUNT_INT_MASK UINT8_C(0x02) +#define BMI2_STEP_ACT_INT_MASK UINT8_C(0x04) + +/*! @name Bit position definitions for BMI2 step-counter/detector feature + * configuration + */ +#define BMI2_STEP_COUNT_RST_CNT_POS UINT8_C(0x0A) +#define BMI2_STEP_BUFFER_SIZE_POS UINT8_C(0X08) + +/*! @name Mask definitions for BMI2 gyroscope user gain feature + * configuration + */ +#define BMI2_GYR_USER_GAIN_RATIO_X_MASK UINT16_C(0x07FF) +#define BMI2_GYR_USER_GAIN_RATIO_Y_MASK UINT16_C(0x07FF) +#define BMI2_GYR_USER_GAIN_RATIO_Z_MASK UINT16_C(0x07FF) + +/*! @name Mask definitions for BMI2 gyroscope user gain saturation status */ +#define BMI2_GYR_USER_GAIN_SAT_STAT_X_MASK UINT8_C(0x01) +#define BMI2_GYR_USER_GAIN_SAT_STAT_Y_MASK UINT8_C(0x02) +#define BMI2_GYR_USER_GAIN_SAT_STAT_Z_MASK UINT8_C(0x04) +#define BMI2_G_TRIGGER_STAT_MASK UINT8_C(0x38) + +/*! @name Bit position definitions for BMI2 gyroscope user gain saturation status */ +#define BMI2_GYR_USER_GAIN_SAT_STAT_Y_POS UINT8_C(0x01) +#define BMI2_GYR_USER_GAIN_SAT_STAT_Z_POS UINT8_C(0x02) +#define BMI2_G_TRIGGER_STAT_POS UINT8_C(0x03) + +/*! @name Mask definitions for MSB values of BMI2 gyroscope compensation */ +#define BMI2_GYR_OFF_COMP_MSB_X_MASK UINT8_C(0x03) +#define BMI2_GYR_OFF_COMP_MSB_Y_MASK UINT8_C(0x0C) +#define BMI2_GYR_OFF_COMP_MSB_Z_MASK UINT8_C(0x30) + +/*! @name Bit positions for MSB values of BMI2 gyroscope compensation */ +#define BMI2_GYR_OFF_COMP_MSB_Y_POS UINT8_C(0x02) +#define BMI2_GYR_OFF_COMP_MSB_Z_POS UINT8_C(0x04) + +/*! @name Mask definitions for MSB values of BMI2 gyroscope compensation from user input */ +#define BMI2_GYR_OFF_COMP_MSB_MASK UINT16_C(0x0300) +#define BMI2_GYR_OFF_COMP_LSB_MASK UINT16_C(0x00FF) + +/*! @name Mask definitions for BMI2 orientation status */ +#define BMI2_ORIENT_DETECT_MASK UINT8_C(0x03) +#define BMI2_ORIENT_FACE_UP_DWN_MASK UINT8_C(0x04) + +/*! @name Bit position definitions for BMI2 orientation status */ +#define BMI2_ORIENT_FACE_UP_DWN_POS UINT8_C(0x02) + +/*! @name Mask definitions for NVM-VFRM error status */ +#define BMI2_NVM_LOAD_ERR_STATUS_MASK UINT8_C(0x01) +#define BMI2_NVM_PROG_ERR_STATUS_MASK UINT8_C(0x02) +#define BMI2_NVM_ERASE_ERR_STATUS_MASK UINT8_C(0x04) +#define BMI2_NVM_END_EXCEED_STATUS_MASK UINT8_C(0x08) +#define BMI2_NVM_PRIV_ERR_STATUS_MASK UINT8_C(0x10) +#define BMI2_VFRM_LOCK_ERR_STATUS_MASK UINT8_C(0x20) +#define BMI2_VFRM_WRITE_ERR_STATUS_MASK UINT8_C(0x40) +#define BMI2_VFRM_FATAL_ERR_STATUS_MASK UINT8_C(0x80) + +/*! @name Bit positions for NVM-VFRM error status */ +#define BMI2_NVM_PROG_ERR_STATUS_POS UINT8_C(0x01) +#define BMI2_NVM_ERASE_ERR_STATUS_POS UINT8_C(0x02) +#define BMI2_NVM_END_EXCEED_STATUS_POS UINT8_C(0x03) +#define BMI2_NVM_PRIV_ERR_STATUS_POS UINT8_C(0x04) +#define BMI2_VFRM_LOCK_ERR_STATUS_POS UINT8_C(0x05) +#define BMI2_VFRM_WRITE_ERR_STATUS_POS UINT8_C(0x06) +#define BMI2_VFRM_FATAL_ERR_STATUS_POS UINT8_C(0x07) + +/*! @name Mask definitions for accelerometer self-test status */ +#define BMI2_ACC_SELF_TEST_DONE_MASK UINT8_C(0x01) +#define BMI2_ACC_X_OK_MASK UINT8_C(0x02) +#define BMI2_ACC_Y_OK_MASK UINT8_C(0x04) +#define BMI2_ACC_Z_OK_MASK UINT8_C(0x08) + +/*! @name Bit Positions for accelerometer self-test status */ +#define BMI2_ACC_X_OK_POS UINT8_C(0x01) +#define BMI2_ACC_Y_OK_POS UINT8_C(0x02) +#define BMI2_ACC_Z_OK_POS UINT8_C(0x03) + +/*! @name Mask definitions for BMI2 high-g feature configuration */ +#define BMI2_HIGH_G_THRES_MASK UINT16_C(0x7FFF) +#define BMI2_HIGH_G_HYST_MASK UINT16_C(0x0FFF) +#define BMI2_HIGH_G_X_SEL_MASK UINT16_C(0x1000) +#define BMI2_HIGH_G_Y_SEL_MASK UINT16_C(0x2000) +#define BMI2_HIGH_G_Z_SEL_MASK UINT16_C(0x4000) +#define BMI2_HIGH_G_DUR_MASK UINT16_C(0x0FFF) + +/*! @name Bit position definitions for BMI2 high-g feature configuration */ +#define BMI2_HIGH_G_X_SEL_POS UINT8_C(0x0C) +#define BMI2_HIGH_G_Y_SEL_POS UINT8_C(0x0D) +#define BMI2_HIGH_G_Z_SEL_POS UINT8_C(0x0E) + +/*! @name Mask definitions for BMI2 low-g feature configuration */ +#define BMI2_LOW_G_THRES_MASK UINT16_C(0x7FFF) +#define BMI2_LOW_G_HYST_MASK UINT16_C(0x0FFF) +#define BMI2_LOW_G_DUR_MASK UINT16_C(0x0FFF) + +/*! @name Mask definitions for BMI2 free-fall detection feature configuration */ +#define BMI2_FREE_FALL_ACCEL_SETT_MASK UINT16_C(0xFFFF) + +/*! @name Mask definitions for BMI2 flat feature configuration */ +#define BMI2_FLAT_THETA_MASK UINT16_C(0x007E) +#define BMI2_FLAT_BLOCK_MASK UINT16_C(0x0180) +#define BMI2_FLAT_HYST_MASK UINT16_C(0x003F) +#define BMI2_FLAT_HOLD_TIME_MASK UINT16_C(0x3FC0) + +/*! @name Bit position definitions for BMI2 flat feature configuration */ +#define BMI2_FLAT_THETA_POS UINT8_C(0x01) +#define BMI2_FLAT_BLOCK_POS UINT8_C(0x07) +#define BMI2_FLAT_HOLD_TIME_POS UINT8_C(0x06) + +/*! @name Mask definitions for BMI2 wrist gesture configuration */ +#define BMI2_WRIST_GEST_WEAR_ARM_MASK UINT16_C(0x0010) + +/*! @name Bit position definitions for wrist gesture configuration */ +#define BMI2_WRIST_GEST_WEAR_ARM_POS UINT8_C(0x04) + +/*! @name Mask definitions for BMI2 wrist gesture wh configuration */ +#define BMI2_WRIST_GEST_WH_DEVICE_POS_MASK UINT16_C(0x0001) +#define BMI2_WRIST_GEST_WH_INT UINT8_C(0x10) +#define BMI2_WRIST_GEST_WH_START_ADD UINT8_C(0x08) + +/*! @name Mask definitions for BMI2 wrist wear wake-up configuration */ +#define BMI2_WRIST_WAKE_UP_WH_INT_MASK UINT8_C(0x08) + +/*! @name Mask definition for BMI2 wrist wear wake-up configuration for wearable variant */ +#define BMI2_WRIST_WAKE_UP_ANGLE_LR_MASK UINT16_C(0x00FF) +#define BMI2_WRIST_WAKE_UP_ANGLE_LL_MASK UINT16_C(0xFF00) +#define BMI2_WRIST_WAKE_UP_ANGLE_PD_MASK UINT16_C(0x00FF) +#define BMI2_WRIST_WAKE_UP_ANGLE_PU_MASK UINT16_C(0xFF00) +#define BMI2_WRIST_WAKE_UP_MIN_DUR_MOVED_MASK UINT16_C(0x00FF) +#define BMI2_WRIST_WAKE_UP_MIN_DUR_QUITE_MASK UINT16_C(0xFF00) + +/*! @name Bit position definition for BMI2 wrist wear wake-up configuration for wearable variant */ +#define BMI2_WRIST_WAKE_UP_ANGLE_LL_POS UINT16_C(0x0008) +#define BMI2_WRIST_WAKE_UP_ANGLE_PU_POS UINT16_C(0x0008) +#define BMI2_WRIST_WAKE_UP_MIN_DUR_QUITE_POS UINT16_C(0x0008) + +/*! @name Macros to define values of BMI2 axis and its sign for re-map + * settings + */ +#define BMI2_MAP_X_AXIS UINT8_C(0x00) +#define BMI2_MAP_Y_AXIS UINT8_C(0x01) +#define BMI2_MAP_Z_AXIS UINT8_C(0x02) +#define BMI2_MAP_POSITIVE UINT8_C(0x00) +#define BMI2_MAP_NEGATIVE UINT8_C(0x01) + +/*! @name Mask definitions of BMI2 axis re-mapping */ +#define BMI2_X_AXIS_MASK UINT8_C(0x03) +#define BMI2_X_AXIS_SIGN_MASK UINT8_C(0x04) +#define BMI2_Y_AXIS_MASK UINT8_C(0x18) +#define BMI2_Y_AXIS_SIGN_MASK UINT8_C(0x20) +#define BMI2_Z_AXIS_MASK UINT8_C(0xC0) +#define BMI2_Z_AXIS_SIGN_MASK UINT8_C(0x01) + +/*! @name Bit position definitions of BMI2 axis re-mapping */ +#define BMI2_X_AXIS_SIGN_POS UINT8_C(0x02) +#define BMI2_Y_AXIS_POS UINT8_C(0x03) +#define BMI2_Y_AXIS_SIGN_POS UINT8_C(0x05) +#define BMI2_Z_AXIS_POS UINT8_C(0x06) + +/*! @name Macros to define polarity */ +#define BMI2_NEG_SIGN UINT8_C(1) +#define BMI2_POS_SIGN UINT8_C(0) + +/*! @name Macro to define related to CRT */ +#define BMI2_CRT_READY_FOR_DOWNLOAD_US UINT16_C(2000) +#define BMI2_CRT_READY_FOR_DOWNLOAD_RETRY UINT8_C(100) + +#define BMI2_CRT_WAIT_RUNNING_US UINT16_C(10000) +#define BMI2_CRT_WAIT_RUNNING_RETRY_EXECUTION UINT8_C(200) + +#define BMI2_CRT_MIN_BURST_WORD_LENGTH UINT8_C(2) +#define BMI2_CRT_MAX_BURST_WORD_LENGTH UINT16_C(255) + +#define BMI2_ACC_FOC_2G_REF UINT16_C(16384) +#define BMI2_ACC_FOC_4G_REF UINT16_C(8192) +#define BMI2_ACC_FOC_8G_REF UINT16_C(4096) +#define BMI2_ACC_FOC_16G_REF UINT16_C(2048) + +#define BMI2_GYRO_FOC_NOISE_LIMIT_NEGATIVE INT8_C(-20) +#define BMI2_GYRO_FOC_NOISE_LIMIT_POSITIVE INT8_C(20) + +/* reference value with positive and negative noise range in lsb */ +#define BMI2_ACC_2G_MAX_NOISE_LIMIT (BMI2_ACC_FOC_2G_REF + UINT16_C(255)) +#define BMI2_ACC_2G_MIN_NOISE_LIMIT (BMI2_ACC_FOC_2G_REF - UINT16_C(255)) +#define BMI2_ACC_4G_MAX_NOISE_LIMIT (BMI2_ACC_FOC_4G_REF + UINT16_C(255)) +#define BMI2_ACC_4G_MIN_NOISE_LIMIT (BMI2_ACC_FOC_4G_REF - UINT16_C(255)) +#define BMI2_ACC_8G_MAX_NOISE_LIMIT (BMI2_ACC_FOC_8G_REF + UINT16_C(255)) +#define BMI2_ACC_8G_MIN_NOISE_LIMIT (BMI2_ACC_FOC_8G_REF - UINT16_C(255)) +#define BMI2_ACC_16G_MAX_NOISE_LIMIT (BMI2_ACC_FOC_16G_REF + UINT16_C(255)) +#define BMI2_ACC_16G_MIN_NOISE_LIMIT (BMI2_ACC_FOC_16G_REF - UINT16_C(255)) + +#define BMI2_FOC_SAMPLE_LIMIT UINT8_C(128) /*! @name Bit wise selection of BMI2 sensors */ #define BMI2_MAIN_SENSORS \ @@ -447,467 +816,538 @@ | BMI2_AUX_SENS_SEL | BMI2_TEMP_SENS_SEL) /*! @name Maximum number of BMI2 main sensors */ -#define BMI2_MAIN_SENS_MAX_NUM UINT8_C(4) +#define BMI2_MAIN_SENS_MAX_NUM UINT8_C(4) /*! @name Macro to specify the number of step counter parameters */ -#define BMI2_STEP_CNT_N_PARAMS UINT8_C(25) +#define BMI2_STEP_CNT_N_PARAMS UINT8_C(25) /*! @name Macro to specify the number of free-fall accel setting parameters */ -#define BMI2_FREE_FALL_ACCEL_SET_PARAMS UINT8_C(7) +#define BMI2_FREE_FALL_ACCEL_SET_PARAMS UINT8_C(7) -/*! @name Macro to select between single and double tap */ -#define BMI2_DOUBLE_TAP_SEL UINT8_C(0) -#define BMI2_SINGLE_TAP_SEL UINT8_C(1) - -#define BMI2_SELECT_GYRO_SELF_TEST UINT8_C(0) -#define BMI2_SELECT_CRT UINT8_C(1) +#define BMI2_SELECT_GYRO_SELF_TEST UINT8_C(0) +#define BMI2_SELECT_CRT UINT8_C(1) /*! @name Macro for NVM enable */ -#define BMI2_NVM_UNLOCK_ENABLE UINT8_C(0x02) -#define BMI2_NVM_UNLOCK_DISABLE UINT8_C(0x00) +#define BMI2_NVM_UNLOCK_ENABLE UINT8_C(0x02) +#define BMI2_NVM_UNLOCK_DISABLE UINT8_C(0x00) /*! @name macro to select between gyro self test and CRT */ -#define BMI2_GYRO_SELF_TEST_SEL UINT8_C(0) -#define BMI2_CRT_SEL UINT8_C(1) +#define BMI2_GYRO_SELF_TEST_SEL UINT8_C(0) +#define BMI2_CRT_SEL UINT8_C(1) /******************************************************************************/ /*! @name Accelerometer Macro Definitions */ /******************************************************************************/ /*! @name Accelerometer Bandwidth parameters */ -#define BMI2_ACC_OSR4_AVG1 UINT8_C(0x00) -#define BMI2_ACC_OSR2_AVG2 UINT8_C(0x01) -#define BMI2_ACC_NORMAL_AVG4 UINT8_C(0x02) -#define BMI2_ACC_CIC_AVG8 UINT8_C(0x03) -#define BMI2_ACC_RES_AVG16 UINT8_C(0x04) -#define BMI2_ACC_RES_AVG32 UINT8_C(0x05) -#define BMI2_ACC_RES_AVG64 UINT8_C(0x06) -#define BMI2_ACC_RES_AVG128 UINT8_C(0x07) +#define BMI2_ACC_OSR4_AVG1 UINT8_C(0x00) +#define BMI2_ACC_OSR2_AVG2 UINT8_C(0x01) +#define BMI2_ACC_NORMAL_AVG4 UINT8_C(0x02) +#define BMI2_ACC_CIC_AVG8 UINT8_C(0x03) +#define BMI2_ACC_RES_AVG16 UINT8_C(0x04) +#define BMI2_ACC_RES_AVG32 UINT8_C(0x05) +#define BMI2_ACC_RES_AVG64 UINT8_C(0x06) +#define BMI2_ACC_RES_AVG128 UINT8_C(0x07) /*! @name Accelerometer Output Data Rate */ -#define BMI2_ACC_ODR_0_78HZ UINT8_C(0x01) -#define BMI2_ACC_ODR_1_56HZ UINT8_C(0x02) -#define BMI2_ACC_ODR_3_12HZ UINT8_C(0x03) -#define BMI2_ACC_ODR_6_25HZ UINT8_C(0x04) -#define BMI2_ACC_ODR_12_5HZ UINT8_C(0x05) -#define BMI2_ACC_ODR_25HZ UINT8_C(0x06) -#define BMI2_ACC_ODR_50HZ UINT8_C(0x07) -#define BMI2_ACC_ODR_100HZ UINT8_C(0x08) -#define BMI2_ACC_ODR_200HZ UINT8_C(0x09) -#define BMI2_ACC_ODR_400HZ UINT8_C(0x0A) -#define BMI2_ACC_ODR_800HZ UINT8_C(0x0B) -#define BMI2_ACC_ODR_1600HZ UINT8_C(0x0C) +#define BMI2_ACC_ODR_0_78HZ UINT8_C(0x01) +#define BMI2_ACC_ODR_1_56HZ UINT8_C(0x02) +#define BMI2_ACC_ODR_3_12HZ UINT8_C(0x03) +#define BMI2_ACC_ODR_6_25HZ UINT8_C(0x04) +#define BMI2_ACC_ODR_12_5HZ UINT8_C(0x05) +#define BMI2_ACC_ODR_25HZ UINT8_C(0x06) +#define BMI2_ACC_ODR_50HZ UINT8_C(0x07) +#define BMI2_ACC_ODR_100HZ UINT8_C(0x08) +#define BMI2_ACC_ODR_200HZ UINT8_C(0x09) +#define BMI2_ACC_ODR_400HZ UINT8_C(0x0A) +#define BMI2_ACC_ODR_800HZ UINT8_C(0x0B) +#define BMI2_ACC_ODR_1600HZ UINT8_C(0x0C) /*! @name Accelerometer G Range */ -#define BMI2_ACC_RANGE_2G UINT8_C(0x00) -#define BMI2_ACC_RANGE_4G UINT8_C(0x01) -#define BMI2_ACC_RANGE_8G UINT8_C(0x02) -#define BMI2_ACC_RANGE_16G UINT8_C(0x03) +#define BMI2_ACC_RANGE_2G UINT8_C(0x00) +#define BMI2_ACC_RANGE_4G UINT8_C(0x01) +#define BMI2_ACC_RANGE_8G UINT8_C(0x02) +#define BMI2_ACC_RANGE_16G UINT8_C(0x03) /*! @name Mask definitions for accelerometer configuration register */ -#define BMI2_ACC_RANGE_MASK UINT8_C(0x03) -#define BMI2_ACC_ODR_MASK UINT8_C(0x0F) -#define BMI2_ACC_BW_PARAM_MASK UINT8_C(0x70) -#define BMI2_ACC_FILTER_PERF_MODE_MASK UINT8_C(0x80) +#define BMI2_ACC_RANGE_MASK UINT8_C(0x03) +#define BMI2_ACC_ODR_MASK UINT8_C(0x0F) +#define BMI2_ACC_BW_PARAM_MASK UINT8_C(0x70) +#define BMI2_ACC_FILTER_PERF_MODE_MASK UINT8_C(0x80) /*! @name Bit position definitions for accelerometer configuration register */ -#define BMI2_ACC_BW_PARAM_POS UINT8_C(0x04) -#define BMI2_ACC_FILTER_PERF_MODE_POS UINT8_C(0x07) +#define BMI2_ACC_BW_PARAM_POS UINT8_C(0x04) +#define BMI2_ACC_FILTER_PERF_MODE_POS UINT8_C(0x07) /*! @name Self test macro to define range */ -#define BMI2_ACC_SELF_TEST_RANGE UINT8_C(16) +#define BMI2_ACC_SELF_TEST_RANGE UINT8_C(16) /*! @name Self test macro to show resulting minimum and maximum difference * signal of the axes in mg */ -#define BMI2_ST_ACC_X_SIG_MIN_DIFF INT16_C(16000) -#define BMI2_ST_ACC_Y_SIG_MIN_DIFF INT16_C(-15000) -#define BMI2_ST_ACC_Z_SIG_MIN_DIFF INT16_C(10000) +#define BMI2_ST_ACC_X_SIG_MIN_DIFF INT16_C(16000) +#define BMI2_ST_ACC_Y_SIG_MIN_DIFF INT16_C(-15000) +#define BMI2_ST_ACC_Z_SIG_MIN_DIFF INT16_C(10000) /*! @name Mask definitions for accelerometer self-test */ -#define BMI2_ACC_SELF_TEST_EN_MASK UINT8_C(0x01) -#define BMI2_ACC_SELF_TEST_SIGN_MASK UINT8_C(0x04) -#define BMI2_ACC_SELF_TEST_AMP_MASK UINT8_C(0x08) +#define BMI2_ACC_SELF_TEST_EN_MASK UINT8_C(0x01) +#define BMI2_ACC_SELF_TEST_SIGN_MASK UINT8_C(0x04) +#define BMI2_ACC_SELF_TEST_AMP_MASK UINT8_C(0x08) /*! @name Bit Positions for accelerometer self-test */ -#define BMI2_ACC_SELF_TEST_SIGN_POS UINT8_C(0x02) -#define BMI2_ACC_SELF_TEST_AMP_POS UINT8_C(0x03) +#define BMI2_ACC_SELF_TEST_SIGN_POS UINT8_C(0x02) +#define BMI2_ACC_SELF_TEST_AMP_POS UINT8_C(0x03) /*! @name MASK definition for gyro self test status */ -#define BMI2_GYR_ST_AXES_DONE_MASK UINT8_C(0X01) -#define BMI2_GYR_AXIS_X_OK_MASK UINT8_C(0x02) -#define BMI2_GYR_AXIS_Y_OK_MASK UINT8_C(0x04) -#define BMI2_GYR_AXIS_Z_OK_MASK UINT8_C(0x08) +#define BMI2_GYR_ST_AXES_DONE_MASK UINT8_C(0X01) +#define BMI2_GYR_AXIS_X_OK_MASK UINT8_C(0x02) +#define BMI2_GYR_AXIS_Y_OK_MASK UINT8_C(0x04) +#define BMI2_GYR_AXIS_Z_OK_MASK UINT8_C(0x08) /*! @name Bit position for gyro self test status */ -#define BMI2_GYR_AXIS_X_OK_POS UINT8_C(0x01) -#define BMI2_GYR_AXIS_Y_OK_POS UINT8_C(0x02) -#define BMI2_GYR_AXIS_Z_OK_POS UINT8_C(0x03) +#define BMI2_GYR_AXIS_X_OK_POS UINT8_C(0x01) +#define BMI2_GYR_AXIS_Y_OK_POS UINT8_C(0x02) +#define BMI2_GYR_AXIS_Z_OK_POS UINT8_C(0x03) /******************************************************************************/ /*! @name Gyroscope Macro Definitions */ /******************************************************************************/ /*! @name Gyroscope Bandwidth parameters */ -#define BMI2_GYR_OSR4_MODE UINT8_C(0x00) -#define BMI2_GYR_OSR2_MODE UINT8_C(0x01) -#define BMI2_GYR_NORMAL_MODE UINT8_C(0x02) -#define BMI2_GYR_CIC_MODE UINT8_C(0x03) +#define BMI2_GYR_OSR4_MODE UINT8_C(0x00) +#define BMI2_GYR_OSR2_MODE UINT8_C(0x01) +#define BMI2_GYR_NORMAL_MODE UINT8_C(0x02) +#define BMI2_GYR_CIC_MODE UINT8_C(0x03) /*! @name Gyroscope Output Data Rate */ -#define BMI2_GYR_ODR_25HZ UINT8_C(0x06) -#define BMI2_GYR_ODR_50HZ UINT8_C(0x07) -#define BMI2_GYR_ODR_100HZ UINT8_C(0x08) -#define BMI2_GYR_ODR_200HZ UINT8_C(0x09) -#define BMI2_GYR_ODR_400HZ UINT8_C(0x0A) -#define BMI2_GYR_ODR_800HZ UINT8_C(0x0B) -#define BMI2_GYR_ODR_1600HZ UINT8_C(0x0C) -#define BMI2_GYR_ODR_3200HZ UINT8_C(0x0D) +#define BMI2_GYR_ODR_25HZ UINT8_C(0x06) +#define BMI2_GYR_ODR_50HZ UINT8_C(0x07) +#define BMI2_GYR_ODR_100HZ UINT8_C(0x08) +#define BMI2_GYR_ODR_200HZ UINT8_C(0x09) +#define BMI2_GYR_ODR_400HZ UINT8_C(0x0A) +#define BMI2_GYR_ODR_800HZ UINT8_C(0x0B) +#define BMI2_GYR_ODR_1600HZ UINT8_C(0x0C) +#define BMI2_GYR_ODR_3200HZ UINT8_C(0x0D) /*! @name Gyroscope OIS Range */ -#define BMI2_GYR_OIS_250 UINT8_C(0x00) -#define BMI2_GYR_OIS_2000 UINT8_C(0x01) +#define BMI2_GYR_OIS_250 UINT8_C(0x00) +#define BMI2_GYR_OIS_2000 UINT8_C(0x01) /*! @name Gyroscope Angular Rate Measurement Range */ -#define BMI2_GYR_RANGE_2000 UINT8_C(0x00) -#define BMI2_GYR_RANGE_1000 UINT8_C(0x01) -#define BMI2_GYR_RANGE_500 UINT8_C(0x02) -#define BMI2_GYR_RANGE_250 UINT8_C(0x03) -#define BMI2_GYR_RANGE_125 UINT8_C(0x04) +#define BMI2_GYR_RANGE_2000 UINT8_C(0x00) +#define BMI2_GYR_RANGE_1000 UINT8_C(0x01) +#define BMI2_GYR_RANGE_500 UINT8_C(0x02) +#define BMI2_GYR_RANGE_250 UINT8_C(0x03) +#define BMI2_GYR_RANGE_125 UINT8_C(0x04) /*! @name Mask definitions for gyroscope configuration register */ -#define BMI2_GYR_RANGE_MASK UINT8_C(0x07) -#define BMI2_GYR_OIS_RANGE_MASK UINT8_C(0x08) -#define BMI2_GYR_ODR_MASK UINT8_C(0x0F) -#define BMI2_GYR_BW_PARAM_MASK UINT8_C(0x30) -#define BMI2_GYR_NOISE_PERF_MODE_MASK UINT8_C(0x40) -#define BMI2_GYR_FILTER_PERF_MODE_MASK UINT8_C(0x80) +#define BMI2_GYR_RANGE_MASK UINT8_C(0x07) +#define BMI2_GYR_OIS_RANGE_MASK UINT8_C(0x08) +#define BMI2_GYR_ODR_MASK UINT8_C(0x0F) +#define BMI2_GYR_BW_PARAM_MASK UINT8_C(0x30) +#define BMI2_GYR_NOISE_PERF_MODE_MASK UINT8_C(0x40) +#define BMI2_GYR_FILTER_PERF_MODE_MASK UINT8_C(0x80) /*! @name Bit position definitions for gyroscope configuration register */ -#define BMI2_GYR_OIS_RANGE_POS UINT8_C(0x03) -#define BMI2_GYR_BW_PARAM_POS UINT8_C(0x04) -#define BMI2_GYR_NOISE_PERF_MODE_POS UINT8_C(0x06) -#define BMI2_GYR_FILTER_PERF_MODE_POS UINT8_C(0x07) +#define BMI2_GYR_OIS_RANGE_POS UINT8_C(0x03) +#define BMI2_GYR_BW_PARAM_POS UINT8_C(0x04) +#define BMI2_GYR_NOISE_PERF_MODE_POS UINT8_C(0x06) +#define BMI2_GYR_FILTER_PERF_MODE_POS UINT8_C(0x07) /******************************************************************************/ /*! @name Auxiliary Macro Definitions */ /******************************************************************************/ /*! @name Auxiliary Output Data Rate */ -#define BMI2_AUX_ODR_RESERVED UINT8_C(0x00) -#define BMI2_AUX_ODR_0_78HZ UINT8_C(0x01) -#define BMI2_AUX_ODR_1_56HZ UINT8_C(0x02) -#define BMI2_AUX_ODR_3_12HZ UINT8_C(0x03) -#define BMI2_AUX_ODR_6_25HZ UINT8_C(0x04) -#define BMI2_AUX_ODR_12_5HZ UINT8_C(0x05) -#define BMI2_AUX_ODR_25HZ UINT8_C(0x06) -#define BMI2_AUX_ODR_50HZ UINT8_C(0x07) -#define BMI2_AUX_ODR_100HZ UINT8_C(0x08) -#define BMI2_AUX_ODR_200HZ UINT8_C(0x09) -#define BMI2_AUX_ODR_400HZ UINT8_C(0x0A) -#define BMI2_AUX_ODR_800HZ UINT8_C(0x0B) +#define BMI2_AUX_ODR_RESERVED UINT8_C(0x00) +#define BMI2_AUX_ODR_0_78HZ UINT8_C(0x01) +#define BMI2_AUX_ODR_1_56HZ UINT8_C(0x02) +#define BMI2_AUX_ODR_3_12HZ UINT8_C(0x03) +#define BMI2_AUX_ODR_6_25HZ UINT8_C(0x04) +#define BMI2_AUX_ODR_12_5HZ UINT8_C(0x05) +#define BMI2_AUX_ODR_25HZ UINT8_C(0x06) +#define BMI2_AUX_ODR_50HZ UINT8_C(0x07) +#define BMI2_AUX_ODR_100HZ UINT8_C(0x08) +#define BMI2_AUX_ODR_200HZ UINT8_C(0x09) +#define BMI2_AUX_ODR_400HZ UINT8_C(0x0A) +#define BMI2_AUX_ODR_800HZ UINT8_C(0x0B) /*! @name Macro to define burst read lengths for both manual and auto modes */ -#define BMI2_AUX_READ_LEN_0 UINT8_C(0x00) -#define BMI2_AUX_READ_LEN_1 UINT8_C(0x01) -#define BMI2_AUX_READ_LEN_2 UINT8_C(0x02) -#define BMI2_AUX_READ_LEN_3 UINT8_C(0x03) +#define BMI2_AUX_READ_LEN_0 UINT8_C(0x00) +#define BMI2_AUX_READ_LEN_1 UINT8_C(0x01) +#define BMI2_AUX_READ_LEN_2 UINT8_C(0x02) +#define BMI2_AUX_READ_LEN_3 UINT8_C(0x03) /*! @name Mask definitions for auxiliary interface configuration register */ -#define BMI2_AUX_SET_I2C_ADDR_MASK UINT8_C(0xFE) -#define BMI2_AUX_MAN_MODE_EN_MASK UINT8_C(0x80) -#define BMI2_AUX_FCU_WR_EN_MASK UINT8_C(0x40) -#define BMI2_AUX_MAN_READ_BURST_MASK UINT8_C(0x0C) -#define BMI2_AUX_READ_BURST_MASK UINT8_C(0x03) -#define BMI2_AUX_ODR_EN_MASK UINT8_C(0x0F) -#define BMI2_AUX_OFFSET_READ_OUT_MASK UINT8_C(0xF0) +#define BMI2_AUX_SET_I2C_ADDR_MASK UINT8_C(0xFE) +#define BMI2_AUX_MAN_MODE_EN_MASK UINT8_C(0x80) +#define BMI2_AUX_FCU_WR_EN_MASK UINT8_C(0x40) +#define BMI2_AUX_MAN_READ_BURST_MASK UINT8_C(0x0C) +#define BMI2_AUX_READ_BURST_MASK UINT8_C(0x03) +#define BMI2_AUX_ODR_EN_MASK UINT8_C(0x0F) +#define BMI2_AUX_OFFSET_READ_OUT_MASK UINT8_C(0xF0) /*! @name Bit positions for auxiliary interface configuration register */ -#define BMI2_AUX_SET_I2C_ADDR_POS UINT8_C(0x01) -#define BMI2_AUX_MAN_MODE_EN_POS UINT8_C(0x07) -#define BMI2_AUX_FCU_WR_EN_POS UINT8_C(0x06) -#define BMI2_AUX_MAN_READ_BURST_POS UINT8_C(0x02) -#define BMI2_AUX_OFFSET_READ_OUT_POS UINT8_C(0x04) +#define BMI2_AUX_SET_I2C_ADDR_POS UINT8_C(0x01) +#define BMI2_AUX_MAN_MODE_EN_POS UINT8_C(0x07) +#define BMI2_AUX_FCU_WR_EN_POS UINT8_C(0x06) +#define BMI2_AUX_MAN_READ_BURST_POS UINT8_C(0x02) +#define BMI2_AUX_OFFSET_READ_OUT_POS UINT8_C(0x04) /******************************************************************************/ /*! @name FIFO Macro Definitions */ /******************************************************************************/ /*! @name Macros to define virtual FIFO frame mode */ -#define BMI2_FIFO_VIRT_FRM_MODE UINT8_C(0x03) +#define BMI2_FIFO_VIRT_FRM_MODE UINT8_C(0x03) /*! @name FIFO Header Mask definitions */ -#define BMI2_FIFO_HEADER_ACC_FRM UINT8_C(0x84) -#define BMI2_FIFO_HEADER_AUX_FRM UINT8_C(0x90) -#define BMI2_FIFO_HEADER_GYR_FRM UINT8_C(0x88) -#define BMI2_FIFO_HEADER_GYR_ACC_FRM UINT8_C(0x8C) -#define BMI2_FIFO_HEADER_AUX_ACC_FRM UINT8_C(0x94) -#define BMI2_FIFO_HEADER_AUX_GYR_FRM UINT8_C(0x98) -#define BMI2_FIFO_HEADER_ALL_FRM UINT8_C(0x9C) -#define BMI2_FIFO_HEADER_SENS_TIME_FRM UINT8_C(0x44) -#define BMI2_FIFO_HEADER_SKIP_FRM UINT8_C(0x40) -#define BMI2_FIFO_HEADER_INPUT_CFG_FRM UINT8_C(0x48) -#define BMI2_FIFO_HEAD_OVER_READ_MSB UINT8_C(0x80) -#define BMI2_FIFO_VIRT_ACT_RECOG_FRM UINT8_C(0xC8) +#define BMI2_FIFO_HEADER_ACC_FRM UINT8_C(0x84) +#define BMI2_FIFO_HEADER_AUX_FRM UINT8_C(0x90) +#define BMI2_FIFO_HEADER_GYR_FRM UINT8_C(0x88) +#define BMI2_FIFO_HEADER_GYR_ACC_FRM UINT8_C(0x8C) +#define BMI2_FIFO_HEADER_AUX_ACC_FRM UINT8_C(0x94) +#define BMI2_FIFO_HEADER_AUX_GYR_FRM UINT8_C(0x98) +#define BMI2_FIFO_HEADER_ALL_FRM UINT8_C(0x9C) +#define BMI2_FIFO_HEADER_SENS_TIME_FRM UINT8_C(0x44) +#define BMI2_FIFO_HEADER_SKIP_FRM UINT8_C(0x40) +#define BMI2_FIFO_HEADER_INPUT_CFG_FRM UINT8_C(0x48) +#define BMI2_FIFO_HEAD_OVER_READ_MSB UINT8_C(0x80) +#define BMI2_FIFO_VIRT_ACT_RECOG_FRM UINT8_C(0xC8) /*! @name BMI2 sensor selection for header-less frames */ -#define BMI2_FIFO_HEAD_LESS_ACC_FRM UINT8_C(0x40) -#define BMI2_FIFO_HEAD_LESS_AUX_FRM UINT8_C(0x20) -#define BMI2_FIFO_HEAD_LESS_GYR_FRM UINT8_C(0x80) -#define BMI2_FIFO_HEAD_LESS_GYR_AUX_FRM UINT8_C(0xA0) -#define BMI2_FIFO_HEAD_LESS_GYR_ACC_FRM UINT8_C(0xC0) -#define BMI2_FIFO_HEAD_LESS_AUX_ACC_FRM UINT8_C(0x60) -#define BMI2_FIFO_HEAD_LESS_ALL_FRM UINT8_C(0xE0) +#define BMI2_FIFO_HEAD_LESS_ACC_FRM UINT8_C(0x40) +#define BMI2_FIFO_HEAD_LESS_AUX_FRM UINT8_C(0x20) +#define BMI2_FIFO_HEAD_LESS_GYR_FRM UINT8_C(0x80) +#define BMI2_FIFO_HEAD_LESS_GYR_AUX_FRM UINT8_C(0xA0) +#define BMI2_FIFO_HEAD_LESS_GYR_ACC_FRM UINT8_C(0xC0) +#define BMI2_FIFO_HEAD_LESS_AUX_ACC_FRM UINT8_C(0x60) +#define BMI2_FIFO_HEAD_LESS_ALL_FRM UINT8_C(0xE0) /*! @name Mask definitions for FIFO frame content configuration */ -#define BMI2_FIFO_STOP_ON_FULL UINT16_C(0x0001) -#define BMI2_FIFO_TIME_EN UINT16_C(0x0002) -#define BMI2_FIFO_TAG_INT1 UINT16_C(0x0300) -#define BMI2_FIFO_TAG_INT2 UINT16_C(0x0C00) -#define BMI2_FIFO_HEADER_EN UINT16_C(0x1000) -#define BMI2_FIFO_AUX_EN UINT16_C(0x2000) -#define BMI2_FIFO_ACC_EN UINT16_C(0x4000) -#define BMI2_FIFO_GYR_EN UINT16_C(0x8000) -#define BMI2_FIFO_ALL_EN UINT16_C(0xE000) +#define BMI2_FIFO_STOP_ON_FULL UINT16_C(0x0001) +#define BMI2_FIFO_TIME_EN UINT16_C(0x0002) +#define BMI2_FIFO_TAG_INT1 UINT16_C(0x0300) +#define BMI2_FIFO_TAG_INT2 UINT16_C(0x0C00) +#define BMI2_FIFO_HEADER_EN UINT16_C(0x1000) +#define BMI2_FIFO_AUX_EN UINT16_C(0x2000) +#define BMI2_FIFO_ACC_EN UINT16_C(0x4000) +#define BMI2_FIFO_GYR_EN UINT16_C(0x8000) +#define BMI2_FIFO_ALL_EN UINT16_C(0xE000) /*! @name FIFO sensor data lengths */ -#define BMI2_FIFO_ACC_LENGTH UINT8_C(6) -#define BMI2_FIFO_GYR_LENGTH UINT8_C(6) -#define BMI2_FIFO_AUX_LENGTH UINT8_C(8) -#define BMI2_FIFO_ACC_AUX_LENGTH UINT8_C(14) -#define BMI2_FIFO_GYR_AUX_LENGTH UINT8_C(14) -#define BMI2_FIFO_ACC_GYR_LENGTH UINT8_C(12) -#define BMI2_FIFO_ALL_LENGTH UINT8_C(20) -#define BMI2_SENSOR_TIME_LENGTH UINT8_C(3) -#define BMI2_FIFO_CONFIG_LENGTH UINT8_C(2) -#define BMI2_FIFO_WM_LENGTH UINT8_C(2) -#define BMI2_MAX_VALUE_FIFO_FILTER UINT8_C(1) -#define BMI2_FIFO_DATA_LENGTH UINT8_C(2) -#define BMI2_FIFO_LENGTH_MSB_BYTE UINT8_C(1) -#define BMI2_FIFO_INPUT_CFG_LENGTH UINT8_C(4) -#define BMI2_FIFO_SKIP_FRM_LENGTH UINT8_C(1) +#define BMI2_FIFO_ACC_LENGTH UINT8_C(6) +#define BMI2_FIFO_GYR_LENGTH UINT8_C(6) +#define BMI2_FIFO_AUX_LENGTH UINT8_C(8) +#define BMI2_FIFO_ACC_AUX_LENGTH UINT8_C(14) +#define BMI2_FIFO_GYR_AUX_LENGTH UINT8_C(14) +#define BMI2_FIFO_ACC_GYR_LENGTH UINT8_C(12) +#define BMI2_FIFO_ALL_LENGTH UINT8_C(20) +#define BMI2_SENSOR_TIME_LENGTH UINT8_C(3) +#define BMI2_FIFO_CONFIG_LENGTH UINT8_C(2) +#define BMI2_FIFO_WM_LENGTH UINT8_C(2) +#define BMI2_MAX_VALUE_FIFO_FILTER UINT8_C(1) +#define BMI2_FIFO_DATA_LENGTH UINT8_C(2) +#define BMI2_FIFO_LENGTH_MSB_BYTE UINT8_C(1) +#define BMI2_FIFO_INPUT_CFG_LENGTH UINT8_C(4) +#define BMI2_FIFO_SKIP_FRM_LENGTH UINT8_C(1) /*! @name FIFO sensor virtual data lengths: sensor data plus sensor time */ -#define BMI2_FIFO_VIRT_ACC_LENGTH UINT8_C(9) -#define BMI2_FIFO_VIRT_GYR_LENGTH UINT8_C(9) -#define BMI2_FIFO_VIRT_AUX_LENGTH UINT8_C(11) -#define BMI2_FIFO_VIRT_ACC_AUX_LENGTH UINT8_C(17) -#define BMI2_FIFO_VIRT_GYR_AUX_LENGTH UINT8_C(17) -#define BMI2_FIFO_VIRT_ACC_GYR_LENGTH UINT8_C(15) -#define BMI2_FIFO_VIRT_ALL_LENGTH UINT8_C(23) +#define BMI2_FIFO_VIRT_ACC_LENGTH UINT8_C(9) +#define BMI2_FIFO_VIRT_GYR_LENGTH UINT8_C(9) +#define BMI2_FIFO_VIRT_AUX_LENGTH UINT8_C(11) +#define BMI2_FIFO_VIRT_ACC_AUX_LENGTH UINT8_C(17) +#define BMI2_FIFO_VIRT_GYR_AUX_LENGTH UINT8_C(17) +#define BMI2_FIFO_VIRT_ACC_GYR_LENGTH UINT8_C(15) +#define BMI2_FIFO_VIRT_ALL_LENGTH UINT8_C(23) /*! @name FIFO sensor virtual data lengths: activity recognition */ -#define BMI2_FIFO_VIRT_ACT_DATA_LENGTH UINT8_C(6) -#define BMI2_FIFO_VIRT_ACT_TIME_LENGTH UINT8_C(4) -#define BMI2_FIFO_VIRT_ACT_TYPE_LENGTH UINT8_C(1) -#define BMI2_FIFO_VIRT_ACT_STAT_LENGTH UINT8_C(1) +#define BMI2_FIFO_VIRT_ACT_DATA_LENGTH UINT8_C(6) +#define BMI2_FIFO_VIRT_ACT_TIME_LENGTH UINT8_C(4) +#define BMI2_FIFO_VIRT_ACT_TYPE_LENGTH UINT8_C(1) +#define BMI2_FIFO_VIRT_ACT_STAT_LENGTH UINT8_C(1) /*! @name BMI2 FIFO data filter modes */ -#define BMI2_FIFO_UNFILTERED_DATA UINT8_C(0) -#define BMI2_FIFO_FILTERED_DATA UINT8_C(1) +#define BMI2_FIFO_UNFILTERED_DATA UINT8_C(0) +#define BMI2_FIFO_FILTERED_DATA UINT8_C(1) /*! @name FIFO frame masks */ -#define BMI2_FIFO_LSB_CONFIG_CHECK UINT8_C(0x00) -#define BMI2_FIFO_MSB_CONFIG_CHECK UINT8_C(0x80) -#define BMI2_FIFO_TAG_INTR_MASK UINT8_C(0xFF) +#define BMI2_FIFO_LSB_CONFIG_CHECK UINT8_C(0x00) +#define BMI2_FIFO_MSB_CONFIG_CHECK UINT8_C(0x80) +#define BMI2_FIFO_TAG_INTR_MASK UINT8_C(0xFF) /*! @name BMI2 Mask definitions of FIFO configuration registers */ -#define BMI2_FIFO_CONFIG_0_MASK UINT16_C(0x0003) -#define BMI2_FIFO_CONFIG_1_MASK UINT16_C(0xFF00) +#define BMI2_FIFO_CONFIG_0_MASK UINT16_C(0x0003) +#define BMI2_FIFO_CONFIG_1_MASK UINT16_C(0xFF00) /*! @name FIFO self wake-up mask definition */ -#define BMI2_FIFO_SELF_WAKE_UP_MASK UINT8_C(0x02) +#define BMI2_FIFO_SELF_WAKE_UP_MASK UINT8_C(0x02) /*! @name FIFO down sampling mask definition */ -#define BMI2_ACC_FIFO_DOWNS_MASK UINT8_C(0x70) -#define BMI2_GYR_FIFO_DOWNS_MASK UINT8_C(0x07) +#define BMI2_ACC_FIFO_DOWNS_MASK UINT8_C(0x70) +#define BMI2_GYR_FIFO_DOWNS_MASK UINT8_C(0x07) /*! @name FIFO down sampling bit positions */ -#define BMI2_ACC_FIFO_DOWNS_POS UINT8_C(0x04) +#define BMI2_ACC_FIFO_DOWNS_POS UINT8_C(0x04) /*! @name FIFO filter mask definition */ -#define BMI2_ACC_FIFO_FILT_DATA_MASK UINT8_C(0x80) -#define BMI2_GYR_FIFO_FILT_DATA_MASK UINT8_C(0x08) +#define BMI2_ACC_FIFO_FILT_DATA_MASK UINT8_C(0x80) +#define BMI2_GYR_FIFO_FILT_DATA_MASK UINT8_C(0x08) /*! @name FIFO filter bit positions */ -#define BMI2_ACC_FIFO_FILT_DATA_POS UINT8_C(0x07) -#define BMI2_GYR_FIFO_FILT_DATA_POS UINT8_C(0x03) +#define BMI2_ACC_FIFO_FILT_DATA_POS UINT8_C(0x07) +#define BMI2_GYR_FIFO_FILT_DATA_POS UINT8_C(0x03) /*! @name FIFO byte counter mask definition */ -#define BMI2_FIFO_BYTE_COUNTER_MSB_MASK UINT8_C(0x3F) +#define BMI2_FIFO_BYTE_COUNTER_MSB_MASK UINT8_C(0x3F) /*! @name FIFO self wake-up bit positions */ -#define BMI2_FIFO_SELF_WAKE_UP_POS UINT8_C(0x01) +#define BMI2_FIFO_SELF_WAKE_UP_POS UINT8_C(0x01) /*! @name Mask Definitions for Virtual FIFO frames */ -#define BMI2_FIFO_VIRT_FRM_MODE_MASK UINT8_C(0xC0) -#define BMI2_FIFO_VIRT_PAYLOAD_MASK UINT8_C(0x3C) +#define BMI2_FIFO_VIRT_FRM_MODE_MASK UINT8_C(0xC0) +#define BMI2_FIFO_VIRT_PAYLOAD_MASK UINT8_C(0x3C) /*! @name Bit Positions for Virtual FIFO frames */ -#define BMI2_FIFO_VIRT_FRM_MODE_POS UINT8_C(0x06) -#define BMI2_FIFO_VIRT_PAYLOAD_POS UINT8_C(0x02) +#define BMI2_FIFO_VIRT_FRM_MODE_POS UINT8_C(0x06) +#define BMI2_FIFO_VIRT_PAYLOAD_POS UINT8_C(0x02) /******************************************************************************/ /*! @name Interrupt Macro Definitions */ /******************************************************************************/ /*! @name BMI2 Interrupt Modes */ /* Non latched */ -#define BMI2_INT_NON_LATCH UINT8_C(0) +#define BMI2_INT_NON_LATCH UINT8_C(0) /* Permanently latched */ -#define BMI2_INT_LATCH UINT8_C(1) +#define BMI2_INT_LATCH UINT8_C(1) /*! @name BMI2 Interrupt Pin Behavior */ -#define BMI2_INT_PUSH_PULL UINT8_C(0) -#define BMI2_INT_OPEN_DRAIN UINT8_C(1) +#define BMI2_INT_PUSH_PULL UINT8_C(0) +#define BMI2_INT_OPEN_DRAIN UINT8_C(1) /*! @name BMI2 Interrupt Pin Level */ -#define BMI2_INT_ACTIVE_LOW UINT8_C(0) -#define BMI2_INT_ACTIVE_HIGH UINT8_C(1) +#define BMI2_INT_ACTIVE_LOW UINT8_C(0) +#define BMI2_INT_ACTIVE_HIGH UINT8_C(1) /*! @name BMI2 Interrupt Output Enable */ -#define BMI2_INT_OUTPUT_DISABLE UINT8_C(0) -#define BMI2_INT_OUTPUT_ENABLE UINT8_C(1) +#define BMI2_INT_OUTPUT_DISABLE UINT8_C(0) +#define BMI2_INT_OUTPUT_ENABLE UINT8_C(1) /*! @name BMI2 Interrupt Input Enable */ -#define BMI2_INT_INPUT_DISABLE UINT8_C(0) -#define BMI2_INT_INPUT_ENABLE UINT8_C(1) +#define BMI2_INT_INPUT_DISABLE UINT8_C(0) +#define BMI2_INT_INPUT_ENABLE UINT8_C(1) /*! @name Mask definitions for interrupt pin configuration */ -#define BMI2_INT_LATCH_MASK UINT8_C(0x01) -#define BMI2_INT_LEVEL_MASK UINT8_C(0x02) -#define BMI2_INT_OPEN_DRAIN_MASK UINT8_C(0x04) -#define BMI2_INT_OUTPUT_EN_MASK UINT8_C(0x08) -#define BMI2_INT_INPUT_EN_MASK UINT8_C(0x10) +#define BMI2_INT_LATCH_MASK UINT8_C(0x01) +#define BMI2_INT_LEVEL_MASK UINT8_C(0x02) +#define BMI2_INT_OPEN_DRAIN_MASK UINT8_C(0x04) +#define BMI2_INT_OUTPUT_EN_MASK UINT8_C(0x08) +#define BMI2_INT_INPUT_EN_MASK UINT8_C(0x10) /*! @name Bit position definitions for interrupt pin configuration */ -#define BMI2_INT_LEVEL_POS UINT8_C(0x01) -#define BMI2_INT_OPEN_DRAIN_POS UINT8_C(0x02) -#define BMI2_INT_OUTPUT_EN_POS UINT8_C(0x03) -#define BMI2_INT_INPUT_EN_POS UINT8_C(0x04) +#define BMI2_INT_LEVEL_POS UINT8_C(0x01) +#define BMI2_INT_OPEN_DRAIN_POS UINT8_C(0x02) +#define BMI2_INT_OUTPUT_EN_POS UINT8_C(0x03) +#define BMI2_INT_INPUT_EN_POS UINT8_C(0x04) /*! @name Mask definitions for data interrupt mapping */ -#define BMI2_FFULL_INT UINT8_C(0x01) -#define BMI2_FWM_INT UINT8_C(0x02) -#define BMI2_DRDY_INT UINT8_C(0x04) -#define BMI2_ERR_INT UINT8_C(0x08) +#define BMI2_FFULL_INT UINT8_C(0x01) +#define BMI2_FWM_INT UINT8_C(0x02) +#define BMI2_DRDY_INT UINT8_C(0x04) +#define BMI2_ERR_INT UINT8_C(0x08) /*! @name Mask definitions for data interrupt status bits */ -#define BMI2_FFULL_INT_STATUS_MASK UINT16_C(0x0100) -#define BMI2_FWM_INT_STATUS_MASK UINT16_C(0x0200) -#define BMI2_ERR_INT_STATUS_MASK UINT16_C(0x0400) -#define BMI2_AUX_DRDY_INT_MASK UINT16_C(0x2000) -#define BMI2_GYR_DRDY_INT_MASK UINT16_C(0x4000) -#define BMI2_ACC_DRDY_INT_MASK UINT16_C(0x8000) +#define BMI2_FFULL_INT_STATUS_MASK UINT16_C(0x0100) +#define BMI2_FWM_INT_STATUS_MASK UINT16_C(0x0200) +#define BMI2_ERR_INT_STATUS_MASK UINT16_C(0x0400) +#define BMI2_AUX_DRDY_INT_MASK UINT16_C(0x2000) +#define BMI2_GYR_DRDY_INT_MASK UINT16_C(0x4000) +#define BMI2_ACC_DRDY_INT_MASK UINT16_C(0x8000) /*! @name Maximum number of interrupt pins */ -#define BMI2_INT_PIN_MAX_NUM UINT8_C(2) +#define BMI2_INT_PIN_MAX_NUM UINT8_C(2) /*! @name Macro for mapping feature interrupts */ -#define BMI2_FEAT_BIT_DISABLE UINT8_C(0) -#define BMI2_FEAT_BIT0 UINT8_C(1) -#define BMI2_FEAT_BIT1 UINT8_C(2) -#define BMI2_FEAT_BIT2 UINT8_C(3) -#define BMI2_FEAT_BIT3 UINT8_C(4) -#define BMI2_FEAT_BIT4 UINT8_C(5) -#define BMI2_FEAT_BIT5 UINT8_C(6) -#define BMI2_FEAT_BIT6 UINT8_C(7) -#define BMI2_FEAT_BIT7 UINT8_C(8) -#define BMI2_FEAT_BIT_MAX UINT8_C(9) +#define BMI2_FEAT_BIT_DISABLE UINT8_C(0) +#define BMI2_FEAT_BIT0 UINT8_C(1) +#define BMI2_FEAT_BIT1 UINT8_C(2) +#define BMI2_FEAT_BIT2 UINT8_C(3) +#define BMI2_FEAT_BIT3 UINT8_C(4) +#define BMI2_FEAT_BIT4 UINT8_C(5) +#define BMI2_FEAT_BIT5 UINT8_C(6) +#define BMI2_FEAT_BIT6 UINT8_C(7) +#define BMI2_FEAT_BIT7 UINT8_C(8) +#define BMI2_FEAT_BIT_MAX UINT8_C(9) /******************************************************************************/ /*! @name OIS Interface Macro Definitions */ /******************************************************************************/ /*! @name Mask definitions for interface configuration register */ -#define BMI2_OIS_IF_EN_MASK UINT8_C(0x10) -#define BMI2_AUX_IF_EN_MASK UINT8_C(0x20) +#define BMI2_OIS_IF_EN_MASK UINT8_C(0x10) +#define BMI2_AUX_IF_EN_MASK UINT8_C(0x20) /*! @name Bit positions for OIS interface enable */ -#define BMI2_OIS_IF_EN_POS UINT8_C(0x04) -#define BMI2_AUX_IF_EN_POS UINT8_C(0x05) +#define BMI2_OIS_IF_EN_POS UINT8_C(0x04) +#define BMI2_AUX_IF_EN_POS UINT8_C(0x05) /******************************************************************************/ /*! @name Macro Definitions for Axes re-mapping */ /******************************************************************************/ /*! @name Macros for the user-defined values of axes and their polarities */ -#define BMI2_X UINT8_C(0x01) -#define BMI2_NEG_X UINT8_C(0x09) -#define BMI2_Y UINT8_C(0x02) -#define BMI2_NEG_Y UINT8_C(0x0A) -#define BMI2_Z UINT8_C(0x04) -#define BMI2_NEG_Z UINT8_C(0x0C) -#define BMI2_AXIS_MASK UINT8_C(0x07) -#define BMI2_AXIS_SIGN UINT8_C(0x08) +#define BMI2_X UINT8_C(0x01) +#define BMI2_NEG_X UINT8_C(0x09) +#define BMI2_Y UINT8_C(0x02) +#define BMI2_NEG_Y UINT8_C(0x0A) +#define BMI2_Z UINT8_C(0x04) +#define BMI2_NEG_Z UINT8_C(0x0C) +#define BMI2_AXIS_MASK UINT8_C(0x07) +#define BMI2_AXIS_SIGN UINT8_C(0x08) /******************************************************************************/ /*! @name Macro Definitions for offset and gain compensation */ /******************************************************************************/ /*! @name Mask definitions of gyroscope offset compensation registers */ -#define BMI2_GYR_GAIN_EN_MASK UINT8_C(0x80) -#define BMI2_GYR_OFF_COMP_EN_MASK UINT8_C(0x40) +#define BMI2_GYR_GAIN_EN_MASK UINT8_C(0x80) +#define BMI2_GYR_OFF_COMP_EN_MASK UINT8_C(0x40) /*! @name Bit positions of gyroscope offset compensation registers */ -#define BMI2_GYR_OFF_COMP_EN_POS UINT8_C(0x06) +#define BMI2_GYR_OFF_COMP_EN_POS UINT8_C(0x06) /*! @name Mask definitions of gyroscope user-gain registers */ -#define BMI2_GYR_USR_GAIN_X_MASK UINT8_C(0x7F) -#define BMI2_GYR_USR_GAIN_Y_MASK UINT8_C(0x7F) -#define BMI2_GYR_USR_GAIN_Z_MASK UINT8_C(0x7F) +#define BMI2_GYR_USR_GAIN_X_MASK UINT8_C(0x7F) +#define BMI2_GYR_USR_GAIN_Y_MASK UINT8_C(0x7F) +#define BMI2_GYR_USR_GAIN_Z_MASK UINT8_C(0x7F) /*! @name Bit positions of gyroscope offset compensation registers */ -#define BMI2_GYR_GAIN_EN_POS UINT8_C(0x07) +#define BMI2_GYR_GAIN_EN_POS UINT8_C(0x07) /******************************************************************************/ /*! @name Macro Definitions for internal status */ /******************************************************************************/ -#define BMI2_NOT_INIT UINT8_C(0x00) -#define BMI2_INIT_OK UINT8_C(0x01) -#define BMI2_INIT_ERR UINT8_C(0x02) -#define BMI2_DRV_ERR UINT8_C(0x03) -#define BMI2_SNS_STOP UINT8_C(0x04) -#define BMI2_NVM_ERROR UINT8_C(0x05) -#define BMI2_START_UP_ERROR UINT8_C(0x06) -#define BMI2_COMPAT_ERROR UINT8_C(0x07) -#define BMI2_VFM_SKIPPED UINT8_C(0x10) -#define BMI2_AXES_MAP_ERROR UINT8_C(0x20) -#define BMI2_ODR_50_HZ_ERROR UINT8_C(0x40) -#define BMI2_ODR_HIGH_ERROR UINT8_C(0x80) +#define BMI2_NOT_INIT UINT8_C(0x00) +#define BMI2_INIT_OK UINT8_C(0x01) +#define BMI2_INIT_ERR UINT8_C(0x02) +#define BMI2_DRV_ERR UINT8_C(0x03) +#define BMI2_SNS_STOP UINT8_C(0x04) +#define BMI2_NVM_ERROR UINT8_C(0x05) +#define BMI2_START_UP_ERROR UINT8_C(0x06) +#define BMI2_COMPAT_ERROR UINT8_C(0x07) +#define BMI2_VFM_SKIPPED UINT8_C(0x10) +#define BMI2_AXES_MAP_ERROR UINT8_C(0x20) +#define BMI2_ODR_50_HZ_ERROR UINT8_C(0x40) +#define BMI2_ODR_HIGH_ERROR UINT8_C(0x80) /******************************************************************************/ /*! @name error status form gyro gain update status. */ /******************************************************************************/ -#define BMI2_G_TRIGGER_NO_ERROR UINT8_C(0x00) +#define BMI2_G_TRIGGER_NO_ERROR UINT8_C(0x00) -#define BMI2_G_TRIGGER_PRECON_ERROR UINT8_C(0x01) -#define BMI2_G_TRIGGER_DL_ERROR UINT8_C(0x02) -#define BMI2_G_TRIGGER_ABORT_ERROR UINT8_C(0x03) +#define BMI2_G_TRIGGER_PRECON_ERROR UINT8_C(0x01) +#define BMI2_G_TRIGGER_DL_ERROR UINT8_C(0x02) +#define BMI2_G_TRIGGER_ABORT_ERROR UINT8_C(0x03) /******************************************************************************/ /*! @name Variant specific features selection macros */ /******************************************************************************/ -#define BMI2_CRT_RTOSK_ENABLE UINT8_C(0x01) -#define BMI2_GYRO_CROSS_SENS_ENABLE UINT8_C(0x02) -#define BMI_GYRO_USER_GAIN_ENABLE UINT8_C(0x08) -#define BMI2_NO_FEATURE_ENABLE UINT8_C(0x00) -#define BMI2_CRT_IN_FIFO_NOT_REQ UINT8_C(0x10) -#define BMI2_MINIMAL_VARIANT UINT8_C(0x20) +#define BMI2_CRT_RTOSK_ENABLE UINT8_C(0x01) +#define BMI2_GYRO_CROSS_SENS_ENABLE UINT8_C(0x02) +#define BMI2_GYRO_USER_GAIN_ENABLE UINT8_C(0x08) +#define BMI2_NO_FEATURE_ENABLE UINT8_C(0x00) +#define BMI2_CRT_IN_FIFO_NOT_REQ UINT8_C(0x10) +#define BMI2_MAXIMUM_FIFO_VARIANT UINT8_C(0x20) + +/*! Pull-up configuration for ASDA */ +#define BMI2_ASDA_PUPSEL_OFF UINT8_C(0x00) +#define BMI2_ASDA_PUPSEL_40K UINT8_C(0x01) +#define BMI2_ASDA_PUPSEL_10K UINT8_C(0x02) +#define BMI2_ASDA_PUPSEL_2K UINT8_C(0x03) /******************************************************************************/ /*! @name Function Pointers */ /******************************************************************************/ -/*! For interfacing to the I2C or SPI read functions */ -typedef int8_t (*bmi2_read_fptr_t)(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len); -/*! For interfacing to the I2C or SPI write functions */ -typedef int8_t (*bmi2_write_fptr_t)(uint8_t dev_addr, uint8_t reg_addr, const uint8_t *data, uint16_t len); +/*! + * @brief Bus communication function pointer which should be mapped to + * the platform specific read functions of the user + * + * @param[in] reg_addr : Register address from which data is read. + * @param[out] reg_data : Pointer to data buffer where read data is stored. + * @param[in] len : Number of bytes of data to be read. + * @param[in, out] intf_ptr : Void pointer that can enable the linking of descriptors + * for interface related call backs. + * + * retval = BMA4_INTF_RET_SUCCESS -> Success + * retval != BMA4_INTF_RET_SUCCESS -> Failure + * + */ +typedef BMI2_INTF_RETURN_TYPE (*bmi2_read_fptr_t)(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr); -/*! For interfacing to the delay function */ -typedef void (*bmi2_delay_fptr_t)(uint32_t period); +/*! + * @brief Bus communication function pointer which should be mapped to + * the platform specific write functions of the user + * + * @param[in] reg_addr : Register address to which the data is written. + * @param[in] reg_data : Pointer to data buffer in which data to be written + * is stored. + * @param[in] len : Number of bytes of data to be written. + * @param[in, out] intf_ptr : Void pointer that can enable the linking of descriptors + * for interface related call backs + * + * retval = BMA4_INTF_RET_SUCCESS -> Success + * retval != BMA4_INTF_RET_SUCCESS -> Failure + * + */ +typedef BMI2_INTF_RETURN_TYPE (*bmi2_write_fptr_t)(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, + void *intf_ptr); + +/*! + * @brief Delay function pointer which should be mapped to + * delay function of the user + * + * @param[in] period : Delay in microseconds. + * @param[in, out] intf_ptr : Void pointer that can enable the linking of descriptors + * for interface related call backs + * + */ +typedef void (*bmi2_delay_fptr_t)(uint32_t period, void *intf_ptr); + +/*! + * @brief To get the configurations for wake_up feature, since wakeup feature is different for bmi260 and bmi261. + * + * @param[out] wake_up : Void pointer to store bmi2_wake_up_config structure. + * @param[in, out] bmi2_dev : Void pointer to store bmi2_dev structure. + * + * @return Result of API execution status + * + * @retval BMI2_OK - Success. + * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval BMI2_E_NULL_PTR - Error: Null pointer error + * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page + * + */ +typedef int8_t (*bmi2_wake_up_fptr_t)(void *wake_up, void *bmi2_dev); + +/*! + * @brief To get the configurations for tap feature. + * + * @param[out] tap : Void pointer to store bmi2_tap_config structure. + * @param[in, out] bmi2_dev : Void pointer to store bmi2_dev structure. + * + * @return Result of API execution status + * + * @retval BMI2_OK - Success. + * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval BMI2_E_NULL_PTR - Error: Null pointer error + * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page + * + */ +typedef int8_t (*bmi2_tap_fptr_t)(void *tap, void *bmi2_dev); /******************************************************************************/ /*! @name Enum Declarations */ /******************************************************************************/ /*! @name Enum to define BMI2 sensor interfaces */ -enum bmi2_intf_type { - BMI2_SPI_INTERFACE = 1, - BMI2_I2C_INTERFACE, - BMI2_I3C_INTERFACE +enum bmi2_intf { + BMI2_SPI_INTF = 0, + BMI2_I2C_INTF, + BMI2_I3C_INTF }; /*! @name Enum to define BMI2 sensor configuration errors for accelerometer @@ -994,13 +1434,13 @@ struct bmi2_axes_remap uint8_t z_axis; /*! Re-mapped x-axis sign */ - int16_t x_axis_sign; + uint8_t x_axis_sign; /*! Re-mapped y-axis sign */ - int16_t y_axis_sign; + uint8_t y_axis_sign; /*! Re-mapped z-axis sign */ - int16_t z_axis_sign; + uint8_t z_axis_sign; }; /*! @name Structure to define the type of sensor and its interrupt pin */ @@ -1013,61 +1453,6 @@ struct bmi2_sens_int_config enum bmi2_hw_int_pin hw_int_pin; }; -/*! @name Structure to define the output configuration value of features */ -struct bmi2_int_map -{ - /*! Output configuration value of sig-motion */ - uint8_t sig_mot_out_conf; - - /*! Output configuration value of any-motion */ - uint8_t any_mot_out_conf; - - /*! Output configuration value of no-motion */ - uint8_t no_mot_out_conf; - - /*! Output configuration value of step-detector */ - uint8_t step_det_out_conf; - - /*! Output configuration value of step-activity */ - uint8_t step_act_out_conf; - - /*! Output configuration value of tilt */ - uint8_t tilt_out_conf; - - /*! Output configuration value of uphold to wake */ - uint8_t up_hold_to_wake_out_conf; - - /*! Output configuration value of glance */ - uint8_t glance_out_conf; - - /*! Output configuration value of wake-up */ - uint8_t wake_up_out_conf; - - /*! Output configuration value of orientation */ - uint8_t orient_out_conf; - - /*! Output configuration value of high-g */ - uint8_t high_g_out_conf; - - /*! Output configuration value of low-g */ - uint8_t low_g_out_conf; - - /*! Output configuration value of flat */ - uint8_t flat_out_conf; - - /*! Output configuration value of S4S */ - uint8_t ext_sync_out_conf; - - /*! Output configuration value of wrist gesture */ - uint8_t wrist_gest_out_conf; - - /*! Output configuration value of wrist wear wake-up */ - uint8_t wrist_wear_wake_up_out_conf; - - /*! Output configuration value of free-fall detection */ - uint8_t freefall_out_conf; -}; - /*! @name Structure to define output for activity recognition */ struct bmi2_act_recog_output { @@ -1455,9 +1840,6 @@ struct bmi2_any_motion_config /*! To select per z-axis */ uint16_t select_z; - - /*! Enable bits for enabling output into the register status bits */ - uint16_t out_conf; }; /*! @name Structure to define no-motion configuration */ @@ -1477,9 +1859,6 @@ struct bmi2_no_motion_config /*! To select per z-axis */ uint16_t select_z; - - /*! Enable bits for enabling output into the register status bits */ - uint16_t out_conf; }; /*! @name Structure to define sig-motion configuration */ @@ -1499,9 +1878,6 @@ struct bmi2_sig_motion_config /*! Parameter 5 */ uint16_t param_5; - - /*! Enable bits for enabling output into the register status bits */ - uint16_t out_conf; }; /*! @name Structure to define step counter/detector/activity configuration */ @@ -1513,15 +1889,7 @@ struct bmi2_step_config /*! Reset counter */ uint16_t reset_counter; - /*! Enable bits for enabling output into the register status bits - * for step-detector - */ - uint16_t out_conf_step_detector; - - /*! Enable bits for enabling output into the register status bits - * for step-activity - */ - uint16_t out_conf_activity; + /*! Step buffer size */ uint8_t step_buffer_size; }; @@ -1538,38 +1906,66 @@ struct bmi2_gyro_user_gain_config uint16_t ratio_z; }; -/*! @name Structure to define tilt configuration */ -struct bmi2_tilt_config -{ - /*! Enable bits for enabling output into the register status bits */ - uint16_t out_conf; -}; - -/*! @name Structure to define uphold to wake configuration */ -struct bmi2_up_hold_to_wake_config -{ - /*! Enable bits for enabling output into the register status bits */ - uint16_t out_conf; -}; - -/*! @name Structure to define glance detector configuration */ -struct bmi2_glance_det_config -{ - /*! Enable bits for enabling output into the register status bits */ - uint16_t out_conf; -}; - /*! @name Structure to define wake-up configuration */ struct bmi2_wake_up_config { - /*! Wake-up sensitivity */ + /*! Wake-up sensitivity for bmi261 */ uint16_t sensitivity; - /*! Enable -> Single Tap; Disable -> Double Tap */ + /*! Tap feature for BMI261 + * For Single tap, single_tap_en = 1 + * For Double tap, single_tap_en = 0 + */ uint16_t single_tap_en; - /*! Enable bits for enabling output into the register status bits */ - uint16_t out_conf; + /*! Enable -> Filtered tap data, Disable -> Unfiltered data */ + uint16_t data_reg_en; + + /*! Scaling factor of threshold */ + uint16_t tap_sens_thres; + + /*! Maximum duration between each taps */ + uint16_t max_gest_dur; + + /*! Minimum quite time between the two gesture detection */ + uint16_t quite_time_after_gest; + + /*! Wait time */ + uint16_t wait_for_timeout; + + /*! Axis selection */ + uint16_t axis_sel; +}; + +/*! @name Structure to define tap configuration */ +struct bmi2_tap_config +{ + /*! Tap sensitivity */ + uint16_t sensitivity; + + /*! Tap feature. + * For Single tap, single_tap_en = 1 + * For Double tap, single_tap_en = 0 + */ + uint16_t single_tap_en; + + /*! Enable -> Filtered tap data, Disable -> Unfiltered data */ + uint16_t data_reg_en; + + /*! Scaling factor of threshold */ + uint16_t tap_sens_thres; + + /*! Maximum duration between each taps */ + uint16_t max_gest_dur; + + /*! Minimum quite time between the two gesture detection */ + uint16_t quite_time_after_gest; + + /*! Wait time */ + uint16_t wait_for_timeout; + + /*! Axis selection */ + uint16_t axis_sel; }; /*! @name Structure to define orientation configuration */ @@ -1589,9 +1985,6 @@ struct bmi2_orient_config /*! Acceleration hysteresis for orientation detection */ uint16_t hysteresis; - - /*! Enable bits for enabling output into the register status bits */ - uint16_t out_conf; }; /*! @name Structure to define high-g configuration */ @@ -1614,9 +2007,6 @@ struct bmi2_high_g_config /*! Duration interval */ uint16_t duration; - - /*! Enable bits for enabling output into the register status bits */ - uint16_t out_conf; }; /*! @name Structure to define low-g configuration */ @@ -1630,10 +2020,6 @@ struct bmi2_low_g_config /*! Duration interval */ uint16_t duration; - - /*! Enable bits for enabling output into the register status bits */ - uint16_t out_conf; - }; /*! @name Structure to define flat configuration */ @@ -1650,16 +2036,6 @@ struct bmi2_flat_config /*! Holds the duration in 50Hz samples(20msec) */ uint16_t hold_time; - - /*! Enable bits for enabling output into the register status bits */ - uint16_t out_conf; -}; - -/*! @name Structure to define external sensor sync configuration */ -struct bmi2_ext_sens_sync_config -{ - /*! Enable bits for enabling output into the register status bits */ - uint16_t out_conf; }; /*! @name Structure to define wrist gesture configuration */ @@ -1668,9 +2044,6 @@ struct bmi2_wrist_gest_config /*! Wearable arm (left or right) */ uint16_t wearable_arm; - /*! Enable bits for enabling output into the register status bits */ - uint16_t out_conf; - /*! Sine of the minimum tilt angle in portrait down direction of the device when wrist is rolled * away from user. The configuration parameter is scaled by 2048 i.e. 2048 * sin(angle). * Range is 1448 to 1774. Default value is 1774. */ @@ -1688,9 +2061,6 @@ struct bmi2_wrist_gest_config /*! @name Structure to define wrist wear wake-up configuration */ struct bmi2_wrist_wear_wake_up_config { - /*! Enable bits for enabling output into the register status bits */ - uint16_t out_conf; - /*! Cosine of min expected attitude change of the device within 1 second time window when * moving within focus position. * The parameter is scaled by 2048 i.e. 2048 * cos(angle). Range is 1024 to 1774. @@ -1736,36 +2106,43 @@ struct bmi2_wrist_wear_wake_up_config struct bmi2_wrist_gest_w_config { /*! Wearable arm (left or right) */ - uint8_t wearable_arm : 1; + uint8_t device_position; - /*! Enable bits for enabling output into the register status bits */ - uint8_t out_conf : 4; + /*! Minimum threshold for flick peak on y-axis */ + uint16_t min_flick_peak_y_threshold; - /*! Sine of the minimum tilt angle in portrait down direction of the device when wrist is rolled - * away from user. The configuration parameter is scaled by 2048 i.e. 2048 * sin(angle). - * Range is 1448 to 1774. Default value is 1774. */ - uint16_t min_flick_peak; + /*! Minimum threshold for flick peak on z-axis */ + uint16_t min_flick_peak_z_threshold; - /*! Value of minimum time difference between wrist roll-out and roll-in movement during flick gesture. - * Range is 3 to 5 samples at 50Hz. Default value is 4 (i.e. 0.08 seconds). */ - uint16_t min_flick_samples; + /*! Maximum expected value of positive gravitational acceleration on x-axis + * when arm is in focus pose */ + uint16_t gravity_bounds_x_pos; - /*! Maximum time within which gesture movement has to be completed. Range is 150 to 250 samples at 50Hz. - * Default value is 200 (i.e. 4 seconds). */ - uint16_t max_duration; + /*! Maximum expected value of negative gravitational acceleration on x-axis + * when arm is in focus pose */ + uint16_t gravity_bounds_x_neg; - /*! Defines the Wait time between the detection of the wrist gesture and reporting the wrist gesture. - * The sample resolution is 20ms. The default value is 25 which is equal to reporting delay 500ms . - */ - uint16_t reporting_delay; + /*! Maximum expected value of negative gravitational acceleration on y-axis + * when arm is in focus pose */ + uint16_t gravity_bounds_y_neg; + + /*! Maximum expected value of negative gravitational acceleration on z-axis + * when arm is in focus pose */ + uint16_t gravity_bounds_z_neg; + + /*! Exponential smoothing coefficient for adaptive peak threshold decay */ + uint16_t flick_peak_decay_coeff; + + /*! Exponential smoothing coefficient for acceleration mean estimation */ + uint16_t lp_mean_filter_coeff; + + /*! Maximum duration between 2 peaks of jiggle in samples @50Hz */ + uint16_t max_duration_jiggle_peaks; }; /*! @name Structure to define wrist wear wake-up configuration for wearable configuration */ struct bmi2_wrist_wear_wake_up_wh_config { - /*! Enable bits for enabling output into the register status bits */ - uint16_t out_conf; - /*! Cosine of min expected attitude change of the device within 1 second time window when * moving within focus position. * The parameter is scaled by 2048 i.e. 2048 * cos(angle). Range is 1024 to 1774. @@ -1830,9 +2207,6 @@ struct bmi2_primary_ois_config /*! @name Structure to configure free-fall detection settings */ struct bmi2_free_fall_det_config { - /*! Enable bits for enabling output into the register status bits */ - uint16_t out_conf; - /*! free-fall accel settings */ uint16_t freefall_accel_settings[BMI2_FREE_FALL_ACCEL_SET_PARAMS]; }; @@ -1867,18 +2241,12 @@ union bmi2_sens_config_types /*! Gyroscope user gain configuration */ struct bmi2_gyro_user_gain_config gyro_gain_update; - /*! Tilt configuration */ - struct bmi2_tilt_config tilt; - - /*! uphold to wake configuration */ - struct bmi2_up_hold_to_wake_config up_hold_to_wake; - - /*! Glance detector configuration */ - struct bmi2_glance_det_config glance_det; - /*! Wake-up configuration */ struct bmi2_wake_up_config tap; + /*! Tap configuration */ + struct bmi2_tap_config tap_conf; + /*! Orientation configuration */ struct bmi2_orient_config orientation; @@ -1891,9 +2259,6 @@ union bmi2_sens_config_types /*! Flat configuration */ struct bmi2_flat_config flat; - /*! External sensor sync configuration */ - struct bmi2_ext_sens_sync_config ext_sens_sync; - /*! Wrist gesture configuration */ struct bmi2_wrist_gest_config wrist_gest; @@ -1936,20 +2301,37 @@ struct bmi2_feature_config uint8_t start_addr; }; +/*! @name Structure to define the feature interrupt configurations */ +struct bmi2_map_int +{ + /*! Defines the type of sensor */ + uint8_t type; + + /*! Defines the feature interrupt */ + uint8_t sens_map_int; +}; + /*! @name Structure to define BMI2 sensor configurations */ struct bmi2_dev { /*! Chip id of BMI2 */ uint8_t chip_id; - /*! Device id of BMI2 */ - uint8_t dev_id; + /*! The interface pointer is used to enable the user + * to link their interface descriptors for reference during the + * implementation of the read and write interfaces to the + * hardware. + */ + void *intf_ptr; /*! To store warnings */ uint8_t info; /*! Type of Interface */ - enum bmi2_intf_type intf; + enum bmi2_intf intf; + + /*! To store interface pointer error */ + BMI2_INTF_RETURN_TYPE intf_rslt; /*! For switching from I2C to SPI */ uint8_t dummy_byte; @@ -1984,14 +2366,11 @@ struct bmi2_dev /*! Array of feature output configuration structure */ const struct bmi2_feature_config *feat_output; - /*! Structure to maintain a copy of feature out_conf values */ - struct bmi2_int_map int_map; - /*! Structure to maintain a copy of the re-mapped axis */ struct bmi2_axes_remap remap; /*! Flag to hold enable status of sensors */ - uint32_t sens_en_stat; + uint64_t sens_en_stat; /*! Read function pointer */ bmi2_read_fptr_t read; @@ -2016,37 +2395,81 @@ struct bmi2_dev /* To store hold the size of config file */ uint16_t config_size; + + /*! Function pointer to get wakeup configurations */ + bmi2_wake_up_fptr_t get_wakeup_config; + + /*! Function pointer to set wakeup configurations */ + bmi2_wake_up_fptr_t set_wakeup_config; + + /*! Function pointer to get tap configurations */ + bmi2_tap_fptr_t get_tap_config; + + /*! Function pointer to set tap configurations */ + bmi2_tap_fptr_t set_tap_config; + + /*! Array of feature interrupts configuration structure */ + struct bmi2_map_int *map_int; + + /*! To define maximum number of interrupts */ + uint8_t sens_int_map; }; /*! @name Structure to enable an accel axis for foc */ -struct accel_foc_g_value +struct bmi2_accel_foc_g_value { - /* '0' to disable x axis and '1' to enable x axis */ + /*! '0' to disable x axis and '1' to enable x axis */ uint8_t x; - /* '0' to disable y axis and '1' to enable y axis */ + /*! '0' to disable y axis and '1' to enable y axis */ uint8_t y; - /* '0' to disable z axis and '1' to enable z axis */ + /*! '0' to disable z axis and '1' to enable z axis */ uint8_t z; - /* '0' for positive input and '1' for negative input */ + /*! '0' for positive input and '1' for negative input */ uint8_t sign; }; /*! @name Structure to configure activity recognition settings */ -struct act_recg_sett +struct bmi2_act_recg_sett { - /* 1 to enable & 0 to disable post processing */ + /*! Activity recognition register 1 */ uint8_t act_rec_1 : 1; + /*! Activity recognition register 2 */ uint16_t act_rec_2; + /*! Activity recognition register 3 */ uint16_t act_rec_3; + /*! Activity recognition register 4 */ uint8_t act_rec_4 : 4; + /*! Activity recognition register 5 */ uint8_t act_rec_5 : 4; }; +/*! @name Structure to configure activity recognition settings for bmi270hc */ +struct bmi2_hc_act_recg_sett +{ + /*! Static segment size for activity classification. */ + uint8_t segment_size; + + /*! Enable/Disable post processing of the activity detected */ + uint8_t pp_en; + + /*! Minimum threshold of the Gini's diversity index (GDI) */ + uint16_t min_gdi_thres; + + /*! Maximum threshold of the Gini's diversity index (GDI) */ + uint16_t max_gdi_thres; + + /*! Buffer size for post processing of the activity detected */ + uint16_t buf_size; + + /*! Minimum segments belonging to a certain activity type */ + uint16_t min_seg_conf; +}; + #endif /* BMI2_DEFS_H_ */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_ois.c b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_ois.c index 407982fa8..04350db08 100644 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_ois.c +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_ois.c @@ -1,40 +1,42 @@ /** -* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. -* -* BSD-3-Clause -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* 1. Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* -* 2. Redistributions in binary form must reproduce the above copyright -* notice, this list of conditions and the following disclaimer in the -* documentation and/or other materials provided with the distribution. -* -* 3. Neither the name of the copyright holder nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) -* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, -* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING -* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -* -* @file bmi2_ois.c -* @date 2020-01-10 -* @version v2.46.1 -* -*//******************************************************************************/ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * BSD-3-Clause + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING + * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * @file bmi2_ois.c + * @date 2020-05-05 + * @version v2.23.2 + * + */ + +/******************************************************************************/ /*! @name Header Files */ /******************************************************************************/ @@ -55,11 +57,12 @@ * bmi2xy_init(), refer the example ois_accel_gyro.c for more info" * * @return Result of API execution status - * @retval zero -> Success / +ve value -> Warning / -ve value -> Error + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t get_ois_acc_gyr_data(struct bmi2_ois_sens_axes_data *ois_data, uint8_t reg_addr, - const struct bmi2_ois_dev *ois_dev, + struct bmi2_ois_dev *ois_dev, int16_t ois_gyr_cross_sens_zx); /*! @@ -69,7 +72,8 @@ static int8_t get_ois_acc_gyr_data(struct bmi2_ois_sens_axes_data *ois_data, * @param[in] ois_dev : Structure instance of bmi2_ois_dev. * * @return Result of API execution status - * @retval zero -> Success / +ve value -> Warning / -ve value -> Error + * @retval 0 -> Success + * @retval < 0 -> Fail */ static int8_t null_ptr_check(const struct bmi2_ois_dev *ois_dev); @@ -92,10 +96,7 @@ static void comp_gyro_cross_axis_sensitivity(struct bmi2_ois_sens_axes_data *ois * @brief This API reads the data from the given OIS register address of bmi2 * sensor. */ -int8_t bmi2_ois_get_regs(uint8_t ois_reg_addr, - uint8_t *ois_reg_data, - uint16_t data_len, - const struct bmi2_ois_dev *ois_dev) +int8_t bmi2_ois_get_regs(uint8_t ois_reg_addr, uint8_t *ois_reg_data, uint16_t data_len, struct bmi2_ois_dev *ois_dev) { /* Variable to define error */ int8_t rslt; @@ -120,8 +121,8 @@ int8_t bmi2_ois_get_regs(uint8_t ois_reg_addr, ois_reg_addr = (ois_reg_addr | BMI2_OIS_SPI_RD_MASK); /* Read from OIS register through OIS interface */ - rslt = ois_dev->ois_read(ois_dev->dev_id, ois_reg_addr, temp_buf, temp_len); - if (rslt == BMI2_OIS_OK) + ois_dev->intf_rslt = ois_dev->ois_read(ois_reg_addr, temp_buf, temp_len, ois_dev->intf_ptr); + if (ois_dev->intf_rslt == BMI2_INTF_RET_SUCCESS) { /* Read the data from the position next to dummy byte */ while (index < data_len) @@ -147,9 +148,9 @@ int8_t bmi2_ois_get_regs(uint8_t ois_reg_addr, * @brief This API writes data to the given OIS register address of bmi2 sensor. */ int8_t bmi2_ois_set_regs(uint8_t ois_reg_addr, - uint8_t *ois_reg_data, + const uint8_t *ois_reg_data, uint16_t data_len, - const struct bmi2_ois_dev *ois_dev) + struct bmi2_ois_dev *ois_dev) { /* Variable to define error */ int8_t rslt; @@ -162,8 +163,8 @@ int8_t bmi2_ois_set_regs(uint8_t ois_reg_addr, ois_reg_addr = (ois_reg_addr & BMI2_OIS_SPI_WR_MASK); /* Burst write to OIS register through OIS interface */ - rslt = ois_dev->ois_write(ois_dev->dev_id, ois_reg_addr, ois_reg_data, data_len); - if (rslt != BMI2_OIS_OK) + ois_dev->intf_rslt = ois_dev->ois_write(ois_reg_addr, ois_reg_data, data_len, ois_dev->intf_ptr); + if (ois_dev->intf_rslt != BMI2_INTF_RET_SUCCESS) { rslt = BMI2_OIS_E_COM_FAIL; } @@ -180,7 +181,7 @@ int8_t bmi2_ois_set_regs(uint8_t ois_reg_addr, * @brief This API enables/disables accelerometer/gyroscope data read through * OIS interface. */ -int8_t bmi2_ois_set_config(const struct bmi2_ois_dev *ois_dev) +int8_t bmi2_ois_set_config(struct bmi2_ois_dev *ois_dev) { /* Variable to define error */ int8_t rslt; @@ -324,12 +325,17 @@ int8_t bmi2_ois_read_data(const uint8_t *sens_sel, /*! Local Function Definitions ****************************************************************************/ +/*! @cond DOXYGEN_SUPRESS */ + +/* Suppressing doxygen warnings triggered for same static function names present across various sensor variant + * directories */ + /*! * @brief This internal API gets the accelerometer and the gyroscope data. */ static int8_t get_ois_acc_gyr_data(struct bmi2_ois_sens_axes_data *ois_data, uint8_t reg_addr, - const struct bmi2_ois_dev *ois_dev, + struct bmi2_ois_dev *ois_dev, int16_t ois_gyr_cross_sens_zx) { /* Variable to define error */ @@ -388,7 +394,7 @@ static int8_t null_ptr_check(const struct bmi2_ois_dev *ois_dev) int8_t rslt = BMI2_OIS_OK; if ((ois_dev == NULL) || (ois_dev->ois_read == NULL) || (ois_dev->ois_write == NULL) || - (ois_dev->ois_delay_ms == NULL)) + (ois_dev->ois_delay_us == NULL)) { /* Device structure pointer is NULL */ rslt = BMI2_OIS_E_NULL_PTR; @@ -407,3 +413,5 @@ static void comp_gyro_cross_axis_sensitivity(struct bmi2_ois_sens_axes_data *ois /* Get the compensated gyroscope x-axis */ ois_data->x = ois_data->x - (int16_t)(((int32_t)ois_gyr_cross_sens_zx * (int32_t)ois_data->z) / 512); } + +/*! @endcond */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_ois.h b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_ois.h index cd1e0f56d..9b700cd95 100644 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_ois.h +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_ois.h @@ -1,79 +1,48 @@ /** -* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. -* -* BSD-3-Clause -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* 1. Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* -* 2. Redistributions in binary form must reproduce the above copyright -* notice, this list of conditions and the following disclaimer in the -* documentation and/or other materials provided with the distribution. -* -* 3. Neither the name of the copyright holder nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) -* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, -* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING -* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -* -* @file bmi2_ois.h -* @date 2020-01-10 -* @version v2.46.1 -* -*//** -* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. -* -* BSD-3-Clause -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* 1. Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* -* 2. Redistributions in binary form must reproduce the above copyright -* notice, this list of conditions and the following disclaimer in the -* documentation and/or other materials provided with the distribution. -* -* 3. Neither the name of the copyright holder nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) -* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, -* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING -* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -* -* @file bmi2_ois.h -* @date 10/01/2020 -* @version -* -*/ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * BSD-3-Clause + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING + * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * @file bmi2_ois.h + * @date 2020-05-05 + * @version v2.23.2 + * + */ -#ifndef BMI2_OIS_H_ -#define BMI2_OIS_H_ +/** + * \ingroup bmi2xy + * \defgroup bmi2_ois BMI2_OIS + * @brief Sensor driver for BMI2_OIS sensor + */ +#ifndef _BMI2_OIS_H +#define _BMI2_OIS_H /*! CPP guard */ #ifdef __cplusplus @@ -95,6 +64,11 @@ extern "C" { /******************************************************************************/ /*! @name Macros */ /******************************************************************************/ + +#ifndef BMI2_INTF_RETURN_TYPE +#define BMI2_INTF_RETURN_TYPE int8_t +#endif + /*! @name Utility macros */ #define BMI2_OIS_SET_BITS(reg_data, bitname, data) \ ((reg_data & ~(bitname##_MASK)) | \ @@ -108,58 +82,105 @@ extern "C" { ((reg_data & ~(bitname##_MASK)) | \ (data & bitname##_MASK)) -#define BMI2_GET_BIT_POS0(reg_data, bitname) (reg_data & (bitname##_MASK)) +#define BMI2_GET_BIT_POS0(reg_data, bitname) (reg_data & (bitname##_MASK)) + +/*! @name For enable and disable */ +#define BMI2_OIS_ENABLE UINT8_C(1) +#define BMI2_OIS_DISABLE UINT8_C(0) + +/*! @name To define sensor interface success code */ +#define BMI2_INTF_RET_SUCCESS INT8_C(0) /*! @name To define success code */ -#define BMI2_OIS_OK UINT8_C(0) +#define BMI2_OIS_OK UINT8_C(0) /*! @name To define error codes */ -#define BMI2_OIS_E_NULL_PTR INT8_C(-1) -#define BMI2_OIS_E_COM_FAIL INT8_C(-2) -#define BMI2_OIS_E_INVALID_SENSOR INT8_C(-8) +#define BMI2_OIS_E_NULL_PTR INT8_C(-1) +#define BMI2_OIS_E_COM_FAIL INT8_C(-2) +#define BMI2_OIS_E_INVALID_SENSOR INT8_C(-8) /*! @name Mask definitions for SPI read/write address for OIS */ -#define BMI2_OIS_SPI_RD_MASK UINT8_C(0x80) -#define BMI2_OIS_SPI_WR_MASK UINT8_C(0x7F) +#define BMI2_OIS_SPI_RD_MASK UINT8_C(0x80) +#define BMI2_OIS_SPI_WR_MASK UINT8_C(0x7F) /*! @name BMI2 OIS data bytes */ -#define BMI2_OIS_ACC_GYR_NUM_BYTES UINT8_C(6) +#define BMI2_OIS_ACC_GYR_NUM_BYTES UINT8_C(6) /*! @name Macros to select sensor for OIS data read */ -#define BMI2_OIS_ACCEL UINT8_C(0x01) -#define BMI2_OIS_GYRO UINT8_C(0x02) +#define BMI2_OIS_ACCEL UINT8_C(0x01) +#define BMI2_OIS_GYRO UINT8_C(0x02) /*! @name Macros to define OIS register addresses */ -#define BMI2_OIS_CONFIG_ADDR UINT8_C(0x40) -#define BMI2_OIS_ACC_X_LSB_ADDR UINT8_C(0x0C) -#define BMI2_OIS_GYR_X_LSB_ADDR UINT8_C(0x12) +#define BMI2_OIS_CONFIG_ADDR UINT8_C(0x40) +#define BMI2_OIS_ACC_X_LSB_ADDR UINT8_C(0x0C) +#define BMI2_OIS_GYR_X_LSB_ADDR UINT8_C(0x12) /*! @name Mask definitions for OIS configurations */ -#define BMI2_OIS_GYR_EN_MASK UINT8_C(0x40) -#define BMI2_OIS_ACC_EN_MASK UINT8_C(0x80) +#define BMI2_OIS_GYR_EN_MASK UINT8_C(0x40) +#define BMI2_OIS_ACC_EN_MASK UINT8_C(0x80) /*! @name Bit Positions for OIS configurations */ -#define BMI2_OIS_GYR_EN_POS UINT8_C(0x06) -#define BMI2_OIS_ACC_EN_POS UINT8_C(0x07) +#define BMI2_OIS_GYR_EN_POS UINT8_C(0x06) +#define BMI2_OIS_ACC_EN_POS UINT8_C(0x07) /*! Low pass filter configuration position and mask */ -#define BMI2_OIS_LP_FILTER_EN_POS UINT8_C(0x00) -#define BMI2_OIS_LP_FILTER_EN_MASK UINT8_C(0x01) +#define BMI2_OIS_LP_FILTER_EN_POS UINT8_C(0x00) +#define BMI2_OIS_LP_FILTER_EN_MASK UINT8_C(0x01) -#define BMI2_OIS_LP_FILTER_CONFIG_POS UINT8_C(0x01) -#define BMI2_OIS_LP_FILTER_CONFIG_MASK UINT8_C(0x06) +#define BMI2_OIS_LP_FILTER_CONFIG_POS UINT8_C(0x01) +#define BMI2_OIS_LP_FILTER_CONFIG_MASK UINT8_C(0x06) -#define BMI2_OIS_LP_FILTER_MUTE_POS UINT8_C(0x05) -#define BMI2_OIS_LP_FILTER_MUTE_MASK UINT8_C(0x20) +#define BMI2_OIS_LP_FILTER_MUTE_POS UINT8_C(0x05) +#define BMI2_OIS_LP_FILTER_MUTE_MASK UINT8_C(0x20) /******************************************************************************/ /*! @name Function Pointers */ /******************************************************************************/ -/*! For interfacing to the I2C or SPI read and write functions */ -typedef int8_t (*bmi2_ois_com_fptr_t)(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len); -/*! For interfacing to the delay function */ -typedef void (*bmi2_ois_delay_fptr_t)(uint32_t period); +/*! + * @brief Bus communication function pointer which should be mapped to + * the platform specific read functions of the user + * + * @param[in] reg_addr : Register address from which data is read. + * @param[out] reg_data : Pointer to data buffer where read data is stored. + * @param[in] len : Number of bytes of data to be read. + * @param[in, out] intf_ptr : Void pointer that can enable the linking of descriptors + * for interface related call backs. + * + * retval = BMI2_INTF_RET_SUCCESS -> Success + * retval != BMI2_INTF_RET_SUCCESS -> Failure + * + */ +typedef BMI2_INTF_RETURN_TYPE (*bmi2_ois_read_fptr_t)(uint8_t reg_addr, uint8_t *data, uint32_t len, void *intf_ptr); + +/*! + * @brief Bus communication function pointer which should be mapped to + * the platform specific write functions of the user + * + * @param[in] reg_addr : Register address to which the data is written. + * @param[in] reg_data : Pointer to data buffer in which data to be written + * is stored. + * @param[in] len : Number of bytes of data to be written. + * @param[in, out] intf_ptr : Void pointer that can enable the linking of descriptors + * for interface related call backs + * + * retval = BMI2_INTF_RET_SUCCESS -> Success + * retval != BMI2_INTF_RET_SUCCESS -> Failure + * + */ +typedef BMI2_INTF_RETURN_TYPE (*bmi2_ois_write_fptr_t)(uint8_t reg_addr, const uint8_t *data, uint32_t len, + void *intf_ptr); + +/*! + * @brief Delay function pointer which should be mapped to + * delay function of the user + * + * @param[in] period : Delay in microseconds. + * @param[in, out] intf_ptr : Void pointer that can enable the linking of descriptors + * for interface related call backs + * + */ +typedef void (*bmi2_ois_delay_fptr_t)(uint32_t period, void *intf_ptr); /******************************************************************************/ /*! @name Structure Declarations */ @@ -181,21 +202,24 @@ struct bmi2_ois_sens_axes_data /*! @name Structure to define bmi2 OIS sensor configurations */ struct bmi2_ois_dev { - /*! Device id of BMI2 */ - uint8_t dev_id; - /*! Read function pointer */ - bmi2_ois_com_fptr_t ois_read; + bmi2_ois_read_fptr_t ois_read; /*! Write function pointer */ - bmi2_ois_com_fptr_t ois_write; + bmi2_ois_write_fptr_t ois_write; /*! Delay function pointer */ - bmi2_ois_delay_fptr_t ois_delay_ms; + bmi2_ois_delay_fptr_t ois_delay_us; /*! Low pass filter en/dis */ uint8_t lp_filter_en; + /*! Void interface pointer */ + void *intf_ptr; + + /*! To store interface pointer error */ + int8_t intf_rslt; + /*! Low pass filter cut-off frequency */ uint8_t lp_filter_config; @@ -213,7 +237,6 @@ struct bmi2_ois_dev /*! Gyroscope data axes */ struct bmi2_ois_sens_axes_data gyr_data; - }; /***************************************************************************/ @@ -221,8 +244,22 @@ struct bmi2_ois_dev /*! BMI2 OIS User Interface function prototypes ****************************************************************************/ +/** + * \ingroup bmi2_ois + * \defgroup bmi2_oisApiRegs Registers + * @brief Read data from the given OIS register address of bmi2 + */ + /*! - * @brief This API reads the data from the given OIS register address of bmi2 + * \ingroup bmi2_oisApiRegs + * \page bmi2_ois_api_bmi2_ois_get_regs bmi2_ois_get_regs + * \code + * int8_t bmi2_ois_get_regs(uint8_t ois_reg_addr, + * uint8_t *ois_reg_data, + * uint16_t data_len, + * const struct bmi2_ois_dev *ois_dev); + * \endcode + * @details This API reads the data from the given OIS register address of bmi2 * sensor. * * @param[in] ois_reg_addr : OIS register address from which data is read. @@ -231,15 +268,21 @@ struct bmi2_ois_dev * @param[in] ois_dev : Structure instance of bmi2_ois_dev. * * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error + * @retval 0 -> Success + * @retval < 0 -> Fail */ -int8_t bmi2_ois_get_regs(uint8_t ois_reg_addr, - uint8_t *ois_reg_data, - uint16_t data_len, - const struct bmi2_ois_dev *ois_dev); +int8_t bmi2_ois_get_regs(uint8_t ois_reg_addr, uint8_t *ois_reg_data, uint16_t data_len, struct bmi2_ois_dev *ois_dev); /*! - * @brief This API writes data to the given OIS register address of bmi2 sensor. + * \ingroup bmi2_oisApiRegs + * \page bmi2_ois_api_bmi2_ois_set_regs bmi2_ois_set_regs + * \code + * int8_t bmi2_ois_set_regs(uint8_t ois_reg_addr, + * uint8_t *ois_reg_data, + * uint16_t data_len, + * const struct bmi2_ois_dev *ois_dev); + * \endcode + * @details This API writes data to the given OIS register address of bmi2 sensor. * * @param[in] ois_reg_addr : OIS register address to which the data is written. * @param[in] ois_reg_data : Pointer to data buffer in which data to be written @@ -248,26 +291,44 @@ int8_t bmi2_ois_get_regs(uint8_t ois_reg_addr, * @param[in] ois_dev : Structure instance of bmi2_ois_dev. * * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error + * @retval 0 -> Success + * @retval < 0 -> Fail */ int8_t bmi2_ois_set_regs(uint8_t ois_reg_addr, - uint8_t *ois_reg_data, + const uint8_t *ois_reg_data, uint16_t data_len, - const struct bmi2_ois_dev *ois_dev); + struct bmi2_ois_dev *ois_dev); + +/** + * \ingroup bmi2_ois + * \defgroup bmi2_oisApiConfig Status update + * @brief Get / Set the status of Enable / Disable accelerometer / gyroscope data read through OIS interface + */ /*! - * @brief This API enables/disables accelerometer/gyroscope data read through + * \ingroup bmi2_oisApiConfig + * \page bmi2_ois_api_bmi2_ois_set_config bmi2_ois_set_config + * \code + * int8_t bmi2_ois_set_config(const struct bmi2_ois_dev *ois_dev); + * \endcode + * @details This API sets the status of enable/disable accelerometer/gyroscope data read through * OIS interface. * * @param[in] ois_dev : Structure instance of bmi2_ois_dev. * * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error + * @retval 0 -> Success + * @retval < 0 -> Fail */ -int8_t bmi2_ois_set_config(const struct bmi2_ois_dev *ois_dev); +int8_t bmi2_ois_set_config(struct bmi2_ois_dev *ois_dev); /*! - * @brief This API gets the status of accelerometer/gyroscope enable for data + * \ingroup bmi2_oisApiConfig + * \page bmi2_ois_api_bmi2_ois_get_config bmi2_ois_get_config + * \code + * int8_t bmi2_ois_get_config(struct bmi2_ois_dev *ois_dev); + * \endcode + * @details This API gets the status of accelerometer/gyroscope enable for data * read through OIS interface. * * @param[in, out] ois_dev : Structure instance of bmi2_ois_dev. @@ -275,12 +336,27 @@ int8_t bmi2_ois_set_config(const struct bmi2_ois_dev *ois_dev); * @note Enabling and disabling is done during OIS structure initialization. * * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error + * @retval 0 -> Success + * @retval < 0 -> Fail */ int8_t bmi2_ois_get_config(struct bmi2_ois_dev *ois_dev); +/** + * \ingroup bmi2_ois + * \defgroup bmi2_oisApiRead Data read + * @brief Read accelerometer / gyroscope data through OIS interface + */ + /*! - * @brief This API reads accelerometer/gyroscope data through OIS interface. + * \ingroup bmi2_oisApiRead + * \page bmi2_ois_api_bmi2_ois_read_data bmi2_ois_read_data + * \code + * int8_t bmi2_ois_read_data(const uint8_t *sens_sel, + * uint8_t n_sens, + * struct bmi2_ois_dev *ois_dev, + * int16_t gyr_cross_sens_zx); + * \endcode + * @details This API reads accelerometer/gyroscope data through OIS interface. * * @param[in] sens_sel : Select sensor whose data is to be read. * @param[in] n_sens : Number of sensors selected. @@ -288,24 +364,24 @@ int8_t bmi2_ois_get_config(struct bmi2_ois_dev *ois_dev); * @param[in] gyr_cross_sens_zx : Store the gyroscope cross sensitivity values taken from the bmi2xy * (refer bmi2_ois example). * + *@verbatim * sens_sel | Value * ---------------|--------------- * BMI2_OIS_ACCEL | 0x01 * BMI2_OIS_GYRO | 0x02 + *@endverbatim * * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error. + * @retval 0 -> Success + * @retval < 0 -> Fail */ int8_t bmi2_ois_read_data(const uint8_t *sens_sel, uint8_t n_sens, struct bmi2_ois_dev *ois_dev, int16_t gyr_cross_sens_zx); -/******************************************************************************/ -/*! @name C++ Guard Macros */ -/******************************************************************************/ #ifdef __cplusplus } #endif /* End of CPP guard */ -#endif /* BMI2_OIS_H_ */ +#endif /* End of _BMI2_OIS_H */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/example/basic.c b/lib/main/BoschSensortec/BMI270-Sensor-API/example/basic.c deleted file mode 100644 index 98c832785..000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/example/basic.c +++ /dev/null @@ -1,177 +0,0 @@ -#include -#include "bmi2.h" -#include "bmi270.h" - -void delay_us(uint32_t period); -int8_t i2c_reg_write(uint8_t i2c_addr, uint8_t reg_addr, const uint8_t *reg_data, uint16_t length); -int8_t i2c_reg_read(uint8_t i2c_addr, uint8_t reg_addr, uint8_t *reg_data, uint16_t length); -int8_t spi_reg_write(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length); -int8_t spi_reg_read(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length); -void print_rslt(int8_t rslt); - -int main(void) -{ - /* Variable to define rslt */ - int8_t rslt; - struct bmi2_dev dev; - - /*To enable I2C interface*/ - dev.read = i2c_reg_read; - dev.write = i2c_reg_write; - dev.delay_us = delay_us; - dev.read_write_len = 128; - dev.intf = BMI2_I2C_INTERFACE; - dev.dev_id = BMI2_I2C_PRIM_ADDR; - - /* To enable SPI interface*/ - - /* dev.read = spi_reg_read; - * dev.write = spi_reg_write; - * dev.delay_us = delay_us; - * dev.read_write_len = 4096; - * dev.intf = BMI2_SPI_INTERFACE; - * dev.dev_id = SPI_CS; - * dev.dummy_byte = 1; - */ - - dev.config_file_ptr = NULL; - - /* Initialize BMI2 */ - rslt = bmi270_init(&dev); - print_rslt(rslt); - - return 0; -} - -/*! - * @brief Function that creates a mandatory delay required in some of the APIs such as "bma4_write_config_file", - * "bma4_write_regs", "bma4_set_accel_config" and so on. - * - * @param[in] period_us : the required wait time in microseconds. - * @return void. - * - */ -void delay_us(uint32_t period) -{ - /* Wait for a period amount of us*/ -} - -/*! - * @brief Function for writing the sensor's registers through I2C bus. - * - * @param[in] i2c_addr : sensor I2C address. - * @param[in] reg_addr : Register address. - * @param[in] reg_data : Pointer to the data buffer whose value is to be written. - * @param[in] length : No of bytes to write. - * - * @return Status of execution - * @retval 0 -> Success - * @retval >0 -> Failure Info - * - */ -int8_t i2c_reg_write(uint8_t i2c_addr, uint8_t reg_addr, const uint8_t *reg_data, uint16_t length) -{ - - /* Write to registers using I2C. Return 0 for a successful execution. */ - return 0; -} - -/*! - * @brief Function for reading the sensor's registers through I2C bus. - * - * @param[in] i2c_addr : Sensor I2C address. - * @param[in] reg_addr : Register address. - * @param[out] reg_data : Pointer to the data buffer to store the read data. - * @param[in] length : No of bytes to read. - * - * @return Status of execution - * @retval 0 -> Success - * @retval >0 -> Failure Info - * - */ -int8_t i2c_reg_read(uint8_t i2c_addr, uint8_t reg_addr, uint8_t *reg_data, uint16_t length) -{ - - /* Read from registers using I2C. Return 0 for a successful execution. */ - return 0; -} - -/*! - * @brief Function for writing the sensor's registers through SPI bus. - * - * @param[in] cs : Chip select to enable the sensor. - * @param[in] reg_addr : Register address. - * @param[in] reg_data : Pointer to the data buffer whose data has to be written. - * @param[in] length : No of bytes to write. - * - * @return Status of execution - * @retval 0 -> Success - * @retval >0 -> Failure Info - * - */ -int8_t spi_reg_write(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length) -{ - - /* Write to registers using SPI. Return 0 for a successful execution. */ - return 0; -} - -/*! - * @brief Function for reading the sensor's registers through SPI bus. - * - * @param[in] cs : Chip select to enable the sensor. - * @param[in] reg_addr : Register address. - * @param[out] reg_data : Pointer to the data buffer to store the read data. - * @param[in] length : No of bytes to read. - * - * @return Status of execution - * @retval 0 -> Success - * @retval >0 -> Failure Info - * - */ -int8_t spi_reg_read(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length) -{ - - /* Read from registers using SPI. Return 0 for a successful execution. */ - return 0; -} - -/*! - * @brief Prints the execution status of the APIs. - * - * @param[in] api_name : name of the API whose execution status has to be printed. - * @param[in] rslt : error code returned by the API whose execution status has to be printed. - * - * @return void. - */ -void print_rslt(int8_t rslt) -{ - switch (rslt) - { - case BMI2_OK: - - /* Do nothing */ - break; - case BMI2_E_NULL_PTR: - printf("Error [%d] : Null pointer\r\n", rslt); - break; - case BMI2_E_COM_FAIL: - printf("Error [%d] : Communication failure\r\n", rslt); - break; - case BMI2_E_DEV_NOT_FOUND: - printf("Error [%d] : Device not found\r\n", rslt); - break; - case BMI2_E_INVALID_SENSOR: - printf("Error [%d] : Invalid sensor\r\n", rslt); - break; - case BMI2_E_SELF_TEST_FAIL: - printf("Warning [%d] : Self test failed\r\n", rslt); - break; - case BMI2_E_INVALID_INT_PIN: - printf("warning [%d] : invalid int pin\r\n", rslt); - break; - default: - printf("Error [%d] : Unknown error code\r\n", rslt); - break; - } -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/example/crt_ex.c b/lib/main/BoschSensortec/BMI270-Sensor-API/example/crt_ex.c deleted file mode 100644 index e2388f3eb..000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/example/crt_ex.c +++ /dev/null @@ -1,219 +0,0 @@ -#include -#include "bmi2.h" -#include "bmi270.h" - - -void delay_us(uint32_t period); -int8_t i2c_reg_read(uint8_t i2c_add, uint8_t reg_add, uint8_t *regd_data, uint16_t length); -int8_t i2c_reg_write(uint8_t i2c_add, uint8_t reg_add, const uint8_t *reg_data, uint16_t length); -int8_t spi_reg_read(uint8_t cs, uint8_t reg_add, uint8_t *reg_data, uint16_t length); -int8_t spi_reg_write(uint8_t cs, uint8_t reg_add, uint8_t *reg_data, uint16_t length); -void print_rslt(int8_t rslt); - -int main(void) -{ - - struct bmi2_dev dev; - - /*! @name Structure to define type of sensor and their respective data */ - struct bmi2_sensor_data sensor_data; - - uint8_t sens_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; - - /*variable to define rslt*/ - int8_t rslt; - - /*to enable i2c interface*/ - dev.read = i2c_reg_read; - dev.write = i2c_reg_write; - dev.delay_us = delay_us; - dev.read_write_len = 128; - dev.intf = BMI2_I2C_INTERFACE; - dev.dev_id = BMI2_I2C_PRIM_ADDR; - - /*to enable spi interface*/ - /* dev.read = spi_reg_read; - dev.write = spi_reg_write; - dev.delay_us = delay_us; - dev.read_write_len = 4096; - dev.intf = BMI2_SPI_INTERFACE; - dev.dev_id = SPI_CS; - dev.dummy_byte = 1; - */ - dev.config_file_ptr = NULL; - - /*to Initialize bmi270*/ - rslt = bmi270_init(&dev); - print_rslt(rslt); - - rslt = bmi2_sensor_enable(&sens_sel[0], 1, &dev); - print_rslt(rslt); - - rslt = bmi2_sensor_disable(&sens_sel[1], 1, &dev); - print_rslt(rslt); - - sensor_data.type = BMI2_ACCEL; - - dev.delay_us(100000); - - printf("\nbefore CRT Accel x,y,z values\n"); - - /* read the accel data before CRT*/ - rslt = bmi2_get_sensor_data(&sensor_data, 1, &dev); - print_rslt(rslt); - printf("\nX axes: %d, Y axes: %d, Z axes: %d \n", sensor_data.sens_data.acc.x, sensor_data.sens_data.acc.y, sensor_data.sens_data.acc.z ); - - /*brief This API is to run the CRT process*/ - rslt = bmi2_do_crt(&dev); - print_rslt(rslt); - if(rslt == BMI2_OK) - { - printf("\nAfter CRT Accel x,y,z values\n"); - - /* read the accel data after CRT*/ - rslt = bmi2_get_sensor_data(&sensor_data, 1, &dev); - print_rslt(rslt); - - printf("X axes: %d, Y axes: %d, Z axes: %d",sensor_data.sens_data.acc.x, sensor_data.sens_data.acc.y, sensor_data.sens_data.acc.z ); - } - -} - -/*! - * @brief Function that creates a mandatory delay required in some of the APIs such as "bma4_write_config_file", - * "bma4_write_regs", "bma4_set_accel_config" and so on. - * - * @param[in] period_us : the required wait time in microseconds. - * @return void. - * - */ -void delay_us(uint32_t period) -{ - /* Wait for a period amount of us*/ -} - -/*! - * @brief Function for writing the sensor's registers through I2C bus. - * - * @param[in] i2c_addr : sensor I2C address. - * @param[in] reg_addr : Register address. - * @param[in] reg_data : Pointer to the data buffer whose value is to be written. - * @param[in] length : No of bytes to write. - * - * @return Status of execution - * @retval 0 -> Success - * @retval >0 -> Failure Info - * - */ -int8_t i2c_reg_write(uint8_t i2c_addr, uint8_t reg_addr, const uint8_t *reg_data, uint16_t length) -{ - - /* Write to registers using I2C. Return 0 for a successful execution. */ - return 0; -} - -/*! - * @brief Function for reading the sensor's registers through I2C bus. - * - * @param[in] i2c_addr : Sensor I2C address. - * @param[in] reg_addr : Register address. - * @param[out] reg_data : Pointer to the data buffer to store the read data. - * @param[in] length : No of bytes to read. - * - * @return Status of execution - * @retval 0 -> Success - * @retval >0 -> Failure Info - * - */ -int8_t i2c_reg_read(uint8_t i2c_addr, uint8_t reg_addr, uint8_t *reg_data, uint16_t length) -{ - - /* Read from registers using I2C. Return 0 for a successful execution. */ - return 0; -} - -/*! - * @brief Function for writing the sensor's registers through SPI bus. - * - * @param[in] cs : Chip select to enable the sensor. - * @param[in] reg_addr : Register address. - * @param[in] reg_data : Pointer to the data buffer whose data has to be written. - * @param[in] length : No of bytes to write. - * - * @return Status of execution - * @retval 0 -> Success - * @retval >0 -> Failure Info - * - */ -int8_t spi_reg_write(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length) -{ - - /* Write to registers using SPI. Return 0 for a successful execution. */ - return 0; -} - -/*! - * @brief Function for reading the sensor's registers through SPI bus. - * - * @param[in] cs : Chip select to enable the sensor. - * @param[in] reg_addr : Register address. - * @param[out] reg_data : Pointer to the data buffer to store the read data. - * @param[in] length : No of bytes to read. - * - * @return Status of execution - * @retval 0 -> Success - * @retval >0 -> Failure Info - * - */ -int8_t spi_reg_read(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length) -{ - - /* Read from registers using SPI. Return 0 for a successful execution. */ - return 0; -} - -/*! - * @brief Prints the execution status of the APIs. - * - * @param[in] api_name : name of the API whose execution status has to be printed. - * @param[in] rslt : error code returned by the API whose execution status has to be printed. - * - * @return void. - */ -void print_rslt(int8_t rslt) -{ - switch (rslt) - { - case BMI2_OK: - - /* Do nothing */ - break; - case BMI2_E_NULL_PTR: - printf("Error [%d] : Null pointer\r\n", rslt); - break; - case BMI2_E_COM_FAIL: - printf("Error [%d] : Communication failure\r\n", rslt); - break; - case BMI2_E_DEV_NOT_FOUND: - printf("Error [%d] : Device not found\r\n", rslt); - break; - case BMI2_E_INVALID_SENSOR: - printf("Error [%d] : Invalid sensor\r\n", rslt); - break; - case BMI2_E_SELF_TEST_FAIL: - printf("Warning [%d] : Self test failed\r\n", rslt); - break; - case BMI2_E_INVALID_INT_PIN: - printf("warning [%d] : invalid int pin\r\n", rslt); - break; - case BMI2_E_CRT_ERROR: - printf("warning[%d] : CRT fail\r\n", rslt); - break; - case BMI2_E_ABORT_ERROR: - printf("warning[%d] : Abort eror\r\n", rslt); - break; - default: - printf("Error [%d] : Unknown error code\r\n", rslt); - break; - } -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/example/motion_interrupt.c b/lib/main/BoschSensortec/BMI270-Sensor-API/example/motion_interrupt.c deleted file mode 100644 index 54b97a313..000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/example/motion_interrupt.c +++ /dev/null @@ -1,230 +0,0 @@ -#include -#include "bmi2.h" -#include "bmi270.h" - -void delay_us(uint32_t period); -int8_t i2c_reg_write(uint8_t i2c_addr, uint8_t reg_addr, const uint8_t *reg_data, uint16_t length); -int8_t i2c_reg_read(uint8_t i2c_addr, uint8_t reg_addr, uint8_t *reg_data, uint16_t length); -int8_t spi_reg_write(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length); -int8_t spi_reg_read(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length); -void print_rslt(int8_t rslt); - -int main(void) -{ - /* Variable to define rslt */ - int8_t rslt; - struct bmi2_dev dev; - struct bmi2_sens_config config; - - /* Array to select sensors */ - uint8_t sens_list[2] = { BMI2_ACCEL, BMI2_NO_MOTION }; - - /* Variable to get any-motion status */ - uint16_t no_mot_status = 0; - - /* Select features and their pins to be mapped to */ - struct bmi2_sens_int_config sens_int = { .type = BMI2_NO_MOTION, .hw_int_pin = BMI2_INT1 }; - - - /*to enable the i2c interface*/ - dev.read = i2c_reg_read; - dev.write = i2c_reg_write; - dev.delay_us = delay_us; - dev.read_write_len = 128; - dev.intf = BMI2_I2C_INTERFACE; - dev.dev_id = BMI2_I2C_PRIM_ADDR; - - /*to enable spi interface*/ - /* - * ac_setup_interface(AC_SPI_STD); - * dev.read = ac_spi_read; - * dev.write = ac_spi_write; - * dev.delay_us = delay_us; - * dev.read_write_len = 4096; - * dev.intf = BMI2_SPI_INTERFACE; - * dev.dev_id = SPI_CS; - * dev.dummy_byte = 1; - */ - - dev.config_file_ptr = NULL; - - /*initialize bmi270 */ - rslt = bmi270_init(&dev); - print_rslt(rslt); - - /* Enable the selected sensors */ - rslt = bmi2_sensor_enable(sens_list, 2, &dev); - print_rslt(rslt); - - /* Configure type of feature */ - config.type = BMI2_NO_MOTION; - - /* Get default configurations for the type of feature selected */ - rslt = bmi2_get_sensor_config(&config, 1, &dev); - print_rslt(rslt); - - /* Set the new configuration */ - rslt = bmi2_set_sensor_config(&config, 1, &dev); - print_rslt(rslt); - - /* Set the new configuration */ - rslt = bmi2_map_feat_int(&sens_int, 1, &dev); - print_rslt(rslt); - - printf("Do not move the board\n"); - - while (1) - { - /* Clear buffer */ - no_mot_status = 0; - - dev.delay_us(50000); - - /* Check the interrupt status of the no-motion */ - rslt = bmi2_get_int_status(&no_mot_status, &dev); - print_rslt(rslt); - if (no_mot_status & BMI270_NO_MOT_STATUS_MASK) - { - printf("no_mot_status = %x\n", no_mot_status); - printf("No-motion interrupt generated"); - break; - } - - dev.delay_us(50000); - } - - return 0; -} - -/*! - * @brief Function that creates a mandatory delay required in some of the APIs such as "bma4_write_config_file", - * "bma4_write_regs", "bma4_set_accel_config" and so on. - * - * @param[in] period_us : the required wait time in microseconds. - * @return void. - * - */ -void delay_us(uint32_t period) -{ - /* Wait for a period amount of us*/ -} - -/*! - * @brief Function for writing the sensor's registers through I2C bus. - * - * @param[in] i2c_addr : sensor I2C address. - * @param[in] reg_addr : Register address. - * @param[in] reg_data : Pointer to the data buffer whose value is to be written. - * @param[in] length : No of bytes to write. - * - * @return Status of execution - * @retval 0 -> Success - * @retval >0 -> Failure Info - * - */ -int8_t i2c_reg_write(uint8_t i2c_addr, uint8_t reg_addr, const uint8_t *reg_data, uint16_t length) -{ - - /* Write to registers using I2C. Return 0 for a successful execution. */ - return 0; -} - -/*! - * @brief Function for reading the sensor's registers through I2C bus. - * - * @param[in] i2c_addr : Sensor I2C address. - * @param[in] reg_addr : Register address. - * @param[out] reg_data : Pointer to the data buffer to store the read data. - * @param[in] length : No of bytes to read. - * - * @return Status of execution - * @retval 0 -> Success - * @retval >0 -> Failure Info - * - */ -int8_t i2c_reg_read(uint8_t i2c_addr, uint8_t reg_addr, uint8_t *reg_data, uint16_t length) -{ - - /* Read from registers using I2C. Return 0 for a successful execution. */ - return 0; -} - -/*! - * @brief Function for writing the sensor's registers through SPI bus. - * - * @param[in] cs : Chip select to enable the sensor. - * @param[in] reg_addr : Register address. - * @param[in] reg_data : Pointer to the data buffer whose data has to be written. - * @param[in] length : No of bytes to write. - * - * @return Status of execution - * @retval 0 -> Success - * @retval >0 -> Failure Info - * - */ -int8_t spi_reg_write(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length) -{ - - /* Write to registers using SPI. Return 0 for a successful execution. */ - return 0; -} - -/*! - * @brief Function for reading the sensor's registers through SPI bus. - * - * @param[in] cs : Chip select to enable the sensor. - * @param[in] reg_addr : Register address. - * @param[out] reg_data : Pointer to the data buffer to store the read data. - * @param[in] length : No of bytes to read. - * - * @return Status of execution - * @retval 0 -> Success - * @retval >0 -> Failure Info - * - */ -int8_t spi_reg_read(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length) -{ - - /* Read from registers using SPI. Return 0 for a successful execution. */ - return 0; -} - -/*! - * @brief Prints the execution status of the APIs. - * - * @param[in] api_name : name of the API whose execution status has to be printed. - * @param[in] rslt : error code returned by the API whose execution status has to be printed. - * - * @return void. - */ -void print_rslt(int8_t rslt) -{ - switch (rslt) - { - case BMI2_OK: - - /* Do nothing */ - break; - case BMI2_E_NULL_PTR: - printf("Error [%d] : Null pointer\r\n", rslt); - break; - case BMI2_E_COM_FAIL: - printf("Error [%d] : Communication failure\r\n", rslt); - break; - case BMI2_E_DEV_NOT_FOUND: - printf("Error [%d] : Device not found\r\n", rslt); - break; - case BMI2_E_INVALID_SENSOR: - printf("Error [%d] : Invalid sensor\r\n", rslt); - break; - case BMI2_E_SELF_TEST_FAIL: - printf("Warning [%d] : Self test failed\r\n", rslt); - break; - case BMI2_E_INVALID_INT_PIN: - printf("warning [%d] : invalid int pin\r\n", rslt); - break; - default: - printf("Error [%d] : Unknown error code\r\n", rslt); - break; - } -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/example/step_counter.c b/lib/main/BoschSensortec/BMI270-Sensor-API/example/step_counter.c deleted file mode 100644 index 9856fc32b..000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/example/step_counter.c +++ /dev/null @@ -1,238 +0,0 @@ -#include -#include "bmi2.h" -#include "bmi270.h" - -void delay_us(uint32_t period); -int8_t i2c_reg_write(uint8_t i2c_addr, uint8_t reg_addr, const uint8_t *reg_data, uint16_t length); -int8_t i2c_reg_read(uint8_t i2c_addr, uint8_t reg_addr, uint8_t *reg_data, uint16_t length); -int8_t spi_reg_write(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length); -int8_t spi_reg_read(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length); -void print_rslt(int8_t rslt); - -int main(void) -{ - struct bmi2_dev dev; - struct bmi2_sensor_data sensor_data; - struct bmi2_sens_config config; - - int8_t rslt; - - /* Sensor type of sensor to select sensor */ - uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_STEP_COUNTER }; - - /* For status */ - uint16_t step_status = 0; - - /* Select features and their pins to be mapped to */ - struct bmi2_sens_int_config sens_int = { .type = BMI2_STEP_COUNTER, .hw_int_pin = BMI2_INT2 }; - - /* Sensor type of sensor to get data */ - sensor_data.type = BMI2_STEP_COUNTER; - - dev.read = i2c_reg_read; - dev.write = i2c_reg_write; - dev.delay_us = delay_us; - dev.read_write_len = 128; - dev.intf = BMI2_I2C_INTERFACE; - dev.dev_id = BMI2_I2C_PRIM_ADDR; - - /* To enable SPI interface*/ - /* - * dev.read = spi_reg_read; - * dev.write = spi_reg_write; - * dev.delay_us = delay_us; - * dev.read_write_len = 4096; - * dev.intf = BMI2_SPI_INTERFACE; - * dev.dev_id = SPI_CS; - * dev.dummy_byte = 1; - */ - - /* Initialize by enabling firmware download */ - - dev.config_file_ptr = NULL; - - rslt = bmi270_init(&dev); - print_rslt(rslt); - - /* Enable the accelerometer and step-counter sensor */ - rslt = bmi2_sensor_enable(sensor_sel, 2, &dev); - print_rslt(rslt); - - /* Update the type of sensor for setting the configurations */ - config.type = BMI2_STEP_COUNTER; - - /* Get default configurations for the type of feature selected */ - rslt = bmi2_get_sensor_config(&config, 1, &dev); - print_rslt(rslt); - - /* Enable water-mark level for to get interrupt after 20 step counts */ - config.cfg.step_counter.watermark_level = 1; - - /* Set the configurations */ - rslt = bmi2_set_sensor_config(&config, 1, &dev); - print_rslt(rslt); - - /* Map the feature interrupt */ - rslt = bmi2_map_feat_int(&sens_int, 1, &dev); - print_rslt(rslt); - - printf("Move the board for step counter\n"); - - while (1) - { - /* Check the interrupt status of the step counter */ - rslt = bmi2_get_int_status(&step_status, &dev); - print_rslt(rslt); - if (rslt == BMI2_OK) - { - if (step_status == BMI270_STEP_CNT_STATUS_MASK) - { - printf("Step detected\n"); - - /* Get step counter output */ - rslt = bmi2_get_sensor_data(&sensor_data, 1, &dev); - print_rslt(rslt); - - /* Print the step counter output */ - printf("No of steps counted = %ld", sensor_data.sens_data.step_counter_output); - break; - } - } - dev.delay_us(50000); - } - - return 0; -} - -/*! - * @brief Function that creates a mandatory delay required in some of the APIs such as "bma4_write_config_file", - * "bma4_write_regs", "bma4_set_accel_config" and so on. - * - * @param[in] period_us : the required wait time in microseconds. - * @return void. - * - */ -void delay_us(uint32_t period) -{ - /* Wait for a period amount of us*/ -} - -/*! - * @brief Function for writing the sensor's registers through I2C bus. - * - * @param[in] i2c_addr : sensor I2C address. - * @param[in] reg_addr : Register address. - * @param[in] reg_data : Pointer to the data buffer whose value is to be written. - * @param[in] length : No of bytes to write. - * - * @return Status of execution - * @retval 0 -> Success - * @retval >0 -> Failure Info - * - */ -int8_t i2c_reg_write(uint8_t i2c_addr, uint8_t reg_addr, const uint8_t *reg_data, uint16_t length) -{ - - /* Write to registers using I2C. Return 0 for a successful execution. */ - return 0; -} - -/*! - * @brief Function for reading the sensor's registers through I2C bus. - * - * @param[in] i2c_addr : Sensor I2C address. - * @param[in] reg_addr : Register address. - * @param[out] reg_data : Pointer to the data buffer to store the read data. - * @param[in] length : No of bytes to read. - * - * @return Status of execution - * @retval 0 -> Success - * @retval >0 -> Failure Info - * - */ -int8_t i2c_reg_read(uint8_t i2c_addr, uint8_t reg_addr, uint8_t *reg_data, uint16_t length) -{ - - /* Read from registers using I2C. Return 0 for a successful execution. */ - return 0; -} - -/*! - * @brief Function for writing the sensor's registers through SPI bus. - * - * @param[in] cs : Chip select to enable the sensor. - * @param[in] reg_addr : Register address. - * @param[in] reg_data : Pointer to the data buffer whose data has to be written. - * @param[in] length : No of bytes to write. - * - * @return Status of execution - * @retval 0 -> Success - * @retval >0 -> Failure Info - * - */ -int8_t spi_reg_write(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length) -{ - - /* Write to registers using SPI. Return 0 for a successful execution. */ - return 0; -} - -/*! - * @brief Function for reading the sensor's registers through SPI bus. - * - * @param[in] cs : Chip select to enable the sensor. - * @param[in] reg_addr : Register address. - * @param[out] reg_data : Pointer to the data buffer to store the read data. - * @param[in] length : No of bytes to read. - * - * @return Status of execution - * @retval 0 -> Success - * @retval >0 -> Failure Info - * - */ -int8_t spi_reg_read(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length) -{ - - /* Read from registers using SPI. Return 0 for a successful execution. */ - return 0; -} - -/*! - * @brief Prints the execution status of the APIs. - * - * @param[in] api_name : name of the API whose execution status has to be printed. - * @param[in] rslt : error code returned by the API whose execution status has to be printed. - * - * @return void. - */ -void print_rslt(int8_t rslt) -{ - switch (rslt) - { - case BMI2_OK: - - /* Do nothing */ - break; - case BMI2_E_NULL_PTR: - printf("Error [%d] : Null pointer\r\n", rslt); - break; - case BMI2_E_COM_FAIL: - printf("Error [%d] : Communication failure\r\n", rslt); - break; - case BMI2_E_DEV_NOT_FOUND: - printf("Error [%d] : Device not found\r\n", rslt); - break; - case BMI2_E_INVALID_SENSOR: - printf("Error [%d] : Invalid sensor\r\n", rslt); - break; - case BMI2_E_SELF_TEST_FAIL: - printf("Warning [%d] : Self test failed\r\n", rslt); - break; - case BMI2_E_INVALID_INT_PIN: - printf("warning [%d] : invalid int pin\r\n", rslt); - break; - default: - printf("Error [%d] : Unknown error code\r\n", rslt); - break; - } -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/example/wrist_gesture.c b/lib/main/BoschSensortec/BMI270-Sensor-API/example/wrist_gesture.c deleted file mode 100644 index c70b25260..000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/example/wrist_gesture.c +++ /dev/null @@ -1,244 +0,0 @@ -#include -#include "bmi2.h" -#include "bmi270.h" - - -void delay_us(uint32_t period); -int8_t i2c_reg_write(uint8_t i2c_addr, uint8_t reg_addr, const uint8_t *reg_data, uint16_t length); -int8_t i2c_reg_read(uint8_t i2c_addr, uint8_t reg_addr, uint8_t *reg_data, uint16_t length); -int8_t spi_reg_write(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length); -int8_t spi_reg_read(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length); -void print_rslt(int8_t rslt); - -int main(void) -{ - - struct bmi2_dev dev; - - /* Variable to define rslt */ - int8_t rslt; - - /* Array to select sensors */ - uint8_t sens_list[2] = { BMI2_ACCEL, BMI2_WRIST_GESTURE }; - - /* Variable to get wrist gesture status */ - uint16_t wrist_gest_status = 0; - - /* Select features and their pins to be mapped to */ - struct bmi2_sens_int_config sens_int = { .type = BMI2_WRIST_GESTURE, .hw_int_pin = BMI2_INT1 }; - - /* Sensor configuration structure */ - struct bmi2_sens_config config = { 0 }; - - /* Sensor data structure */ - struct bmi2_sensor_data sens_data = { 0 }; - - /*To enable I2C interface*/ - dev.read = i2c_reg_read; - dev.write = i2c_reg_write; - dev.delay_us = delay_us; - dev.read_write_len = 128; - dev.intf = BMI2_I2C_INTERFACE; - dev.dev_id = BMI2_I2C_PRIM_ADDR; - - /* To enable SPI interface*/ - - /* - * dev.read = spi_reg_read; - * dev.write = spi_reg_write; - * dev.delay_us = delay_us; - * dev.read_write_len = 4096; - * dev.intf = BMI2_SPI_INTERFACE; - * dev.dev_id = SPI_CS; - * dev.dummy_byte = 1; - */ - - dev.config_file_ptr = NULL; - - /* Initialize BMI270 */ - rslt = bmi270_init(&dev); - print_rslt(rslt); - - /* Enable the selected sensors */ - rslt = bmi2_sensor_enable(sens_list, 2, &dev); - print_rslt(rslt); - - /* Configure type of feature */ - config.type = BMI2_WRIST_GESTURE; - - /* Get default configurations for the type of feature selected */ - rslt = bmi2_get_sensor_config(&config, 1, &dev); - print_rslt(rslt); - if (rslt == BMI2_OK) - { - /* Arm Position where the device is attached */ - config.cfg.wrist_gest.wearable_arm = BMI2_ARM_LEFT; - - /* Set the new configuration along with interrupt mapping */ - rslt = bmi2_set_sensor_config(&config, 1, &dev); - print_rslt(rslt); - } - - /* Map the feature interrupt */ - rslt = bmi2_map_feat_int(&sens_int, 1, &dev); - print_rslt(rslt); - printf("Flip the board in portrait landscape mode:\n"); - while (1) - { - /* Check the interrupt status of the wrist gesture */ - rslt = bmi2_get_int_status(&wrist_gest_status, &dev); - print_rslt(rslt); - if (rslt == BMI2_OK) - { - if (wrist_gest_status & BMI270_WRIST_GEST_STATUS_MASK) - { - printf("Wrist gesture detected\n"); - - /* Get wrist gesture output */ - rslt = bmi2_get_sensor_data(&sens_data, 1, &dev); - print_rslt(rslt); - - /* Print the wrist gesture output */ - printf("wrist gesture = %d\r\n", sens_data.sens_data.wrist_gesture_output); - break; - } - - dev.delay_us(100000); - } - - return 0; - } -} - -/*! - * @brief Function that creates a mandatory delay required in some of the APIs such as "bma4_write_config_file", - * "bma4_write_regs", "bma4_set_accel_config" and so on. - * - * @param[in] period_us : the required wait time in microseconds. - * @return void. - * - */ -void delay_us(uint32_t period) -{ - /* Wait for a period amount of us*/ -} - -/*! - * @brief Function for writing the sensor's registers through I2C bus. - * - * @param[in] i2c_addr : sensor I2C address. - * @param[in] reg_addr : Register address. - * @param[in] reg_data : Pointer to the data buffer whose value is to be written. - * @param[in] length : No of bytes to write. - * - * @return Status of execution - * @retval 0 -> Success - * @retval >0 -> Failure Info - * - */ -int8_t i2c_reg_write(uint8_t i2c_addr, uint8_t reg_addr, const uint8_t *reg_data, uint16_t length) -{ - - /* Write to registers using I2C. Return 0 for a successful execution. */ - return 0; -} - -/*! - * @brief Function for reading the sensor's registers through I2C bus. - * - * @param[in] i2c_addr : Sensor I2C address. - * @param[in] reg_addr : Register address. - * @param[out] reg_data : Pointer to the data buffer to store the read data. - * @param[in] length : No of bytes to read. - * - * @return Status of execution - * @retval 0 -> Success - * @retval >0 -> Failure Info - * - */ -int8_t i2c_reg_read(uint8_t i2c_addr, uint8_t reg_addr, uint8_t *reg_data, uint16_t length) -{ - - /* Read from registers using I2C. Return 0 for a successful execution. */ - return 0; -} - -/*! - * @brief Function for writing the sensor's registers through SPI bus. - * - * @param[in] cs : Chip select to enable the sensor. - * @param[in] reg_addr : Register address. - * @param[in] reg_data : Pointer to the data buffer whose data has to be written. - * @param[in] length : No of bytes to write. - * - * @return Status of execution - * @retval 0 -> Success - * @retval >0 -> Failure Info - * - */ -int8_t spi_reg_write(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length) -{ - - /* Write to registers using SPI. Return 0 for a successful execution. */ - return 0; -} - -/*! - * @brief Function for reading the sensor's registers through SPI bus. - * - * @param[in] cs : Chip select to enable the sensor. - * @param[in] reg_addr : Register address. - * @param[out] reg_data : Pointer to the data buffer to store the read data. - * @param[in] length : No of bytes to read. - * - * @return Status of execution - * @retval 0 -> Success - * @retval >0 -> Failure Info - * - */ -int8_t spi_reg_read(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length) -{ - - /* Read from registers using SPI. Return 0 for a successful execution. */ - return 0; -} - -/*! - * @brief Prints the execution status of the APIs. - * - * @param[in] api_name : name of the API whose execution status has to be printed. - * @param[in] rslt : error code returned by the API whose execution status has to be printed. - * - * @return void. - */ -void print_rslt(int8_t rslt) -{ - switch (rslt) - { - case BMI2_OK: - - /* Do nothing */ - break; - case BMI2_E_NULL_PTR: - printf("Error [%d] : Null pointer\r\n", rslt); - break; - case BMI2_E_COM_FAIL: - printf("Error [%d] : Communication failure\r\n", rslt); - break; - case BMI2_E_DEV_NOT_FOUND: - printf("Error [%d] : Device not found\r\n", rslt); - break; - case BMI2_E_INVALID_SENSOR: - printf("Error [%d] : Invalid sensor\r\n", rslt); - break; - case BMI2_E_SELF_TEST_FAIL: - printf("Warning [%d] : Self test failed\r\n", rslt); - break; - case BMI2_E_INVALID_INT_PIN: - printf("warning [%d] : invalid int pin\r\n", rslt); - break; - default: - printf("Error [%d] : Unknown error code\r\n", rslt); - break; - } -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/example/wrist_wear_wakeup.c b/lib/main/BoschSensortec/BMI270-Sensor-API/example/wrist_wear_wakeup.c deleted file mode 100644 index 82410619a..000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/example/wrist_wear_wakeup.c +++ /dev/null @@ -1,235 +0,0 @@ -#include -#include "bmi2.h" -#include "bmi270.h" - -void delay_us(uint32_t period); -int8_t i2c_reg_write(uint8_t i2c_addr, uint8_t reg_addr, const uint8_t *reg_data, uint16_t length); -int8_t i2c_reg_read(uint8_t i2c_addr, uint8_t reg_addr, uint8_t *reg_data, uint16_t length); -int8_t spi_reg_write(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length); -int8_t spi_reg_read(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length); -void print_rslt(int8_t rslt); - -int main(void) -{ - struct bmi2_dev dev; - - /* Variable to define rslt */ - int8_t rslt; - - /* Array to select sensors */ - uint8_t sens_list[2] = { BMI2_ACCEL, BMI2_WRIST_WEAR_WAKE_UP }; - - /* Variable to get wrist gesture status */ - uint16_t wrist_wear_wakeup_status = 0; - - /* Select features and their pins to be mapped to */ - struct bmi2_sens_int_config sens_int = { .type = BMI2_WRIST_WEAR_WAKE_UP, .hw_int_pin = BMI2_INT1 }; - - /* Sensor configuration structure */ - struct bmi2_sens_config config = { 0 }; - - /* Sensor data structure */ - struct bmi2_sensor_data sens_data = { 0 }; - - dev.read = i2c_reg_read; - dev.write = i2c_reg_write; - dev.delay_us = delay_us; - dev.read_write_len = 128; - dev.intf = BMI2_I2C_INTERFACE; - dev.dev_id = BMI2_I2C_PRIM_ADDR; - - /* To enable SPI interface*/ - /* dev.read = spi_reg_read; - * dev.write = spi_reg_write; - * dev.delay_us = delay_us; - * dev.read_write_len = 4096; - * dev.intf = BMI2_SPI_INTERFACE; - * dev.dev_id = SPI_CS; - * dev.dummy_byte = 1; - */ - dev.config_file_ptr = NULL; - - /* Initialize BMI2 */ - rslt = bmi270_init(&dev); - print_rslt(rslt); - - /* Enable the selected sensors */ - rslt = bmi2_sensor_enable(sens_list, 2, &dev); - print_rslt(rslt); - - /* Configure type of feature */ - config.type = BMI2_WRIST_WEAR_WAKE_UP; - - /* Get default configurations for the type of feature selected */ - rslt = bmi2_get_sensor_config(&config, 1, &dev); - print_rslt(rslt); - if (rslt == BMI2_OK) - { - /* Set the new configuration along with interrupt mapping */ - rslt = bmi2_set_sensor_config(&config, 1, &dev); - print_rslt(rslt); - } - - /* Map the feature interrupt */ - rslt = bmi2_map_feat_int(&sens_int, 1, &dev); - print_rslt(rslt); - printf("Lift the board in portrait landscape position and tilt in a particular direction\n"); - while (1) - { - /* Check the interrupt status of the wrist gesture */ - rslt = bmi2_get_int_status(&wrist_wear_wakeup_status, &dev); - print_rslt(rslt); - if (rslt == BMI2_OK) - { - if (wrist_wear_wakeup_status & BMI270_WRIST_WAKE_UP_STATUS_MASK) - { - printf("Wrist wear wakeup detected\n"); - - /* Get wrist gesture output */ - rslt = bmi2_get_sensor_data(&sens_data, 1, &dev); - print_rslt(rslt); - - /* Print the wrist gesture output */ - printf("wrist gesture = %d\r\n", sens_data.sens_data.wrist_gesture_output); - break; - } - - dev.delay_us(100000); - } - - return 0; - } -} - -/*! - * @brief Function that creates a mandatory delay required in some of the APIs such as "bma4_write_config_file", - * "bma4_write_regs", "bma4_set_accel_config" and so on. - * - * @param[in] period_us : the required wait time in microseconds. - * @return void. - * - */ -void delay_us(uint32_t period) -{ - /* Wait for a period amount of ms*/ -} - -/*! - * @brief Function for writing the sensor's registers through I2C bus. - * - * @param[in] i2c_addr : sensor I2C address. - * @param[in] reg_addr : Register address. - * @param[in] reg_data : Pointer to the data buffer whose value is to be written. - * @param[in] length : No of bytes to write. - * - * @return Status of execution - * @retval 0 -> Success - * @retval >0 -> Failure Info - * - */ -int8_t i2c_reg_write(uint8_t i2c_addr, uint8_t reg_addr, const uint8_t *reg_data, uint16_t length) -{ - - /* Write to registers using I2C. Return 0 for a successful execution. */ - return 0; -} - -/*! - * @brief Function for reading the sensor's registers through I2C bus. - * - * @param[in] i2c_addr : Sensor I2C address. - * @param[in] reg_addr : Register address. - * @param[out] reg_data : Pointer to the data buffer to store the read data. - * @param[in] length : No of bytes to read. - * - * @return Status of execution - * @retval 0 -> Success - * @retval >0 -> Failure Info - * - */ -int8_t i2c_reg_read(uint8_t i2c_addr, uint8_t reg_addr, uint8_t *reg_data, uint16_t length) -{ - - /* Read from registers using I2C. Return 0 for a successful execution. */ - return 0; -} - -/*! - * @brief Function for writing the sensor's registers through SPI bus. - * - * @param[in] cs : Chip select to enable the sensor. - * @param[in] reg_addr : Register address. - * @param[in] reg_data : Pointer to the data buffer whose data has to be written. - * @param[in] length : No of bytes to write. - * - * @return Status of execution - * @retval 0 -> Success - * @retval >0 -> Failure Info - * - */ -int8_t spi_reg_write(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length) -{ - - /* Write to registers using SPI. Return 0 for a successful execution. */ - return 0; -} - -/*! - * @brief Function for reading the sensor's registers through SPI bus. - * - * @param[in] cs : Chip select to enable the sensor. - * @param[in] reg_addr : Register address. - * @param[out] reg_data : Pointer to the data buffer to store the read data. - * @param[in] length : No of bytes to read. - * - * @return Status of execution - * @retval 0 -> Success - * @retval >0 -> Failure Info - * - */ -int8_t spi_reg_read(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length) -{ - - /* Read from registers using SPI. Return 0 for a successful execution. */ - return 0; -} - -/*! - * @brief Prints the execution status of the APIs. - * - * @param[in] api_name : name of the API whose execution status has to be printed. - * @param[in] rslt : error code returned by the API whose execution status has to be printed. - * - * @return void. - */ -void print_rslt(int8_t rslt) -{ - switch (rslt) - { - case BMI2_OK: - - /* Do nothing */ - break; - case BMI2_E_NULL_PTR: - printf("Error [%d] : Null pointer\r\n", rslt); - break; - case BMI2_E_COM_FAIL: - printf("Error [%d] : Communication failure\r\n", rslt); - break; - case BMI2_E_DEV_NOT_FOUND: - printf("Error [%d] : Device not found\r\n", rslt); - break; - case BMI2_E_INVALID_SENSOR: - printf("Error [%d] : Invalid sensor\r\n", rslt); - break; - case BMI2_E_SELF_TEST_FAIL: - printf("Warning [%d] : Self test failed\r\n", rslt); - break; - case BMI2_E_INVALID_INT_PIN: - printf("warning [%d] : invalid int pin\r\n", rslt); - break; - default: - printf("Error [%d] : Unknown error code\r\n", rslt); - break; - } -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel/Makefile new file mode 100644 index 000000000..c63d602ce --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= accel.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel/accel.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel/accel.c new file mode 100644 index 000000000..ea385d744 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel/accel.c @@ -0,0 +1,200 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270.h" +#include "common.h" + +/******************************************************************************/ +/*! Macro definition */ + +/*! Earth's gravity in m/s^2 */ +#define GRAVITY_EARTH (9.80665f) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_accel_config(struct bmi2_dev *bmi2_dev); + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + * + * @param[in] val : LSB from each axis. + * @param[in] g_range : Gravity range. + * @param[in] bit_width : Resolution for accel. + * + * @return Gravity. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to define limit to print accel data. */ + uint8_t limit = 20; + + /* Assign accel sensor to variable. */ + uint8_t sensor_list = BMI2_ACCEL; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Create an instance of sensor data structure. */ + struct bmi2_sensor_data sensor_data = { 0 }; + + /* Initialize the interrupt status of accel. */ + uint16_t int_status = 0; + + uint8_t indx = 0; + float x = 0, y = 0, z = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the accel sensor. */ + rslt = bmi270_sensor_enable(&sensor_list, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Accel configuration settings. */ + rslt = set_accel_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Assign accel sensor. */ + sensor_data.type = BMI2_ACCEL; + printf("Accel and m/s2 data \n"); + printf("Accel data collected at 2G Range with 16-bit resolution\n"); + + /* Loop to print the accel data when interrupt occurs. */ + while (indx <= limit) + { + /* To get the status of accel data ready interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the accel data ready interrupt status and print the status for 10 samples. */ + if (int_status & BMI2_ACC_DRDY_INT_MASK) + { + /* Get accelerometer data for x, y and z axis. */ + rslt = bmi270_get_sensor_data(&sensor_data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + printf("\nAcc_X = %d\t", sensor_data.sens_data.acc.x); + printf("Acc_Y = %d\t", sensor_data.sens_data.acc.y); + printf("Acc_Z = %d\r\n", sensor_data.sens_data.acc.z); + + /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ + x = lsb_to_mps2(sensor_data.sens_data.acc.x, 2, bmi2_dev.resolution); + y = lsb_to_mps2(sensor_data.sens_data.acc.y, 2, bmi2_dev.resolution); + z = lsb_to_mps2(sensor_data.sens_data.acc.z, 2, bmi2_dev.resolution); + + /* Print the data in m/s2. */ + printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); + + indx++; + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel. + */ +static int8_t set_accel_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accelerometer configuration. */ + struct bmi2_sens_config config; + + /* Configure the type of feature. */ + config.type = BMI2_ACCEL; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_get_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config.cfg.acc.odr = BMI2_ACC_ODR_200HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config.cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples. + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config.cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config.cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set the accel configurations. */ + rslt = bmi270_set_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Map data ready interrupt to interrupt pin. */ + rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (GRAVITY_EARTH * val * g_range) / half_scale; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel_gyro/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel_gyro/Makefile new file mode 100644 index 000000000..c0617d02e --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel_gyro/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= accel_gyro.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel_gyro/accel_gyro.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel_gyro/accel_gyro.c new file mode 100644 index 000000000..1e7c9dfc0 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel_gyro/accel_gyro.c @@ -0,0 +1,268 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270.h" +#include "common.h" + +/******************************************************************************/ +/*! Macro definition */ + +/*! Earth's gravity in m/s^2 */ +#define GRAVITY_EARTH (9.80665f) + +/*! Macros to select the sensors */ +#define ACCEL UINT8_C(0x00) +#define GYRO UINT8_C(0x01) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev); + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + * + * @param[in] val : LSB from each axis. + * @param[in] g_range : Gravity range. + * @param[in] bit_width : Resolution for accel. + * + * @return Gravity. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + * + * @param[in] val : LSB from each axis. + * @param[in] dps : Degree per second. + * @param[in] bit_width : Resolution for gyro. + * + * @return Degree per second. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to define limit to print accel data. */ + uint8_t limit = 10; + + /* Assign accel and gyro sensor to variable. */ + uint8_t sensor_list[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Create an instance of sensor data structure. */ + struct bmi2_sensor_data sensor_data[2] = { { 0 } }; + + /* Initialize the interrupt status of accel and gyro. */ + uint16_t int_status = 0; + + uint8_t indx = 1; + + float x = 0, y = 0, z = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_SPI_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the accel and gyro sensor. */ + rslt = bmi270_sensor_enable(sensor_list, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Accel and gyro configuration settings. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Assign accel and gyro sensor. */ + sensor_data[ACCEL].type = BMI2_ACCEL; + sensor_data[GYRO].type = BMI2_GYRO; + + /* Loop to print accel and gyro data when interrupt occurs. */ + while (indx <= limit) + { + /* To get the data ready interrupt status of accel and gyro. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the data ready interrupt status and print the status for 10 samples. */ + if ((int_status & BMI2_ACC_DRDY_INT_MASK) && (int_status & BMI2_GYR_DRDY_INT_MASK)) + { + /* Get accel and gyro data for x, y and z axis. */ + rslt = bmi270_get_sensor_data(sensor_data, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("\n******* Accel(Raw and m/s2) Gyro(Raw and dps) data : %d *******\n", indx); + + printf("\nAcc_x = %d\t", sensor_data[ACCEL].sens_data.acc.x); + printf("Acc_y = %d\t", sensor_data[ACCEL].sens_data.acc.y); + printf("Acc_z = %d", sensor_data[ACCEL].sens_data.acc.z); + + /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ + x = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.x, 2, bmi2_dev.resolution); + y = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.y, 2, bmi2_dev.resolution); + z = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.z, 2, bmi2_dev.resolution); + + /* Print the data in m/s2. */ + printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); + + printf("\nGyr_X = %d\t", sensor_data[GYRO].sens_data.gyr.x); + printf("Gyr_Y = %d\t", sensor_data[GYRO].sens_data.gyr.y); + printf("Gyr_Z= %d\n", sensor_data[GYRO].sens_data.gyr.z); + + /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ + x = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.x, 2000, bmi2_dev.resolution); + y = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.y, 2000, bmi2_dev.resolution); + z = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.z, 2000, bmi2_dev.resolution); + + /* Print the data in dps. */ + printf("Gyro_DPS_X = %4.2f, Gyro_DPS_Y = %4.2f, Gyro_DPS_Z = %4.2f\n", x, y, z); + + indx++; + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accelerometer and gyro configuration. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[ACCEL].type = BMI2_ACCEL; + config[GYRO].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_get_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Map data ready interrupt to interrupt pin. */ + rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config[ACCEL].cfg.acc.odr = BMI2_ACC_ODR_200HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[ACCEL].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples. + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[ACCEL].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[ACCEL].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config[GYRO].cfg.gyr.odr = BMI2_GYR_ODR_200HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[GYRO].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[GYRO].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[GYRO].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[GYRO].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set the accel and gyro configurations. */ + rslt = bmi270_set_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (GRAVITY_EARTH * val * g_range) / half_scale; +} + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/any_motion_interrupt/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/any_motion_interrupt/Makefile new file mode 100644 index 000000000..9158f0577 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/any_motion_interrupt/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= any_motion_interrupt.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/any_motion_interrupt/any_motion_interrupt.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/any_motion_interrupt/any_motion_interrupt.c new file mode 100644 index 000000000..062b29eb3 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/any_motion_interrupt/any_motion_interrupt.c @@ -0,0 +1,135 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270.h" +#include "common.h" + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for any-motion. + * + * @param[in] bmi2_dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_feature_config(struct bmi2_dev *bmi2_dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Accel sensor and any-motion feature are listed in array. */ + uint8_t sens_list[2] = { BMI2_ACCEL, BMI2_ANY_MOTION }; + + /* Variable to get any-motion interrupt status. */ + uint16_t int_status = 0; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Select features and their pins to be mapped to. */ + struct bmi2_sens_int_config sens_int = { .type = BMI2_ANY_MOTION, .hw_int_pin = BMI2_INT1 }; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the selected sensors. */ + rslt = bmi270_sensor_enable(sens_list, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Set feature configurations for any-motion. */ + rslt = set_feature_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Map the feature interrupt for any-motion. */ + rslt = bmi270_map_feat_int(&sens_int, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + printf("Move the board\n"); + + /* Loop to get any-motion interrupt. */ + do + { + /* Clear buffer. */ + int_status = 0; + + /* To get the interrupt status of any-motion. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the interrupt status of any-motion. */ + if (int_status & BMI270_ANY_MOT_STATUS_MASK) + { + printf("Any-motion interrupt is generated\n"); + break; + } + } while (rslt == BMI2_OK); + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for any-motion. + */ +static int8_t set_feature_config(struct bmi2_dev *bmi2_dev) +{ + + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define the type of sensor and its configurations. */ + struct bmi2_sens_config config; + + /* Configure the type of feature. */ + config.type = BMI2_ANY_MOTION; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_get_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* 1LSB equals 20ms. Default is 100ms, setting to 80ms. */ + config.cfg.any_motion.duration = 0x04; + + /* 1LSB equals to 0.48mg. Default is 83mg, setting to 50mg. */ + config.cfg.any_motion.threshold = 0x68; + + /* Set new configurations. */ + rslt = bmi270_set_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/common/common.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/common/common.c new file mode 100644 index 000000000..afff37bc8 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/common/common.c @@ -0,0 +1,372 @@ +/** + * Copyright (C) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +#include +#include +#include + +#include "coines.h" +#include "bmi2_defs.h" + +/******************************************************************************/ +/*! Macro definitions */ +#define BMI2XY_SHUTTLE_ID UINT16_C(0x1B8) + +/*! Macro that defines read write length */ +#define READ_WRITE_LEN UINT8_C(46) + +/******************************************************************************/ +/*! Static variable definition */ + +/*! Variable that holds the I2C device address or SPI chip selection */ +static uint8_t dev_addr; + +/******************************************************************************/ +/*! User interface functions */ + +/*! + * I2C read function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_read_i2c(dev_addr, reg_addr, reg_data, (uint16_t)len); +} + +/*! + * I2C write function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_write_i2c(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len); +} + +/*! + * SPI read function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_read_spi(dev_addr, reg_addr, reg_data, (uint16_t)len); +} + +/*! + * SPI write function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_write_spi(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len); +} + +/*! + * Delay function map to COINES platform + */ +void bmi2_delay_us(uint32_t period, void *intf_ptr) +{ + coines_delay_usec(period); +} + +/*! + * @brief Function to select the interface between SPI and I2C. + * Also to initialize coines platform + */ +int8_t bmi2_interface_init(struct bmi2_dev *bmi, uint8_t intf) +{ + int8_t rslt = BMI2_OK; + struct coines_board_info board_info; + + if (bmi != NULL) + { + int16_t result = coines_open_comm_intf(COINES_COMM_INTF_USB); + if (result < COINES_SUCCESS) + { + printf( + "\n Unable to connect with Application Board ! \n" " 1. Check if the board is connected and powered on. \n" " 2. Check if Application Board USB driver is installed. \n" + " 3. Check if board is in use by another application. (Insufficient permissions to access USB) \n"); + exit(result); + } + + result = coines_get_board_info(&board_info); + +#if defined(PC) + setbuf(stdout, NULL); +#endif + + if (result == COINES_SUCCESS) + { + if ((board_info.shuttle_id != BMI2XY_SHUTTLE_ID)) + { + printf("! Warning invalid sensor shuttle \n ," "This application will not support this sensor \n"); + exit(COINES_E_FAILURE); + } + } + + coines_set_shuttleboard_vdd_vddio_config(0, 0); + coines_delay_msec(100); + + /* Bus configuration : I2C */ + if (intf == BMI2_I2C_INTF) + { + printf("I2C Interface \n"); + + /* To initialize the user I2C function */ + dev_addr = BMI2_I2C_PRIM_ADDR; + bmi->intf = BMI2_I2C_INTF; + bmi->read = bmi2_i2c_read; + bmi->write = bmi2_i2c_write; + coines_config_i2c_bus(COINES_I2C_BUS_0, COINES_I2C_FAST_MODE); + } + /* Bus configuration : SPI */ + else if (intf == BMI2_SPI_INTF) + { + printf("SPI Interface \n"); + + /* To initialize the user SPI function */ + dev_addr = COINES_SHUTTLE_PIN_7; + bmi->intf = BMI2_SPI_INTF; + bmi->read = bmi2_spi_read; + bmi->write = bmi2_spi_write; + coines_config_spi_bus(COINES_SPI_BUS_0, COINES_SPI_SPEED_5_MHZ, COINES_SPI_MODE3); + coines_set_pin_config(COINES_SHUTTLE_PIN_7, COINES_PIN_DIRECTION_OUT, COINES_PIN_VALUE_HIGH); + } + + /* Assign device address to interface pointer */ + bmi->intf_ptr = &dev_addr; + + /* Configure delay in microseconds */ + bmi->delay_us = bmi2_delay_us; + + /* Configure max read/write length (in bytes) ( Supported length depends on target machine) */ + bmi->read_write_len = READ_WRITE_LEN; + + /* Assign to NULL to load the default config file. */ + bmi->config_file_ptr = NULL; + + coines_delay_usec(10000); + + coines_set_shuttleboard_vdd_vddio_config(3300, 3300); + + coines_delay_usec(10000); + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; + +} + +/*! + * @brief Prints the execution status of the APIs. + */ +void bmi2_error_codes_print_result(int8_t rslt) +{ + switch (rslt) + { + case BMI2_OK: + + /* Do nothing */ + break; + + case BMI2_W_FIFO_EMPTY: + printf("Warning [%d] : FIFO empty\r\n", rslt); + break; + case BMI2_W_PARTIAL_READ: + printf("Warning [%d] : FIFO partial read\r\n", rslt); + break; + case BMI2_E_NULL_PTR: + printf( + "Error [%d] : Null pointer error. It occurs when the user tries to assign value (not address) to a pointer," " which has been initialized to NULL.\r\n", + rslt); + break; + + case BMI2_E_COM_FAIL: + printf( + "Error [%d] : Communication failure error. It occurs due to read/write operation failure and also due " "to power failure during communication\r\n", + rslt); + break; + + case BMI2_E_DEV_NOT_FOUND: + printf("Error [%d] : Device not found error. It occurs when the device chip id is incorrectly read\r\n", + rslt); + break; + + case BMI2_E_INVALID_SENSOR: + printf( + "Error [%d] : Invalid sensor error. It occurs when there is a mismatch in the requested feature with the " "available one\r\n", + rslt); + break; + + case BMI2_E_SELF_TEST_FAIL: + printf( + "Error [%d] : Self-test failed error. It occurs when the validation of accel self-test data is " "not satisfied\r\n", + rslt); + break; + + case BMI2_E_INVALID_INT_PIN: + printf( + "Error [%d] : Invalid interrupt pin error. It occurs when the user tries to configure interrupt pins " "apart from INT1 and INT2\r\n", + rslt); + break; + + case BMI2_E_OUT_OF_RANGE: + printf( + "Error [%d] : Out of range error. It occurs when the data exceeds from filtered or unfiltered data from " "fifo and also when the range exceeds the maximum range for accel and gyro while performing FOC\r\n", + rslt); + break; + + case BMI2_E_ACC_INVALID_CFG: + printf( + "Error [%d] : Invalid Accel configuration error. It occurs when there is an error in accel configuration" " register which could be one among range, BW or filter performance in reg address 0x40\r\n", + rslt); + break; + + case BMI2_E_GYRO_INVALID_CFG: + printf( + "Error [%d] : Invalid Gyro configuration error. It occurs when there is a error in gyro configuration" "register which could be one among range, BW or filter performance in reg address 0x42\r\n", + rslt); + break; + + case BMI2_E_ACC_GYR_INVALID_CFG: + printf( + "Error [%d] : Invalid Accel-Gyro configuration error. It occurs when there is a error in accel and gyro" " configuration registers which could be one among range, BW or filter performance in reg address 0x40 " "and 0x42\r\n", + rslt); + break; + + case BMI2_E_CONFIG_LOAD: + printf( + "Error [%d] : Configuration load error. It occurs when failure observed while loading the configuration " "into the sensor\r\n", + rslt); + break; + + case BMI2_E_INVALID_PAGE: + printf( + "Error [%d] : Invalid page error. It occurs due to failure in writing the correct feature configuration " "from selected page\r\n", + rslt); + break; + + case BMI2_E_SET_APS_FAIL: + printf( + "Error [%d] : APS failure error. It occurs due to failure in write of advance power mode configuration " "register\r\n", + rslt); + break; + + case BMI2_E_AUX_INVALID_CFG: + printf( + "Error [%d] : Invalid AUX configuration error. It occurs when the auxiliary interface settings are not " "enabled properly\r\n", + rslt); + break; + + case BMI2_E_AUX_BUSY: + printf( + "Error [%d] : AUX busy error. It occurs when the auxiliary interface buses are engaged while configuring" " the AUX\r\n", + rslt); + break; + + case BMI2_E_REMAP_ERROR: + printf( + "Error [%d] : Remap error. It occurs due to failure in assigning the remap axes data for all the axes " "after change in axis position\r\n", + rslt); + break; + + case BMI2_E_GYR_USER_GAIN_UPD_FAIL: + printf( + "Error [%d] : Gyro user gain update fail error. It occurs when the reading of user gain update status " "fails\r\n", + rslt); + break; + + case BMI2_E_SELF_TEST_NOT_DONE: + printf( + "Error [%d] : Self-test not done error. It occurs when the self-test process is ongoing or not " "completed\r\n", + rslt); + break; + + case BMI2_E_INVALID_INPUT: + printf("Error [%d] : Invalid input error. It occurs when the sensor input validity fails\r\n", rslt); + break; + + case BMI2_E_INVALID_STATUS: + printf("Error [%d] : Invalid status error. It occurs when the feature/sensor validity fails\r\n", rslt); + break; + + case BMI2_E_CRT_ERROR: + printf("Error [%d] : CRT error. It occurs when the CRT test has failed\r\n", rslt); + break; + + case BMI2_E_ST_ALREADY_RUNNING: + printf( + "Error [%d] : Self-test already running error. It occurs when the self-test is already running and " "another has been initiated\r\n", + rslt); + break; + + case BMI2_E_CRT_READY_FOR_DL_FAIL_ABORT: + printf( + "Error [%d] : CRT ready for download fail abort error. It occurs when download in CRT fails due to wrong " "address location\r\n", + rslt); + break; + + case BMI2_E_DL_ERROR: + printf( + "Error [%d] : Download error. It occurs when write length exceeds that of the maximum burst length\r\n", + rslt); + break; + + case BMI2_E_PRECON_ERROR: + printf( + "Error [%d] : Pre-conditional error. It occurs when precondition to start the feature was not " "completed\r\n", + rslt); + break; + + case BMI2_E_ABORT_ERROR: + printf("Error [%d] : Abort error. It occurs when the device was shaken during CRT test\r\n", rslt); + break; + + case BMI2_E_WRITE_CYCLE_ONGOING: + printf( + "Error [%d] : Write cycle ongoing error. It occurs when the write cycle is already running and another " "has been initiated\r\n", + rslt); + break; + + case BMI2_E_ST_NOT_RUNING: + printf( + "Error [%d] : Self-test is not running error. It occurs when self-test running is disabled while it's " "running\r\n", + rslt); + break; + + case BMI2_E_DATA_RDY_INT_FAILED: + printf( + "Error [%d] : Data ready interrupt error. It occurs when the sample count exceeds the FOC sample limit " "and data ready status is not updated\r\n", + rslt); + break; + + case BMI2_E_INVALID_FOC_POSITION: + printf( + "Error [%d] : Invalid FOC position error. It occurs when average FOC data is obtained for the wrong" " axes\r\n", + rslt); + break; + + default: + printf("Error [%d] : Unknown error code\r\n", rslt); + break; + } +} + +/*! + * @brief Deinitializes coines platform + * + * @return void. + */ +void bmi2_coines_deinit(void) +{ + coines_close_comm_intf(COINES_COMM_INTF_USB); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/common/common.h b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/common/common.h new file mode 100644 index 000000000..763983da4 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/common/common.h @@ -0,0 +1,122 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +#ifndef _COMMON_H +#define _COMMON_H + +/*! CPP guard */ +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include "bmi2.h" + +/*! + * @brief Function for reading the sensor's registers through I2C bus. + * + * @param[in] reg_addr : Register address. + * @param[out] reg_data : Pointer to the data buffer to store the read data. + * @param[in] length : No of bytes to read. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief Function for writing the sensor's registers through I2C bus. + * + * @param[in] reg_addr : Register address. + * @param[in] reg_data : Pointer to the data buffer whose value is to be written. + * @param[in] length : No of bytes to write. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief Function for reading the sensor's registers through SPI bus. + * + * @param[in] reg_addr : Register address. + * @param[out] reg_data : Pointer to the data buffer to store the read data. + * @param[in] length : No of bytes to read. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief Function for writing the sensor's registers through SPI bus. + * + * @param[in] reg_addr : Register address. + * @param[in] reg_data : Pointer to the data buffer whose data has to be written. + * @param[in] length : No of bytes to write. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief This function provides the delay for required time (Microsecond) as per the input provided in some of the + * APIs. + * + * @param[in] period_us : The required wait time in microsecond. + * @param[in] intf_ptr : Interface pointer + * + * @return void. + * + */ +void bmi2_delay_us(uint32_t period, void *intf_ptr); + +/*! + * @brief Function to select the interface between SPI and I2C. + * + * @param[in] bma : Structure instance of bmi2_dev + * @param[in] intf : Interface selection parameter + * + * @return Status of execution + * @retval 0 -> Success + * @retval < 0 -> Failure Info + */ +int8_t bmi2_interface_init(struct bmi2_dev *bma, uint8_t intf); + +/*! + * @brief Prints the execution status of the APIs. + * + * @param[in] rslt : Error code returned by the API whose execution status has to be printed. + * + * @return void. + */ +void bmi2_error_codes_print_result(int8_t rslt); + +/*! + * @brief Deinitializes coines platform + * + * @return void. + */ +void bmi2_coines_deinit(void); + +#ifdef __cplusplus +} +#endif /* End of CPP guard */ + +#endif /* _COMMON_H */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/component_retrim/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/component_retrim/Makefile new file mode 100644 index 000000000..aad6a0b87 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/component_retrim/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= component_retrim.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/component_retrim/component_retrim.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/component_retrim/component_retrim.c new file mode 100644 index 000000000..d85c148d2 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/component_retrim/component_retrim.c @@ -0,0 +1,52 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270.h" +#include "common.h" + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* This API runs the CRT process. */ + rslt = bmi2_do_crt(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Do not move the board while doing CRT. If so, it will throw an abort error. */ + if (rslt == BMI2_OK) + { + printf("CRT successfully completed."); + } + } + + bmi2_coines_deinit(); + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_header_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_header_mode/Makefile new file mode 100644 index 000000000..83a8f49ff --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_header_mode/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= fifo_full_header_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_header_mode/fifo_full_header_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_header_mode/fifo_full_header_mode.c new file mode 100644 index 000000000..2118c241d --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_header_mode/fifo_full_header_mode.c @@ -0,0 +1,315 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270.h" +#include "common.h" + +/******************************************************************************/ +/*! Macros */ + +/*! Buffer size allocated to store raw FIFO data. */ +#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) + +/*! Length of data to be read from FIFO. */ +#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) + +/*! Number of accel frames to be extracted from FIFO. */ + +/*! Calculation for frame count: Total frame count = Fifo buffer size(2048)/ Total frames(6 Accel, 6 Gyro and 1 header, + * totaling to 13) which equals to 157. + */ +#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(157) + +/*! Number of gyro frames to be extracted from FIFO. */ +#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(157) + +/*! Macro to read sensortime byte in FIFO. */ +#define SENSORTIME_OVERHEAD_BYTE UINT8_C(3) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + * @param[in] dev : Structure instance of bmi2_dev. + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev); + +/******************************************************************************/ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + uint16_t index = 0; + uint16_t fifo_length = 0; + uint16_t config = 0; + + /* To read sensortime, extra 3 bytes are added to fifo buffer. */ + uint16_t fifo_buffer_size = BMI2_FIFO_RAW_DATA_BUFFER_SIZE + SENSORTIME_OVERHEAD_BYTE; + + /* Variable to get fifo full interrupt status. */ + uint16_t int_status = 0; + + uint16_t accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + + uint16_t gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + /* Variable to store the available frame length count in FIFO */ + uint8_t frame_count; + + int8_t try = 1; + + /* Number of bytes of FIFO data. */ + uint8_t fifo_data[fifo_buffer_size]; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Array of accelerometer frames -> Total bytes = + * 157 * (6 axes + 1 header bytes) = 1099 bytes */ + struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; + + /* Array of gyro frames -> Total bytes = + * 157 * (6 axes + 1 header bytes) = 1099 bytes */ + struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; + + /* Initialize FIFO frame structure. */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Accel and gyro sensor are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable accel and gyro sensor. */ + rslt = bmi270_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configuration settings for accel and gyro. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Before setting FIFO, disable the advance power save mode. */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_get_fifo_config(&config, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Update FIFO structure. */ + /* Mapping the buffer to store the fifo data. */ + fifoframe.data = fifo_data; + + /* Length of FIFO frame. */ + /* To read sensortime, extra 3 bytes are added to fifo user length. */ + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH + SENSORTIME_OVERHEAD_BYTE; + + /* Set FIFO configuration by enabling accel, gyro and timestamp. + * NOTE 1: The header mode is enabled by default. + * NOTE 2: By default the FIFO operating mode is in FIFO mode. + * NOTE 3: Sensortime is enabled by default */ + printf("FIFO is configured in header mode\n"); + rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Map FIFO full interrupt. */ + fifoframe.data_int_map = BMI2_FFULL_INT; + rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + while (try <= 10) + { + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if ((rslt == BMI2_OK) && (int_status & BMI2_FFULL_INT_STATUS_MASK)) + { + printf("\nIteration : %d\n", try); + + accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + + gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("\nFIFO data bytes available : %d \n", fifo_length); + printf("\nFIFO data bytes requested : %d \n", fifoframe.length); + + /* Read FIFO data. */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nFIFO accel frames requested : %d \n", accel_frame_length); + + /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ + rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); + + printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); + + /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ + rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); + + /* Calculating the frame count from the available bytes in FIFO + * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len + header_byte)) */ + frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH + 1)); + printf("\nAvailable frame count from available bytes: %d\n", frame_count); + + printf("\nExtracted accel frames\n"); + + if (accel_frame_length > frame_count) + { + accel_frame_length = frame_count; + } + + /* Print the parsed accelerometer data from the FIFO buffer. */ + for (index = 0; index < accel_frame_length; index++) + { + printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, + fifo_accel_data[index].y, fifo_accel_data[index].z); + } + + printf("\nExtracted gyro frames\n"); + + if (gyro_frame_length > frame_count) + { + gyro_frame_length = frame_count; + } + + /* Print the parsed gyro data from the FIFO buffer. */ + for (index = 0; index < gyro_frame_length; index++) + { + printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", + index, + fifo_gyro_data[index].x, + fifo_gyro_data[index].y, + fifo_gyro_data[index].z); + } + + /* Print control frames like sensor time and skipped frame count. */ + printf("\nSkipped frame count = %d\n", fifoframe.skipped_frame_count); + + printf("Sensor time = %lu\r\n", fifoframe.sensor_time); + + try++; + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accel and gyro configurations. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[0].type = BMI2_ACCEL; + config[1].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_get_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Accel configuration settings. */ + /* Set Output Data Rate */ + config[0].cfg.acc.odr = BMI2_ACC_ODR_25HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Gyro configuration settings. */ + /* Set Output Data Rate */ + config[1].cfg.gyr.odr = BMI2_GYR_ODR_25HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set new configurations. */ + rslt = bmi270_set_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_headerless_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_headerless_mode/Makefile new file mode 100644 index 000000000..ae9a413fe --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_headerless_mode/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= fifo_full_headerless_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_headerless_mode/fifo_full_headerless_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_headerless_mode/fifo_full_headerless_mode.c new file mode 100644 index 000000000..53dddc4ba --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_headerless_mode/fifo_full_headerless_mode.c @@ -0,0 +1,304 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270.h" +#include "common.h" + +/******************************************************************************/ +/*! Macros */ + +/*! Buffer size allocated to store raw FIFO data */ +#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) + +/*! Length of data to be read from FIFO */ +#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) + +/*! Number of accel frames to be extracted from FIFO */ + +/*! Calculation for frame count: Total frame count = Fifo buffer size(2048)/ Total frames(6 Accel, 6 Gyro totaling to + * 12) which equals to 170. + */ +#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(169) + +/*! Number of gyro frames to be extracted from FIFO */ +#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(169) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + * @param[in] dev : Structure instance of bmi2_dev. + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev); + +/******************************************************************************/ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + uint16_t index = 0; + + uint16_t fifo_length = 0; + + uint8_t try = 1; + + /* Variable to get fifo full interrupt status. */ + uint16_t int_status = 0; + + uint16_t accel_frame_length; + + uint16_t gyro_frame_length; + + /* Variable to store the available frame length count in FIFO */ + uint8_t frame_count; + + /* Number of bytes of FIFO data. */ + uint8_t fifo_data[BMI2_FIFO_RAW_DATA_BUFFER_SIZE] = { 0 }; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Array of accelerometer frames -> Total bytes = + * 170 * (6 axes bytes) = 1020 bytes */ + struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; + + /* Array of gyro frames -> Total bytes = + * 170 * (6 axes bytes) = 1020 bytes */ + struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; + + /* Initialize FIFO frame structure. */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Accel and gyro sensor are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable accel and gyro sensor. */ + rslt = bmi270_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configuration settings for accel and gyro. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Before setting FIFO, disable the advance power save mode. */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Update FIFO structure. */ + /* Mapping the buffer to store the fifo data. */ + fifoframe.data = fifo_data; + + /* Length of FIFO frame. */ + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; + + /* Set FIFO configuration by enabling accel, gyro. + * NOTE 1: The header mode is enabled by default. + * NOTE 2: By default the FIFO operating mode is FIFO mode. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("FIFO is configured in headerless mode\n"); + + /* To enable headerless mode, disable the header. */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_fifo_config(BMI2_FIFO_HEADER_EN, BMI2_DISABLE, &bmi2_dev); + } + + /* Map FIFO full interrupt. */ + fifoframe.data_int_map = BMI2_FFULL_INT; + rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + while (try <= 10) + { + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if ((rslt == BMI2_OK) && (int_status & BMI2_FFULL_INT_STATUS_MASK)) + { + printf("\nIteration : %d\n", try); + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + printf("\nFIFO data bytes available : %d \n", fifo_length); + printf("\nFIFO data bytes requested : %d \n", fifoframe.length); + + /* Read FIFO data. */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nFIFO accel frames requested : %d \n", accel_frame_length); + + /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ + rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); + + printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); + + /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ + rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); + + /* Calculating the frame count from the available bytes in FIFO + * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len)) */ + frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH)); + printf("\nAvailable frame count from available bytes: %d\n", frame_count); + + printf("\nExtracted accel frames\n"); + + if (accel_frame_length > frame_count) + { + accel_frame_length = frame_count; + } + + /* Print the parsed accelerometer data from the FIFO buffer. */ + for (index = 0; index < accel_frame_length; index++) + { + printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, + fifo_accel_data[index].y, fifo_accel_data[index].z); + } + + printf("\nExtracted gyro frames\n"); + + if (gyro_frame_length > frame_count) + { + gyro_frame_length = frame_count; + } + + /* Print the parsed gyro data from the FIFO buffer. */ + for (index = 0; index < gyro_frame_length; index++) + { + printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", + index, + fifo_gyro_data[index].x, + fifo_gyro_data[index].y, + fifo_gyro_data[index].z); + } + } + + try++; + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accel and gyro configurations. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[0].type = BMI2_ACCEL; + config[1].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_get_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Accel configuration settings. */ + /* Set Output Data Rate */ + config[0].cfg.acc.odr = BMI2_ACC_ODR_100HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Gyro configuration settings. */ + /* Set Output Data Rate */ + config[1].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set new configurations. */ + rslt = bmi270_set_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_header_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_header_mode/Makefile new file mode 100644 index 000000000..08158205b --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_header_mode/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= fifo_watermark_header_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_header_mode/fifo_watermark_header_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_header_mode/fifo_watermark_header_mode.c new file mode 100644 index 000000000..faaeb307e --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_header_mode/fifo_watermark_header_mode.c @@ -0,0 +1,335 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270.h" +#include "common.h" + +/******************************************************************************/ +/*! Macros */ + +/*! Buffer size allocated to store raw FIFO data */ +#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) + +/*! Length of data to be read from FIFO */ +#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) + +/*! Number of accel frames to be extracted from FIFO */ + +/*! + * Calculation: + * fifo_watermark_level = 650, accel_frame_len = 6, gyro_frame_len = 6 header_byte = 1. + * fifo_accel_frame_count = (650 / (6 + 6 + 1 )) = 50 frames + * NOTE: Extra frames are read in order to get sensor time + */ +#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(55) + +/*! Number of gyro frames to be extracted from FIFO */ +#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(55) + +/*! Setting a watermark level in FIFO */ +#define BMI270_FIFO_WATERMARK_LEVEL UINT16_C(650) + +/*! Macro to read sensortime byte in FIFO */ +#define SENSORTIME_OVERHEAD_BYTE UINT8_C(3) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + * @param[in] dev : Structure instance of bmi2_dev. + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to store the available frame length count in FIFO */ + uint8_t frame_count; + + /* Variable to index bytes. */ + uint16_t index = 0; + + uint16_t fifo_length = 0; + + uint16_t accel_frame_length; + + uint16_t gyro_frame_length; + + uint8_t try = 1; + + /* To read sensortime, extra 3 bytes are added to fifo buffer */ + uint16_t fifo_buffer_size = BMI2_FIFO_RAW_DATA_BUFFER_SIZE + SENSORTIME_OVERHEAD_BYTE; + + /* Number of bytes of FIFO data. */ + uint8_t fifo_data[fifo_buffer_size]; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Array of accelerometer frames -> Total bytes = + * 55 * (6 axes bytes) = 330 bytes */ + struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; + + /* Array of gyro frames -> Total bytes = + * 55 * (6 axes bytes) = 330 bytes */ + struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; + + /* Initialize FIFO frame structure. */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Accel and gyro sensor are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Variable to get fifo water-mark interrupt status. */ + uint16_t int_status = 0; + + uint16_t watermark = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable accel and gyro sensor. */ + rslt = bmi270_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configuration settings for accel and gyro. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Before setting FIFO, disable the advance power save mode. */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Update FIFO structure. */ + /* Mapping the buffer to store the fifo data. */ + fifoframe.data = fifo_data; + + /* Length of FIFO frame. */ + /* To read sensortime, extra 3 bytes are added to fifo user length. */ + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH + SENSORTIME_OVERHEAD_BYTE; + + /* Set FIFO configuration by enabling accel, gyro and timestamp. + * NOTE 1: The header mode is enabled by default. + * NOTE 2: By default the FIFO operating mode is FIFO mode. + * NOTE 3: Sensortime is enabled by default */ + printf("FIFO is configured in header mode\n"); + rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* FIFO water-mark interrupt is enabled. */ + fifoframe.data_int_map = BMI2_FWM_INT; + + /* Map water-mark interrupt to the required interrupt pin. */ + rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Set water-mark level. */ + fifoframe.wm_lvl = BMI270_FIFO_WATERMARK_LEVEL; + + /* Set the water-mark level if water-mark interrupt is mapped. */ + rslt = bmi2_set_fifo_wm(fifoframe.wm_lvl, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + rslt = bmi2_get_fifo_wm(&watermark, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + printf("\nFIFO watermark level is %d\n", watermark); + + while (try <= 10) + { + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the status of FIFO watermark interrupt. */ + if ((rslt == BMI2_OK) && (int_status & BMI2_FWM_INT_STATUS_MASK)) + { + printf("\nIteration : %d\n", try); + + printf("\nWatermark interrupt occurred\n"); + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + printf("\nFIFO data bytes available : %d \n", fifo_length); + printf("\nFIFO data bytes requested : %d \n", fifoframe.length); + + /* Read FIFO data. */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nFIFO accel frames requested : %d \n", accel_frame_length); + + /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ + rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); + + printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); + + /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ + rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); + + /* Calculating the frame count from the available bytes in FIFO + * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len + header_byte)) */ + frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH + 1)); + printf("\nAvailable frame count from available bytes: %d\n", frame_count); + + printf("\nExracted accel frames\n"); + + if (accel_frame_length > frame_count) + { + accel_frame_length = frame_count; + } + + /* Print the parsed accelerometer data from the FIFO buffer. */ + for (index = 0; index < accel_frame_length; index++) + { + printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, + fifo_accel_data[index].y, fifo_accel_data[index].z); + } + + printf("\nExtracted gyro frames\n"); + + if (gyro_frame_length > frame_count) + { + gyro_frame_length = frame_count; + } + + /* Print the parsed gyro data from the FIFO buffer. */ + for (index = 0; index < gyro_frame_length; index++) + { + printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", + index, + fifo_gyro_data[index].x, + fifo_gyro_data[index].y, + fifo_gyro_data[index].z); + } + + /* Print control frames like sensor time and skipped frame count. */ + printf("Skipped frame count = %d\n", fifoframe.skipped_frame_count); + printf("Sensor time = %lu\r\n", fifoframe.sensor_time); + + try++; + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accel and gyro configurations. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[0].type = BMI2_ACCEL; + config[1].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_get_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Accel configuration settings. */ + /* Set Output Data Rate */ + config[0].cfg.acc.odr = BMI2_ACC_ODR_25HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Gyro configuration settings. */ + /* Set Output Data Rate */ + config[1].cfg.gyr.odr = BMI2_GYR_ODR_25HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set new configurations. */ + rslt = bmi270_set_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_headerless_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_headerless_mode/Makefile new file mode 100644 index 000000000..527a18ab8 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_headerless_mode/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= fifo_watermark_headerless_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c new file mode 100644 index 000000000..b3adf8965 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c @@ -0,0 +1,331 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270.h" +#include "common.h" + +/******************************************************************************/ +/*! Macros */ + +/*! Buffer size allocated to store raw FIFO data */ +#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) + +/*! Length of data to be read from FIFO */ +#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) + +/*! Number of accel frames to be extracted from FIFO */ + +/*! + * Calculation: + * fifo_watermark_level = 650, accel_frame_len = 6, gyro_frame_len = 6. + * fifo_accel_frame_count = (650 / (6 + 6 )) = 54 frames + */ +#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(55) + +/*! Number of gyro frames to be extracted from FIFO */ +#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(55) + +/*! Setting a watermark level in FIFO */ +#define BMI270_FIFO_WATERMARK_LEVEL UINT16_C(650) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + * @param[in] dev : Structure instance of bmi2_dev. + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to index bytes. */ + uint16_t index = 0; + + /* Variable to store the available frame length count in FIFO */ + uint8_t frame_count; + + uint16_t fifo_length = 0; + + uint16_t accel_frame_length; + + uint16_t gyro_frame_length; + + uint8_t try = 1; + + /* Number of bytes of FIFO data. */ + uint8_t fifo_data[BMI2_FIFO_RAW_DATA_BUFFER_SIZE] = { 0 }; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Array of accelerometer frames -> Total bytes = + * 50 * (6 axes bytes) = 300 bytes */ + struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; + + /* Array of gyro frames -> Total bytes = + * 50 * (6 axes bytes) = 300 bytes */ + struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; + + /* Initialize FIFO frame structure. */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Accel and gyro sensor are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Variable to get fifo water-mark interrupt status. */ + uint16_t int_status = 0; + + uint16_t watermark = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable accel and gyro sensor. */ + rslt = bmi270_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configuration settings for accel and gyro. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Before setting FIFO, disable the advance power save mode. */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Update FIFO structure. */ + /* Mapping the buffer to store the fifo data. */ + fifoframe.data = fifo_data; + + /* Length of FIFO frame. */ + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; + + /* Set FIFO configuration by enabling accel, gyro. + * NOTE 1: The header mode is enabled by default. + * NOTE 2: By default the FIFO operating mode is FIFO mode. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("FIFO is configured in headerless mode\n"); + + /* To enable headerless mode, disable the header. */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_fifo_config(BMI2_FIFO_HEADER_EN, BMI2_DISABLE, &bmi2_dev); + } + + /* FIFO water-mark interrupt is enabled. */ + fifoframe.data_int_map = BMI2_FWM_INT; + + /* Map water-mark interrupt to the required interrupt pin. */ + rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Set water-mark level. */ + fifoframe.wm_lvl = BMI270_FIFO_WATERMARK_LEVEL; + + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; + + /* Set the water-mark level if water-mark interrupt is mapped. */ + rslt = bmi2_set_fifo_wm(fifoframe.wm_lvl, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + rslt = bmi2_get_fifo_wm(&watermark, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + printf("\nFIFO watermark level is %d\n", watermark); + + while (try <= 10) + { + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the status of FIFO watermark interrupt. */ + if ((rslt == BMI2_OK) && (int_status & BMI2_FWM_INT_STATUS_MASK)) + { + printf("\nIteration : %d\n", try); + + printf("\nWatermark interrupt occurred\n"); + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + printf("\nFIFO data bytes available : %d \n", fifo_length); + printf("\nFIFO data bytes requested : %d \n", fifoframe.length); + + /* Read FIFO data. */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nFIFO accel frames requested : %d \n", accel_frame_length); + + /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ + rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); + + printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); + + /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ + rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); + + /* Calculating the frame count from the available bytes in FIFO + * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len)) */ + frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH)); + printf("\nAvailable frame count from available bytes: %d\n", frame_count); + + printf("\nExracted accel frames\n"); + + if (accel_frame_length > frame_count) + { + accel_frame_length = frame_count; + } + + /* Print the parsed accelerometer data from the FIFO buffer. */ + for (index = 0; index < accel_frame_length; index++) + { + printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, + fifo_accel_data[index].y, fifo_accel_data[index].z); + } + + printf("\nExtracted gyro frames\n"); + + if (gyro_frame_length > frame_count) + { + gyro_frame_length = frame_count; + } + + /* Print the parsed gyro data from the FIFO buffer. */ + for (index = 0; index < gyro_frame_length; index++) + { + printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", + index, + fifo_gyro_data[index].x, + fifo_gyro_data[index].y, + fifo_gyro_data[index].z); + } + } + + try++; + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accel and gyro configurations. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[0].type = BMI2_ACCEL; + config[1].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_get_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameter according to their requirement. */ + /* Accel configuration settings. */ + /* Set Output Data Rate */ + config[0].cfg.acc.odr = BMI2_ACC_ODR_100HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Gyro configuration settings. */ + /* Set Output Data Rate */ + config[1].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set new configurations. */ + rslt = bmi270_set_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/gyro/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/gyro/Makefile new file mode 100644 index 000000000..e1f03b8bd --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/gyro/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= gyro.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/gyro/gyro.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/gyro/gyro.c new file mode 100644 index 000000000..1bb0ca471 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/gyro/gyro.c @@ -0,0 +1,195 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270.h" +#include "common.h" + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for gyro. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_gyro_config(struct bmi2_dev *dev); + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + * + * @param[in] val : LSB from each axis. + * @param[in] dps : Degree per second. + * @param[in] bit_width : Resolution for gyro. + * + * @return Degree per second. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to define limit to print gyro data. */ + uint8_t limit = 10; + + uint8_t indx = 0; + + float x = 0, y = 0, z = 0; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Create an instance of sensor data structure. */ + struct bmi2_sensor_data sensor_data = { 0 }; + + /* Assign gyro sensor to variable. */ + uint8_t sens_list = BMI2_GYRO; + + /* Initialize the interrupt status of gyro. */ + uint16_t int_status = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the selected sensors. */ + rslt = bmi270_sensor_enable(&sens_list, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Gyro configuration settings. */ + rslt = set_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Select gyro sensor. */ + sensor_data.type = BMI2_GYRO; + printf("Gyro and DPS data\n"); + printf("Gyro data collected at Range 2000 dps with 16-bit resolution\n"); + + /* Loop to print gyro data when interrupt occurs. */ + while (indx <= limit) + { + /* To get the data ready interrupt status of gyro. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the data ready interrupt status and print the status for 10 samples. */ + if (int_status & BMI2_GYR_DRDY_INT_MASK) + { + /* Get gyro data for x, y and z axis. */ + rslt = bmi270_get_sensor_data(&sensor_data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("\nGyr_X = %d\t", sensor_data.sens_data.gyr.x); + printf("Gyr_Y = %d\t", sensor_data.sens_data.gyr.y); + printf("Gyr_Z = %d\r\n", sensor_data.sens_data.gyr.z); + + /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ + x = lsb_to_dps(sensor_data.sens_data.gyr.x, 2000, bmi2_dev.resolution); + y = lsb_to_dps(sensor_data.sens_data.gyr.y, 2000, bmi2_dev.resolution); + z = lsb_to_dps(sensor_data.sens_data.gyr.z, 2000, bmi2_dev.resolution); + + /* Print the data in dps. */ + printf("Gyr_DPS_X = %4.2f, Gyr_DPS_Y = %4.2f, Gyr_DPS_Z = %4.2f\n", x, y, z); + + indx++; + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for gyro. + */ +static int8_t set_gyro_config(struct bmi2_dev *dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define the type of sensor and its configurations. */ + struct bmi2_sens_config config; + + /* Configure the type of feature. */ + config.type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_get_sensor_config(&config, 1, dev); + bmi2_error_codes_print_result(rslt); + + /* Map data ready interrupt to interrupt pin. */ + rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT2, dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config.cfg.gyr.odr = BMI2_GYR_ODR_100HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config.cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config.cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config.cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config.cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set the gyro configurations. */ + rslt = bmi270_set_sensor_config(&config, 1, dev); + } + + return rslt; +} + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/no_motion_interrupt/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/no_motion_interrupt/Makefile new file mode 100644 index 000000000..c5df6dfb3 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/no_motion_interrupt/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= no_motion_interrupt.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/no_motion_interrupt/no_motion_interrupt.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/no_motion_interrupt/no_motion_interrupt.c new file mode 100644 index 000000000..5c4630660 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/no_motion_interrupt/no_motion_interrupt.c @@ -0,0 +1,134 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270.h" +#include "common.h" + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for no-motion. + * + * @param[in] bmi2_dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_feature_config(struct bmi2_dev *bmi2_dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Accel sensor and no-motion feature are listed in array. */ + uint8_t sens_list[2] = { BMI2_ACCEL, BMI2_NO_MOTION }; + + /* Variable to get no-motion interrupt status. */ + uint16_t int_status = 0; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Select features and their pins to be mapped to. */ + struct bmi2_sens_int_config sens_int = { .type = BMI2_NO_MOTION, .hw_int_pin = BMI2_INT1 }; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the selected sensors. */ + rslt = bmi270_sensor_enable(sens_list, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Set feature configurations for no-motion. */ + rslt = set_feature_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Map the feature interrupt for no-motion. */ + rslt = bmi270_map_feat_int(&sens_int, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + printf("Do not move the board\n"); + + /* Loop to get no-motion interrupt. */ + do + { + /* Clear buffer. */ + int_status = 0; + + /* To get the interrupt status of no-motion. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the interrupt status of no-motion. */ + if (int_status & BMI270_NO_MOT_STATUS_MASK) + { + printf("No-motion interrupt is generated\n"); + break; + } + } while (rslt == BMI2_OK); + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for no-motion. + */ +static int8_t set_feature_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define the type of sensor and its configurations. */ + struct bmi2_sens_config config; + + /* Configure the type of feature. */ + config.type = BMI2_NO_MOTION; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_get_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* 1LSB equals 20ms. Default is 100ms, setting to 80ms. */ + config.cfg.no_motion.duration = 0x04; + + /* 1LSB equals to 0.48mg. Default is 83mg, setting to 50mg. */ + config.cfg.no_motion.threshold = 0x68; + + /* Set new configurations. */ + rslt = bmi270_set_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_data_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_data_mode/Makefile new file mode 100644 index 000000000..db669c09d --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_data_mode/Makefile @@ -0,0 +1,23 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= read_aux_data_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +BMM150_SOURCE ?= ../../../bmm150 + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270.c \ +$(BMM150_SOURCE)/bmm150.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(BMM150_SOURCE) \ +$(COMMON_LOCATION)/common + + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_data_mode/read_aux_data_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_data_mode/read_aux_data_mode.c new file mode 100644 index 000000000..a542f97c6 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_data_mode/read_aux_data_mode.c @@ -0,0 +1,327 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270.h" +#include "bmm150.h" +#include "coines.h" +#include "common.h" + +/******************************************************************************/ +/*! Macro definition */ + +/*! Macros to select the sensors */ +#define ACCEL UINT8_C(0x00) +#define GYRO UINT8_C(0x01) +#define AUX UINT8_C(0x02) + +/*! Earth's gravity in m/s^2 */ +#define GRAVITY_EARTH (9.80665f) + +/*****************************************************************************/ +/*! Structure declaration */ + +/* Sensor initialization configuration. */ +struct bmi2_dev bmi2_dev; + +/*******************************************************************************/ +/*! Functions */ + +/** + * user_aux_read - Reads data from auxiliary sensor in manual mode. + * + * @param[in] reg_addr : Register address. + * @param[out] aux_data : Aux data pointer to store the read data. + * @param[in] length : No of bytes to read. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval 0 -> Success + * @retval < 0 -> Failure Info + */ +static int8_t user_aux_read(uint8_t reg_addr, uint8_t *aux_data, uint32_t len, void *intf_ptr); + +/** + * user_aux_write - Writes data to the auxiliary sensor in manual mode. + * + * @param[in] reg_addr : Register address. + * @param[out] aux_data : Aux data pointer to store the data being written. + * @param[in] length : No of bytes to read. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval 0 -> Success + * @retval < 0 -> Failure Info + */ +static int8_t user_aux_write(uint8_t reg_addr, const uint8_t *aux_data, uint32_t len, void *intf_ptr); + +/*! + * @brief This function provides the delay for required time (Microsecond) as per the input provided in some of the + * APIs. + * + * @param[in] period_us : The required wait time in microsecond. + * @param[in] intf_ptr : Interface pointer + * + * @return void. + */ +static void user_delay_us(uint32_t period, void *intf_ptr); + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + * + * @param[in] val : LSB from each axis. + * @param[in] g_range : Gravity range. + * @param[in] bit_width : Resolution for accel. + * + * @return Gravity. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + * + * @param[in] val : LSB from each axis. + * @param[in] dps : Degree per second. + * @param[in] bit_width : Resolution for gyro. + * + * @return Degree per second. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to select the pull-up resistor which is set to trim register */ + uint8_t regdata; + + /* Variable to define limit to print aux data. */ + uint8_t limit = 20; + + /* Accel, Gyro and Aux sensors are listed in array. */ + uint8_t sensor_list[3] = { BMI2_ACCEL, BMI2_GYRO, BMI2_AUX }; + + /* Structure to define the type of the sensor and its configurations. */ + struct bmi2_sens_config config[3]; + + /* Sensor initialization configuration. */ + struct bmm150_dev bmm150_dev; + + /* bmm150 settings configuration */ + struct bmm150_settings settings; + + /* bmm150 magnetometer data */ + struct bmm150_mag_data mag_data; + + /* Structure to define type of sensor and their respective data. */ + struct bmi2_sensor_data sensor_data[3]; + + /* Variables to define read the accel and gyro data in float */ + float x = 0, y = 0, z = 0; + + config[ACCEL].type = BMI2_ACCEL; + config[GYRO].type = BMI2_GYRO; + config[AUX].type = BMI2_AUX; + + /* To enable the i2c interface settings for bmm150. */ + uint8_t bmm150_dev_addr = BMM150_DEFAULT_I2C_ADDRESS; + bmm150_dev.intf_ptr = &bmm150_dev_addr; + bmm150_dev.read = user_aux_read; + bmm150_dev.write = user_aux_write; + bmm150_dev.delay_us = user_delay_us; + + /* As per datasheet, aux interface with bmi270 will support only for I2C */ + bmm150_dev.intf = BMM150_I2C_INTF; + + sensor_data[ACCEL].type = BMI2_ACCEL; + sensor_data[GYRO].type = BMI2_GYRO; + sensor_data[AUX].type = BMI2_AUX; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Pull-up resistor 2k is set to the trim regiter */ + regdata = BMI2_ASDA_PUPSEL_2K; + rslt = bmi2_set_regs(BMI2_AUX_IF_TRIM, ®data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable the accel, gyro and aux sensor. */ + rslt = bmi270_sensor_enable(sensor_list, 3, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_get_sensor_config(config, 3, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configurations for accel. */ + config[ACCEL].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + config[ACCEL].cfg.acc.bwp = BMI2_ACC_OSR2_AVG2; + config[ACCEL].cfg.acc.odr = BMI2_ACC_ODR_100HZ; + config[ACCEL].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* Configurations for gyro. */ + config[GYRO].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + config[GYRO].cfg.gyr.noise_perf = BMI2_GYR_RANGE_2000; + config[GYRO].cfg.gyr.bwp = BMI2_GYR_OSR2_MODE; + config[GYRO].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; + config[GYRO].cfg.gyr.range = BMI2_GYR_RANGE_2000; + config[GYRO].cfg.gyr.ois_range = BMI2_GYR_OIS_2000; + + /* Configurations for aux. */ + config[AUX].cfg.aux.odr = BMI2_AUX_ODR_100HZ; + config[AUX].cfg.aux.aux_en = BMI2_ENABLE; + config[AUX].cfg.aux.i2c_device_addr = BMM150_DEFAULT_I2C_ADDRESS; + config[AUX].cfg.aux.manual_en = BMI2_ENABLE; + config[AUX].cfg.aux.fcu_write_en = BMI2_ENABLE; + config[AUX].cfg.aux.man_rd_burst = BMI2_AUX_READ_LEN_3; + config[AUX].cfg.aux.read_addr = BMM150_REG_DATA_X_LSB; + + /* Set new configurations for accel, gyro and aux. */ + rslt = bmi270_set_sensor_config(config, 3, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmm150. */ + rslt = bmm150_init(&bmm150_dev); + bmi2_error_codes_print_result(rslt); + + /* Set the power mode to normal mode. */ + settings.pwr_mode = BMM150_POWERMODE_NORMAL; + rslt = bmm150_set_op_mode(&settings, &bmm150_dev); + bmi2_error_codes_print_result(rslt); + + rslt = bmi270_get_sensor_config(config, 3, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Disable manual mode so that the data mode is enabled. */ + config[AUX].cfg.aux.manual_en = BMI2_DISABLE; + + /* Set the aux configurations. */ + rslt = bmi270_set_sensor_config(config, 3, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("MAGNETOMETER, ACCEL, AND GYRO DATA IN DATA MODE\n"); + + if (bmm150_dev.chip_id == BMM150_CHIP_ID) + { + while (limit--) + { + /* Delay has been added to get aux, accel and gyro data at the interval of every 0.05 second. */ + bmi2_dev.delay_us(50000, bmi2_dev.intf_ptr); + + rslt = bmi270_get_sensor_data(sensor_data, 3, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nAcc_x = %d\t", sensor_data[ACCEL].sens_data.acc.x); + printf("Acc_y = %d\t", sensor_data[ACCEL].sens_data.acc.y); + printf("Acc_z = %d", sensor_data[ACCEL].sens_data.acc.z); + + /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ + x = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.x, 2, bmi2_dev.resolution); + y = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.y, 2, bmi2_dev.resolution); + z = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.z, 2, bmi2_dev.resolution); + + /* Print the data in m/s2. */ + printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); + + printf("\nGyr_X = %d\t", sensor_data[GYRO].sens_data.gyr.x); + printf("Gyr_Y = %d\t", sensor_data[GYRO].sens_data.gyr.y); + printf("Gyr_Z= %d", sensor_data[GYRO].sens_data.gyr.z); + + /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ + x = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.x, 2000, bmi2_dev.resolution); + y = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.y, 2000, bmi2_dev.resolution); + z = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.z, 2000, bmi2_dev.resolution); + + /* Print the data in dps. */ + printf("\nGyro_DPS_X = %4.2f, Gyro_DPS_Y = %4.2f, Gyro_DPS_Z = %4.2f\n", x, y, z); + + /* Compensating the raw auxiliary data available from the BMM150 API. */ + rslt = bmm150_aux_mag_data(sensor_data[AUX].sens_data.aux_data, &mag_data, &bmm150_dev); + bmi2_error_codes_print_result(rslt); + printf("\nMag_x_axis = %d \t Mag_y_axis = %d \t Mag_z_axis = %d \t\n", + mag_data.x, + mag_data.y, + mag_data.z); + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This function reads the data from auxiliary sensor in manual mode. + */ +static int8_t user_aux_read(uint8_t reg_addr, uint8_t *aux_data, uint32_t len, void *intf_ptr) +{ + int8_t rslt; + + /* Discarding the parameter id as it is redundant */ + rslt = bmi2_read_aux_man_mode(reg_addr, aux_data, len, &bmi2_dev); + + return rslt; +} + +/*! + * @brief This function writes the data to auxiliary sensor in manual mode. + */ +static int8_t user_aux_write(uint8_t reg_addr, const uint8_t *aux_data, uint32_t len, void *intf_ptr) +{ + int8_t rslt; + + /* Discarding the parameter id as it is redundant */ + rslt = bmi2_write_aux_man_mode(reg_addr, aux_data, len, &bmi2_dev); + + return rslt; +} + +/*! + * Delay function map to COINES platform + */ +static void user_delay_us(uint32_t period, void *intf_ptr) +{ + coines_delay_usec(period); +} + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (GRAVITY_EARTH * val * g_range) / half_scale; +} + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_manual_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_manual_mode/Makefile new file mode 100644 index 000000000..cc9e6e4e7 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_manual_mode/Makefile @@ -0,0 +1,23 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= read_aux_manual_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +BMM150_SOURCE ?= ../../../bmm150 + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270.c \ +$(BMM150_SOURCE)/bmm150.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(BMM150_SOURCE) \ +$(COMMON_LOCATION)/common + + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_manual_mode/read_aux_manual_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_manual_mode/read_aux_manual_mode.c new file mode 100644 index 000000000..ced8f59c7 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_manual_mode/read_aux_manual_mode.c @@ -0,0 +1,322 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270.h" +#include "bmm150.h" +#include "coines.h" +#include "common.h" + +/******************************************************************************/ +/*! Macro definition */ + +/*! Macros to select the sensors */ +#define ACCEL UINT8_C(0x00) +#define GYRO UINT8_C(0x01) +#define AUX UINT8_C(0x02) + +/*! Earth's gravity in m/s^2 */ +#define GRAVITY_EARTH (9.80665f) + +/*****************************************************************************/ +/*! Structure declaration */ + +/* Sensor initialization configuration. */ +struct bmi2_dev bmi2_dev; + +/******************************************************************************/ +/*! Functions */ + +/** + * user_aux_read - Reads data from auxiliary sensor in manual mode. + * + * @param[in] reg_addr : Register address. + * @param[out] aux_data : Aux data pointer to store the read data. + * @param[in] length : No of bytes to read. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval 0 -> Success + * @retval < 0 -> Failure Info + */ +static int8_t user_aux_read(uint8_t reg_addr, uint8_t *aux_data, uint32_t len, void *intf_ptr); + +/** + * user_aux_write - Writes data to the auxiliary sensor in manual mode. + * + * @param[in] reg_addr : Register address. + * @param[out] aux_data : Aux data pointer to store the data being written. + * @param[in] length : No of bytes to read. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval 0 -> Success + * @retval < 0 -> Failure Info + */ +static int8_t user_aux_write(uint8_t reg_addr, const uint8_t *aux_data, uint32_t len, void *intf_ptr); + +/*! + * @brief This function provides the delay for required time (Microsecond) as per the input provided in some of the + * APIs. + * + * @param[in] period_us : The required wait time in microsecond. + * @param[in] intf_ptr : Interface pointer + * + * @return void. + */ +static void user_delay_us(uint32_t period, void *intf_ptr); + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + * + * @param[in] val : LSB from each axis. + * @param[in] g_range : Gravity range. + * @param[in] bit_width : Resolution for accel. + * + * @return Gravity. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + * + * @param[in] val : LSB from each axis. + * @param[in] dps : Degree per second. + * @param[in] bit_width : Resolution for gyro. + * + * @return Degree per second. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to select the pull-up resistor which is set to trim register */ + uint8_t regdata; + + /* Variable to define limit to print aux data. */ + uint8_t limit = 20; + + /* Accel, Gyro and Aux sensors are listed in array. */ + uint8_t sensor_list[3] = { BMI2_ACCEL, BMI2_GYRO, BMI2_AUX }; + + /* Sensor initialization configuration. */ + struct bmm150_dev bmm150_dev; + + /* bmm150 settings configuration */ + struct bmm150_settings settings; + + /* bmm150 magnetometer data */ + struct bmm150_mag_data mag_data; + + /* Structure to define the type of the sensor and its configurations. */ + struct bmi2_sens_config config[3]; + + /* Structure to define type of sensor and their respective data. */ + struct bmi2_sensor_data sensor_data[2]; + + /* Variables to define read the accel and gyro data in float */ + float x = 0, y = 0, z = 0; + + config[ACCEL].type = BMI2_ACCEL; + config[GYRO].type = BMI2_GYRO; + config[AUX].type = BMI2_AUX; + + sensor_data[ACCEL].type = BMI2_ACCEL; + sensor_data[GYRO].type = BMI2_GYRO; + + /* Array of eight bytes to store x, y, z and r axis aux data. */ + uint8_t aux_data[8] = { 0 }; + + /* To enable the i2c interface settings for bmm150. */ + uint8_t bmm150_dev_addr = BMM150_DEFAULT_I2C_ADDRESS; + bmm150_dev.intf_ptr = &bmm150_dev_addr; + bmm150_dev.read = user_aux_read; + bmm150_dev.write = user_aux_write; + bmm150_dev.delay_us = user_delay_us; + + /* As per datasheet, aux interface with bmi270 will support only for I2C */ + bmm150_dev.intf = BMM150_I2C_INTF; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Pull-up resistor 2k is set to the trim register */ + regdata = BMI2_ASDA_PUPSEL_2K; + rslt = bmi2_set_regs(BMI2_AUX_IF_TRIM, ®data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable the accel, gyro and aux sensor. */ + rslt = bmi270_sensor_enable(sensor_list, 3, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_get_sensor_config(config, 3, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configurations for accel. */ + config[ACCEL].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + config[ACCEL].cfg.acc.bwp = BMI2_ACC_OSR2_AVG2; + config[ACCEL].cfg.acc.odr = BMI2_ACC_ODR_100HZ; + config[ACCEL].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* Configurations for gyro. */ + config[GYRO].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + config[GYRO].cfg.gyr.noise_perf = BMI2_GYR_RANGE_2000; + config[GYRO].cfg.gyr.bwp = BMI2_GYR_OSR2_MODE; + config[GYRO].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; + config[GYRO].cfg.gyr.range = BMI2_GYR_RANGE_2000; + config[GYRO].cfg.gyr.ois_range = BMI2_GYR_OIS_2000; + + /* Configurations for aux. */ + config[AUX].cfg.aux.odr = BMI2_AUX_ODR_100HZ; + config[AUX].cfg.aux.aux_en = BMI2_ENABLE; + config[AUX].cfg.aux.i2c_device_addr = BMM150_DEFAULT_I2C_ADDRESS; + config[AUX].cfg.aux.manual_en = BMI2_ENABLE; + config[AUX].cfg.aux.fcu_write_en = BMI2_ENABLE; + config[AUX].cfg.aux.man_rd_burst = BMI2_AUX_READ_LEN_3; + + /* Set new configurations for accel, gyro and aux. */ + rslt = bmi270_set_sensor_config(config, 3, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmm150. */ + rslt = bmm150_init(&bmm150_dev); + bmi2_error_codes_print_result(rslt); + + /* Set the power mode to normal mode. */ + settings.pwr_mode = BMM150_POWERMODE_NORMAL; + rslt = bmm150_set_op_mode(&settings, &bmm150_dev); + bmi2_error_codes_print_result(rslt); + + printf("MAGNETOMETER, ACCEL AND GYRO DATA IN MANUAL MODE\n"); + + if (bmm150_dev.chip_id == BMM150_CHIP_ID) + { + while (limit--) + { + /* Delay has been added to get aux, accel and gyro data at the interval of every 0.05 second. */ + bmi2_dev.delay_us(50000, bmi2_dev.intf_ptr); + + rslt = bmi270_get_sensor_data(sensor_data, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nAcc_x = %d\t", sensor_data[ACCEL].sens_data.acc.x); + printf("Acc_y = %d\t", sensor_data[ACCEL].sens_data.acc.y); + printf("Acc_z = %d", sensor_data[ACCEL].sens_data.acc.z); + + /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ + x = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.x, 2, bmi2_dev.resolution); + y = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.y, 2, bmi2_dev.resolution); + z = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.z, 2, bmi2_dev.resolution); + + /* Print the data in m/s2. */ + printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); + + printf("\nGyr_X = %d\t", sensor_data[GYRO].sens_data.gyr.x); + printf("Gyr_Y = %d\t", sensor_data[GYRO].sens_data.gyr.y); + printf("Gyr_Z = %d", sensor_data[GYRO].sens_data.gyr.z); + + /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ + x = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.x, 2000, bmi2_dev.resolution); + y = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.y, 2000, bmi2_dev.resolution); + z = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.z, 2000, bmi2_dev.resolution); + + /* Print the data in dps. */ + printf("\nGyro_DPS_X = %4.2f, Gyro_DPS_Y = %4.2f, Gyro_DPS_Z = %4.2f\n", x, y, z); + } + + /* Read aux data from the bmm150 data registers. */ + rslt = bmi2_read_aux_man_mode(BMM150_REG_DATA_X_LSB, aux_data, 8, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + if (rslt == BMI2_OK) + { + /* Compensating the raw auxiliary data available from the BMM150 API. */ + rslt = bmm150_aux_mag_data(aux_data, &mag_data, &bmm150_dev); + bmi2_error_codes_print_result(rslt); + printf("Mag_x_axis = %d \t Mag_y_axis = %d \t Mag_z_axis = %d \t\n", mag_data.x, mag_data.y, + mag_data.z); + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This function reads the data from auxiliary sensor in manual mode. + */ +int8_t user_aux_read(uint8_t reg_addr, uint8_t *aux_data, uint32_t len, void *intf_ptr) +{ + int8_t rslt; + + /* Discarding the parameter id as it is redundant */ + rslt = bmi2_read_aux_man_mode(reg_addr, aux_data, len, &bmi2_dev); + + return rslt; +} + +/*! + * @brief This function writes the data to auxiliary sensor in manual mode. + */ +int8_t user_aux_write(uint8_t reg_addr, const uint8_t *aux_data, uint32_t len, void *intf_ptr) +{ + int8_t rslt; + + /* Discarding the parameter id as it is redundant */ + rslt = bmi2_write_aux_man_mode(reg_addr, aux_data, len, &bmi2_dev); + + return rslt; +} + +/*! + * Delay function map to COINES platform + */ +static void user_delay_us(uint32_t period, void *intf_ptr) +{ + coines_delay_usec(period); +} + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (GRAVITY_EARTH * val * g_range) / half_scale; +} + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/sig_motion/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/sig_motion/Makefile new file mode 100644 index 000000000..97edcc2d1 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/sig_motion/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= sig_motion.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/sig_motion/sig_motion.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/sig_motion/sig_motion.c new file mode 100644 index 000000000..47e1c2864 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/sig_motion/sig_motion.c @@ -0,0 +1,93 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270.h" +#include "common.h" + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define the type of sensor and its configurations. */ + struct bmi2_sens_config config; + + /* Accel sensor and significant-motion feature are listed in array. */ + uint8_t sens_list[2] = { BMI2_ACCEL, BMI2_SIG_MOTION }; + + /* Variable to get significant-motion interrupt status. */ + uint16_t int_status = 0; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Select features and their pins to be mapped to. */ + struct bmi2_sens_int_config sens_int = { .type = BMI2_SIG_MOTION, .hw_int_pin = BMI2_INT1 }; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the selected sensors. */ + rslt = bmi270_sensor_enable(sens_list, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configure the type of feature. */ + config.type = BMI2_SIG_MOTION; + + if (rslt == BMI2_OK) + { + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_get_sensor_config(&config, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Map the feature interrupt for significant-motion. */ + rslt = bmi270_map_feat_int(&sens_int, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* By default the significant-motion interrupt occurs when the sensor is in motion for about 5 seconds. + * */ + printf("Move the board for 5 secs in any direction\n"); + + do + { + /* To get the interrupt status of significant-motion. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the interrupt status of significant-motion. */ + if (int_status & BMI270_SIG_MOT_STATUS_MASK) + { + printf("Significant motion interrupt is generated\n"); + break; + } + } while (rslt == BMI2_OK); + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_activity/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_activity/Makefile new file mode 100644 index 000000000..91962d502 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_activity/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= step_activity.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_activity/step_activity.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_activity/step_activity.c new file mode 100644 index 000000000..87775b706 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_activity/step_activity.c @@ -0,0 +1,110 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270.h" +#include "common.h" + +/******************************************************************************/ +/*! Functions */ +/* This function starts the execution of program. */ +int main(void) +{ + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Structure to define the type of sensor and their respective data. */ + struct bmi2_sensor_data sensor_data; + + /* Structure to define the type of sensor and its configurations. */ + struct bmi2_sens_config config; + + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Accel sensor and step activity feature are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_STEP_ACTIVITY }; + + /* Type of sensor to get step activity data. */ + sensor_data.type = BMI2_STEP_ACTIVITY; + + /* Variable to get step activity interrupt status. */ + uint16_t int_status = 0; + + uint16_t loop = 10; + + /* Select features and their pins to be mapped to. */ + struct bmi2_sens_int_config sens_int = { .type = BMI2_STEP_ACTIVITY, .hw_int_pin = BMI2_INT2 }; + + /* The step activities are listed in array. */ + const char *activity_output[4] = { "BMI2_STILL", "BMI2_WALK", "BMI2_RUN", "BMI2_UNKNOWN" }; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the selected sensor. */ + rslt = bmi270_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configure the type of sensor. */ + config.type = BMI2_STEP_ACTIVITY; + + if (rslt == BMI2_OK) + { + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_get_sensor_config(&config, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Map the feature interrupt for step activity. */ + rslt = bmi270_map_feat_int(&sens_int, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("\nMove the board in steps to perform step activity:\n"); + + /* Loop to get step activity. */ + while (loop) + { + /* To get the interrupt status of step detector. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the interrupt status of step detector. */ + if (int_status & BMI270_STEP_ACT_STATUS_MASK) + { + printf("Step detected\n"); + + /* Get step activity output. */ + rslt = bmi270_get_sensor_data(&sensor_data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Print the step activity output. */ + printf("Step activity = %s\n", activity_output[sensor_data.sens_data.activity_output]); + + loop--; + } + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_counter/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_counter/Makefile new file mode 100644 index 000000000..b47fb0e9d --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_counter/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= step_counter.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_counter/step_counter.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_counter/step_counter.c new file mode 100644 index 000000000..b08f958a2 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_counter/step_counter.c @@ -0,0 +1,146 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270.h" +#include "common.h" + +/******************************************************************************/ +/*! Static function declaration */ + +/*! + * @brief This internal API is used to set configurations for step counter. + * + * @param[in] bmi2_dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_feature_config(struct bmi2_dev *bmi2_dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Structure to define type of sensor and their respective data. */ + struct bmi2_sensor_data sensor_data; + + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Accel sensor and step counter feature are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_STEP_COUNTER }; + + /* Variable to get step counter interrupt status. */ + uint16_t int_status = 0; + + /* Select features and their pins to be mapped to. */ + struct bmi2_sens_int_config sens_int = { .type = BMI2_STEP_COUNTER, .hw_int_pin = BMI2_INT2 }; + + /* Type of sensor to get step counter data. */ + sensor_data.type = BMI2_STEP_COUNTER; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the selected sensor. */ + rslt = bmi270_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Set the feature configuration for step counter. */ + rslt = set_feature_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("Step counter watermark level set to 1 (20 steps)\n"); + + if (rslt == BMI2_OK) + { + /* Map the step counter feature interrupt. */ + rslt = bmi270_map_feat_int(&sens_int, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Move the board in steps for 20 times to get step counter interrupt. */ + printf("Move the board in steps\n"); + + /* Loop to get number of steps counted. */ + do + { + /* To get the interrupt status of the step counter. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the interrupt status of the step counter. */ + if (int_status & BMI270_STEP_CNT_STATUS_MASK) + { + printf("Step counter interrupt occurred when watermark level (20 steps) is reached\n"); + + /* Get step counter output. */ + rslt = bmi270_get_sensor_data(&sensor_data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Print the step counter output. */ + printf("No of steps counted = %ld", sensor_data.sens_data.step_counter_output); + break; + } + } while (rslt == BMI2_OK); + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for step counter. + */ +static int8_t set_feature_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define the type of sensor and its configurations. */ + struct bmi2_sens_config config; + + /* Configure the type of sensor. */ + config.type = BMI2_STEP_COUNTER; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_get_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Setting water-mark level to 1 for step counter to get interrupt after 20 step counts. Every 20 steps once + * output triggers. */ + config.cfg.step_counter.watermark_level = 1; + + /* Set new configuration. */ + rslt = bmi270_set_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_gesture/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_gesture/Makefile new file mode 100644 index 000000000..0f959907b --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_gesture/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= wrist_gesture.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_gesture/wrist_gesture.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_gesture/wrist_gesture.c new file mode 100644 index 000000000..be494b8be --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_gesture/wrist_gesture.c @@ -0,0 +1,147 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270.h" +#include "common.h" + +/******************************************************************************/ +/*! Static function declaration */ + +/*! + * @brief This internal API is used to set the sensor configuration. + * + * @param[in] bmi2_dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t bmi2_set_config(struct bmi2_dev *bmi2_dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program */ +int main(void) +{ + /* Variable to define result */ + int8_t rslt; + + /* Initialize status of wrist gesture interrupt */ + uint16_t int_status = 0; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Select features and their pins to be mapped to */ + struct bmi2_sens_int_config sens_int = { .type = BMI2_WRIST_GESTURE, .hw_int_pin = BMI2_INT1 }; + + /* Sensor data structure */ + struct bmi2_sensor_data sens_data = { 0 }; + + /* The gesture movements are listed in array */ + const char *gesture_output[6] = + { "unknown_gesture", "push_arm_down", "pivot_up", "wrist_shake_jiggle", "flick_in", "flick_out" }; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + sens_data.type = BMI2_WRIST_GESTURE; + + /* Set the sensor configuration */ + rslt = bmi2_set_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Map the feature interrupt */ + rslt = bmi270_map_feat_int(&sens_int, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("Flip the board in portrait/landscape mode:\n"); + + /* Loop to print the wrist gesture data when interrupt occurs */ + while (1) + { + /* Get the interrupt status of the wrist gesture */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if ((rslt == BMI2_OK) && (int_status & BMI270_WRIST_GEST_STATUS_MASK)) + { + printf("Wrist gesture detected\n"); + + /* Get wrist gesture output */ + rslt = bmi270_get_sensor_data(&sens_data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("Wrist gesture = %d\r\n", sens_data.sens_data.wrist_gesture_output); + + printf("Gesture output = %s\n", gesture_output[sens_data.sens_data.wrist_gesture_output]); + break; + } + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API sets the sensor configuration + */ +static int8_t bmi2_set_config(struct bmi2_dev *bmi2_dev) +{ + /* Variable to define result */ + int8_t rslt; + + /* List the sensors which are required to enable */ + uint8_t sens_list[2] = { BMI2_ACCEL, BMI2_WRIST_GESTURE }; + + /* Structure to define the type of the sensor and its configurations */ + struct bmi2_sens_config config; + + /* Configure type of feature */ + config.type = BMI2_WRIST_GESTURE; + + /* Enable the selected sensors */ + rslt = bmi270_sensor_enable(sens_list, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Get default configurations for the type of feature selected */ + rslt = bmi270_get_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + config.cfg.wrist_gest.wearable_arm = BMI2_ARM_LEFT; + + /* Set the new configuration along with interrupt mapping */ + rslt = bmi270_set_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_wear_wakeup/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_wear_wakeup/Makefile new file mode 100644 index 000000000..1e20da2ea --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_wear_wakeup/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= wrist_wear_wakeup.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_wear_wakeup/wrist_wear_wakeup.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_wear_wakeup/wrist_wear_wakeup.c new file mode 100644 index 000000000..3547b2d30 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_wear_wakeup/wrist_wear_wakeup.c @@ -0,0 +1,131 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270.h" +#include "common.h" + +/******************************************************************************/ +/*! Static function declaration */ + +/*! + * @brief This internal API is used to set the sensor configuration. + * + * @param[in] bmi2_dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t bmi2_set_config(struct bmi2_dev *bmi2_dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program */ +int main(void) +{ + /* Variable to define result */ + int8_t rslt; + + /* Initialize status of wrist wear wakeup interrupt */ + uint16_t int_status = 0; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Select features and their pins to be mapped to */ + struct bmi2_sens_int_config sens_int = { .type = BMI2_WRIST_WEAR_WAKE_UP, .hw_int_pin = BMI2_INT1 }; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Set the sensor configuration */ + rslt = bmi2_set_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Map the feature interrupt */ + rslt = bmi270_map_feat_int(&sens_int, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("Tilt the board in landscape position to trigger wrist wear wakeup\n"); + + /* Loop to print the wrist wear wakeup data when interrupt occurs */ + while (1) + { + int_status = 0; + + /* To get the interrupt status of the wrist wear wakeup */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the interrupt status of the wrist gesture */ + if ((rslt == BMI2_OK) && (int_status & BMI270_WRIST_WAKE_UP_STATUS_MASK)) + { + printf("Wrist wear wakeup detected\n"); + break; + } + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API sets the sensor configuration + */ +static int8_t bmi2_set_config(struct bmi2_dev *bmi2_dev) +{ + /* Variable to define result */ + int8_t rslt; + + /* List the sensors which are required to enable */ + uint8_t sens_list[2] = { BMI2_ACCEL, BMI2_WRIST_WEAR_WAKE_UP }; + + /* Structure to define the type of the sensor and its configurations */ + struct bmi2_sens_config config; + + /* Configure type of feature */ + config.type = BMI2_WRIST_WEAR_WAKE_UP; + + /* Enable the selected sensors */ + rslt = bmi270_sensor_enable(sens_list, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Get default configurations for the type of feature selected */ + rslt = bmi270_get_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Set the new configuration */ + rslt = bmi270_set_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel/Makefile new file mode 100644 index 000000000..d21517245 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= accel.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_context.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel/accel.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel/accel.c new file mode 100644 index 000000000..5b36fffe4 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel/accel.c @@ -0,0 +1,200 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_context.h" +#include "common.h" + +/******************************************************************************/ +/*! Macro definition */ + +/*! Earth's gravity in m/s^2 */ +#define GRAVITY_EARTH (9.80665f) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_accel_config(struct bmi2_dev *bmi2_dev); + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + * + * @param[in] val : LSB from each axis. + * @param[in] g_range : Gravity range. + * @param[in] bit_width : Resolution for accel. + * + * @return Gravity. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to define limit to print accel data. */ + uint8_t limit = 20; + + /* Assign accel sensor to variable. */ + uint8_t sensor_list = BMI2_ACCEL; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Create an instance of sensor data structure. */ + struct bmi2_sensor_data sensor_data = { 0 }; + + /* Initialize the interrupt status of accel. */ + uint16_t int_status = 0; + + uint8_t indx = 0; + float x = 0, y = 0, z = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_context. */ + rslt = bmi270_context_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the accel sensor. */ + rslt = bmi270_context_sensor_enable(&sensor_list, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Accel configuration settings. */ + rslt = set_accel_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Assign accel sensor. */ + sensor_data.type = BMI2_ACCEL; + printf("Accel and m/s2 data \n"); + printf("Accel data collected at 2G Range with 16-bit resolution\n"); + + /* Loop to print the accel data when interrupt occurs. */ + while (indx <= limit) + { + /* To get the status of accel data ready interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the accel data ready interrupt status and print the status for 10 samples. */ + if (int_status & BMI2_ACC_DRDY_INT_MASK) + { + /* Get accelerometer data for x, y and z axis. */ + rslt = bmi270_context_get_sensor_data(&sensor_data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + printf("\nAcc_X = %d\t", sensor_data.sens_data.acc.x); + printf("Acc_Y = %d\t", sensor_data.sens_data.acc.y); + printf("Acc_Z = %d\r\n", sensor_data.sens_data.acc.z); + + /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ + x = lsb_to_mps2(sensor_data.sens_data.acc.x, 2, bmi2_dev.resolution); + y = lsb_to_mps2(sensor_data.sens_data.acc.y, 2, bmi2_dev.resolution); + z = lsb_to_mps2(sensor_data.sens_data.acc.z, 2, bmi2_dev.resolution); + + /* Print the data in m/s2. */ + printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); + + indx++; + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel. + */ +static int8_t set_accel_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accelerometer configuration. */ + struct bmi2_sens_config config; + + /* Configure the type of feature. */ + config.type = BMI2_ACCEL; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_context_get_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config.cfg.acc.odr = BMI2_ACC_ODR_200HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config.cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples. + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config.cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config.cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set the accel configurations. */ + rslt = bmi270_context_set_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Map data ready interrupt to interrupt pin. */ + rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (GRAVITY_EARTH * val * g_range) / half_scale; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel_gyro/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel_gyro/Makefile new file mode 100644 index 000000000..2eed19561 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel_gyro/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= accel_gyro.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_context.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel_gyro/accel_gyro.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel_gyro/accel_gyro.c new file mode 100644 index 000000000..7458969ef --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel_gyro/accel_gyro.c @@ -0,0 +1,268 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_context.h" +#include "common.h" + +/******************************************************************************/ +/*! Macro definition */ + +/*! Earth's gravity in m/s^2 */ +#define GRAVITY_EARTH (9.80665f) + +/*! Macros to select the sensors */ +#define ACCEL UINT8_C(0x00) +#define GYRO UINT8_C(0x01) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev); + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + * + * @param[in] val : LSB from each axis. + * @param[in] g_range : Gravity range. + * @param[in] bit_width : Resolution for accel. + * + * @return Gravity. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + * + * @param[in] val : LSB from each axis. + * @param[in] dps : Degree per second. + * @param[in] bit_width : Resolution for gyro. + * + * @return Degree per second. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to define limit to print accel data. */ + uint8_t limit = 10; + + /* Assign accel and gyro sensor to variable. */ + uint8_t sensor_list[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Create an instance of sensor data structure. */ + struct bmi2_sensor_data sensor_data[2] = { { 0 } }; + + /* Initialize the interrupt status of accel and gyro. */ + uint16_t int_status = 0; + + uint8_t indx = 1; + + float x = 0, y = 0, z = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_context. */ + rslt = bmi270_context_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the accel and gyro sensor. */ + rslt = bmi270_context_sensor_enable(sensor_list, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Accel and gyro configuration settings. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Assign accel and gyro sensor. */ + sensor_data[ACCEL].type = BMI2_ACCEL; + sensor_data[GYRO].type = BMI2_GYRO; + + /* Loop to print accel and gyro data when interrupt occurs. */ + while (indx <= limit) + { + /* To get the data ready interrupt status of accel and gyro. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the data ready interrupt status and print the status for 10 samples. */ + if ((int_status & BMI2_ACC_DRDY_INT_MASK) && (int_status & BMI2_GYR_DRDY_INT_MASK)) + { + /* Get accel and gyro data for x, y and z axis. */ + rslt = bmi270_context_get_sensor_data(sensor_data, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("\n******* Accel(Raw and m/s2) Gyro(Raw and dps) data : %d *******\n", indx); + + printf("\nAcc_x = %d\t", sensor_data[ACCEL].sens_data.acc.x); + printf("Acc_y = %d\t", sensor_data[ACCEL].sens_data.acc.y); + printf("Acc_z = %d", sensor_data[ACCEL].sens_data.acc.z); + + /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ + x = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.x, 2, bmi2_dev.resolution); + y = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.y, 2, bmi2_dev.resolution); + z = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.z, 2, bmi2_dev.resolution); + + /* Print the data in m/s2. */ + printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); + + printf("\nGyr_X = %d\t", sensor_data[GYRO].sens_data.gyr.x); + printf("Gyr_Y = %d\t", sensor_data[GYRO].sens_data.gyr.y); + printf("Gyr_Z = %d\n", sensor_data[GYRO].sens_data.gyr.z); + + /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ + x = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.x, 2000, bmi2_dev.resolution); + y = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.y, 2000, bmi2_dev.resolution); + z = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.z, 2000, bmi2_dev.resolution); + + /* Print the data in dps. */ + printf("Gyro_DPS_X = %4.2f, Gyro_DPS_Y = %4.2f, Gyro_DPS_Z = %4.2f\n", x, y, z); + + indx++; + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accelerometer and gyro configuration. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[ACCEL].type = BMI2_ACCEL; + config[GYRO].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_context_get_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Map data ready interrupt to interrupt pin. */ + rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config[ACCEL].cfg.acc.odr = BMI2_ACC_ODR_200HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[ACCEL].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples. + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[ACCEL].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[ACCEL].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config[GYRO].cfg.gyr.odr = BMI2_GYR_ODR_200HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[GYRO].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[GYRO].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[GYRO].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[GYRO].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set the accel and gyro configurations. */ + rslt = bmi270_context_set_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (GRAVITY_EARTH * val * g_range) / half_scale; +} + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/activity_recognition/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/activity_recognition/Makefile new file mode 100644 index 000000000..28ed939cd --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/activity_recognition/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= activity_recognition.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_context.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/activity_recognition/activity_recognition.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/activity_recognition/activity_recognition.c new file mode 100644 index 000000000..9f0714d38 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/activity_recognition/activity_recognition.c @@ -0,0 +1,154 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_context.h" +#include "common.h" + +/******************************************************************************/ +/*! Static function declaration */ + +/*! + * @brief This internal API is used to set configurations for step counter. + * + * @param[in] bmi2_dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_config(struct bmi2_dev *bmi2_dev); + +/******************************************************************************/ +/*! Functions */ +/* This function starts the execution of program. */ +int main(void) +{ + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to define index for FIFO data */ + uint8_t count; + + /* Various Activity recognitions are listed in array. */ + const char *activity_reg_output[6] = { "OTHERS", "STILL", "WALKING", "RUNNING", "ON_BICYCLE", "IN_VEHICLE" }; + + /* Accel sensor and step activity feature are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_ACTIVITY_RECOGNITION }; + + /* Array to define FIFO data */ + uint8_t fifo_data[516] = { 0 }; + + /* Variable to store number of activity frames */ + uint16_t act_frames; + + /* Structure to define a FIFO */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Structure to define output for activity recognition */ + struct bmi2_act_recog_output act_recog_data[5] = { { 0 } }; + + uint16_t fifo_length; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_context. */ + rslt = bmi270_context_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the selected sensor. */ + rslt = bmi270_context_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + rslt = set_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Update FIFO structure */ + fifoframe.data = fifo_data; + fifoframe.length = 516; + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + printf("Fifo length = %d \n", fifo_length); + + /* Read FIFO data */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + while (rslt != BMI2_W_FIFO_EMPTY) + { + /* Provide the number of frames to be read */ + act_frames = 5; + + printf("Requested FIFO data frames : %d\n", act_frames); + + /* Get the activity output */ + rslt = bmi270_context_get_act_recog_output(act_recog_data, &act_frames, &fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("Parsed FIFO data frames : %d\r\n", act_frames); + + for (count = 0; count < act_frames; count++) + { + printf( + "Activity Recognition output[%d]:Sensor time: %lu\t Previous activity: %s\t current: %s\n", + count, + act_recog_data[count].time_stamp, + activity_reg_output[act_recog_data[count].prev_act], + activity_reg_output[act_recog_data[count].curr_act]); + } + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +static int8_t set_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + uint8_t temp_data[2]; + + /* Disable advanced power save */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Reset FIFO */ + rslt = bmi2_set_command_register(BMI2_FIFO_FLUSH_CMD, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("Move the board for activity recognition for 30 sec :\n"); + bmi2_dev->delay_us(30000000, bmi2_dev->intf_ptr); + + if (rslt == BMI2_OK) + { + rslt = bmi2_get_regs(BMI2_FIFO_CONFIG_0_ADDR, temp_data, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/common/common.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/common/common.c new file mode 100644 index 000000000..afff37bc8 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/common/common.c @@ -0,0 +1,372 @@ +/** + * Copyright (C) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +#include +#include +#include + +#include "coines.h" +#include "bmi2_defs.h" + +/******************************************************************************/ +/*! Macro definitions */ +#define BMI2XY_SHUTTLE_ID UINT16_C(0x1B8) + +/*! Macro that defines read write length */ +#define READ_WRITE_LEN UINT8_C(46) + +/******************************************************************************/ +/*! Static variable definition */ + +/*! Variable that holds the I2C device address or SPI chip selection */ +static uint8_t dev_addr; + +/******************************************************************************/ +/*! User interface functions */ + +/*! + * I2C read function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_read_i2c(dev_addr, reg_addr, reg_data, (uint16_t)len); +} + +/*! + * I2C write function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_write_i2c(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len); +} + +/*! + * SPI read function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_read_spi(dev_addr, reg_addr, reg_data, (uint16_t)len); +} + +/*! + * SPI write function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_write_spi(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len); +} + +/*! + * Delay function map to COINES platform + */ +void bmi2_delay_us(uint32_t period, void *intf_ptr) +{ + coines_delay_usec(period); +} + +/*! + * @brief Function to select the interface between SPI and I2C. + * Also to initialize coines platform + */ +int8_t bmi2_interface_init(struct bmi2_dev *bmi, uint8_t intf) +{ + int8_t rslt = BMI2_OK; + struct coines_board_info board_info; + + if (bmi != NULL) + { + int16_t result = coines_open_comm_intf(COINES_COMM_INTF_USB); + if (result < COINES_SUCCESS) + { + printf( + "\n Unable to connect with Application Board ! \n" " 1. Check if the board is connected and powered on. \n" " 2. Check if Application Board USB driver is installed. \n" + " 3. Check if board is in use by another application. (Insufficient permissions to access USB) \n"); + exit(result); + } + + result = coines_get_board_info(&board_info); + +#if defined(PC) + setbuf(stdout, NULL); +#endif + + if (result == COINES_SUCCESS) + { + if ((board_info.shuttle_id != BMI2XY_SHUTTLE_ID)) + { + printf("! Warning invalid sensor shuttle \n ," "This application will not support this sensor \n"); + exit(COINES_E_FAILURE); + } + } + + coines_set_shuttleboard_vdd_vddio_config(0, 0); + coines_delay_msec(100); + + /* Bus configuration : I2C */ + if (intf == BMI2_I2C_INTF) + { + printf("I2C Interface \n"); + + /* To initialize the user I2C function */ + dev_addr = BMI2_I2C_PRIM_ADDR; + bmi->intf = BMI2_I2C_INTF; + bmi->read = bmi2_i2c_read; + bmi->write = bmi2_i2c_write; + coines_config_i2c_bus(COINES_I2C_BUS_0, COINES_I2C_FAST_MODE); + } + /* Bus configuration : SPI */ + else if (intf == BMI2_SPI_INTF) + { + printf("SPI Interface \n"); + + /* To initialize the user SPI function */ + dev_addr = COINES_SHUTTLE_PIN_7; + bmi->intf = BMI2_SPI_INTF; + bmi->read = bmi2_spi_read; + bmi->write = bmi2_spi_write; + coines_config_spi_bus(COINES_SPI_BUS_0, COINES_SPI_SPEED_5_MHZ, COINES_SPI_MODE3); + coines_set_pin_config(COINES_SHUTTLE_PIN_7, COINES_PIN_DIRECTION_OUT, COINES_PIN_VALUE_HIGH); + } + + /* Assign device address to interface pointer */ + bmi->intf_ptr = &dev_addr; + + /* Configure delay in microseconds */ + bmi->delay_us = bmi2_delay_us; + + /* Configure max read/write length (in bytes) ( Supported length depends on target machine) */ + bmi->read_write_len = READ_WRITE_LEN; + + /* Assign to NULL to load the default config file. */ + bmi->config_file_ptr = NULL; + + coines_delay_usec(10000); + + coines_set_shuttleboard_vdd_vddio_config(3300, 3300); + + coines_delay_usec(10000); + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; + +} + +/*! + * @brief Prints the execution status of the APIs. + */ +void bmi2_error_codes_print_result(int8_t rslt) +{ + switch (rslt) + { + case BMI2_OK: + + /* Do nothing */ + break; + + case BMI2_W_FIFO_EMPTY: + printf("Warning [%d] : FIFO empty\r\n", rslt); + break; + case BMI2_W_PARTIAL_READ: + printf("Warning [%d] : FIFO partial read\r\n", rslt); + break; + case BMI2_E_NULL_PTR: + printf( + "Error [%d] : Null pointer error. It occurs when the user tries to assign value (not address) to a pointer," " which has been initialized to NULL.\r\n", + rslt); + break; + + case BMI2_E_COM_FAIL: + printf( + "Error [%d] : Communication failure error. It occurs due to read/write operation failure and also due " "to power failure during communication\r\n", + rslt); + break; + + case BMI2_E_DEV_NOT_FOUND: + printf("Error [%d] : Device not found error. It occurs when the device chip id is incorrectly read\r\n", + rslt); + break; + + case BMI2_E_INVALID_SENSOR: + printf( + "Error [%d] : Invalid sensor error. It occurs when there is a mismatch in the requested feature with the " "available one\r\n", + rslt); + break; + + case BMI2_E_SELF_TEST_FAIL: + printf( + "Error [%d] : Self-test failed error. It occurs when the validation of accel self-test data is " "not satisfied\r\n", + rslt); + break; + + case BMI2_E_INVALID_INT_PIN: + printf( + "Error [%d] : Invalid interrupt pin error. It occurs when the user tries to configure interrupt pins " "apart from INT1 and INT2\r\n", + rslt); + break; + + case BMI2_E_OUT_OF_RANGE: + printf( + "Error [%d] : Out of range error. It occurs when the data exceeds from filtered or unfiltered data from " "fifo and also when the range exceeds the maximum range for accel and gyro while performing FOC\r\n", + rslt); + break; + + case BMI2_E_ACC_INVALID_CFG: + printf( + "Error [%d] : Invalid Accel configuration error. It occurs when there is an error in accel configuration" " register which could be one among range, BW or filter performance in reg address 0x40\r\n", + rslt); + break; + + case BMI2_E_GYRO_INVALID_CFG: + printf( + "Error [%d] : Invalid Gyro configuration error. It occurs when there is a error in gyro configuration" "register which could be one among range, BW or filter performance in reg address 0x42\r\n", + rslt); + break; + + case BMI2_E_ACC_GYR_INVALID_CFG: + printf( + "Error [%d] : Invalid Accel-Gyro configuration error. It occurs when there is a error in accel and gyro" " configuration registers which could be one among range, BW or filter performance in reg address 0x40 " "and 0x42\r\n", + rslt); + break; + + case BMI2_E_CONFIG_LOAD: + printf( + "Error [%d] : Configuration load error. It occurs when failure observed while loading the configuration " "into the sensor\r\n", + rslt); + break; + + case BMI2_E_INVALID_PAGE: + printf( + "Error [%d] : Invalid page error. It occurs due to failure in writing the correct feature configuration " "from selected page\r\n", + rslt); + break; + + case BMI2_E_SET_APS_FAIL: + printf( + "Error [%d] : APS failure error. It occurs due to failure in write of advance power mode configuration " "register\r\n", + rslt); + break; + + case BMI2_E_AUX_INVALID_CFG: + printf( + "Error [%d] : Invalid AUX configuration error. It occurs when the auxiliary interface settings are not " "enabled properly\r\n", + rslt); + break; + + case BMI2_E_AUX_BUSY: + printf( + "Error [%d] : AUX busy error. It occurs when the auxiliary interface buses are engaged while configuring" " the AUX\r\n", + rslt); + break; + + case BMI2_E_REMAP_ERROR: + printf( + "Error [%d] : Remap error. It occurs due to failure in assigning the remap axes data for all the axes " "after change in axis position\r\n", + rslt); + break; + + case BMI2_E_GYR_USER_GAIN_UPD_FAIL: + printf( + "Error [%d] : Gyro user gain update fail error. It occurs when the reading of user gain update status " "fails\r\n", + rslt); + break; + + case BMI2_E_SELF_TEST_NOT_DONE: + printf( + "Error [%d] : Self-test not done error. It occurs when the self-test process is ongoing or not " "completed\r\n", + rslt); + break; + + case BMI2_E_INVALID_INPUT: + printf("Error [%d] : Invalid input error. It occurs when the sensor input validity fails\r\n", rslt); + break; + + case BMI2_E_INVALID_STATUS: + printf("Error [%d] : Invalid status error. It occurs when the feature/sensor validity fails\r\n", rslt); + break; + + case BMI2_E_CRT_ERROR: + printf("Error [%d] : CRT error. It occurs when the CRT test has failed\r\n", rslt); + break; + + case BMI2_E_ST_ALREADY_RUNNING: + printf( + "Error [%d] : Self-test already running error. It occurs when the self-test is already running and " "another has been initiated\r\n", + rslt); + break; + + case BMI2_E_CRT_READY_FOR_DL_FAIL_ABORT: + printf( + "Error [%d] : CRT ready for download fail abort error. It occurs when download in CRT fails due to wrong " "address location\r\n", + rslt); + break; + + case BMI2_E_DL_ERROR: + printf( + "Error [%d] : Download error. It occurs when write length exceeds that of the maximum burst length\r\n", + rslt); + break; + + case BMI2_E_PRECON_ERROR: + printf( + "Error [%d] : Pre-conditional error. It occurs when precondition to start the feature was not " "completed\r\n", + rslt); + break; + + case BMI2_E_ABORT_ERROR: + printf("Error [%d] : Abort error. It occurs when the device was shaken during CRT test\r\n", rslt); + break; + + case BMI2_E_WRITE_CYCLE_ONGOING: + printf( + "Error [%d] : Write cycle ongoing error. It occurs when the write cycle is already running and another " "has been initiated\r\n", + rslt); + break; + + case BMI2_E_ST_NOT_RUNING: + printf( + "Error [%d] : Self-test is not running error. It occurs when self-test running is disabled while it's " "running\r\n", + rslt); + break; + + case BMI2_E_DATA_RDY_INT_FAILED: + printf( + "Error [%d] : Data ready interrupt error. It occurs when the sample count exceeds the FOC sample limit " "and data ready status is not updated\r\n", + rslt); + break; + + case BMI2_E_INVALID_FOC_POSITION: + printf( + "Error [%d] : Invalid FOC position error. It occurs when average FOC data is obtained for the wrong" " axes\r\n", + rslt); + break; + + default: + printf("Error [%d] : Unknown error code\r\n", rslt); + break; + } +} + +/*! + * @brief Deinitializes coines platform + * + * @return void. + */ +void bmi2_coines_deinit(void) +{ + coines_close_comm_intf(COINES_COMM_INTF_USB); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/common/common.h b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/common/common.h new file mode 100644 index 000000000..763983da4 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/common/common.h @@ -0,0 +1,122 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +#ifndef _COMMON_H +#define _COMMON_H + +/*! CPP guard */ +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include "bmi2.h" + +/*! + * @brief Function for reading the sensor's registers through I2C bus. + * + * @param[in] reg_addr : Register address. + * @param[out] reg_data : Pointer to the data buffer to store the read data. + * @param[in] length : No of bytes to read. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief Function for writing the sensor's registers through I2C bus. + * + * @param[in] reg_addr : Register address. + * @param[in] reg_data : Pointer to the data buffer whose value is to be written. + * @param[in] length : No of bytes to write. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief Function for reading the sensor's registers through SPI bus. + * + * @param[in] reg_addr : Register address. + * @param[out] reg_data : Pointer to the data buffer to store the read data. + * @param[in] length : No of bytes to read. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief Function for writing the sensor's registers through SPI bus. + * + * @param[in] reg_addr : Register address. + * @param[in] reg_data : Pointer to the data buffer whose data has to be written. + * @param[in] length : No of bytes to write. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief This function provides the delay for required time (Microsecond) as per the input provided in some of the + * APIs. + * + * @param[in] period_us : The required wait time in microsecond. + * @param[in] intf_ptr : Interface pointer + * + * @return void. + * + */ +void bmi2_delay_us(uint32_t period, void *intf_ptr); + +/*! + * @brief Function to select the interface between SPI and I2C. + * + * @param[in] bma : Structure instance of bmi2_dev + * @param[in] intf : Interface selection parameter + * + * @return Status of execution + * @retval 0 -> Success + * @retval < 0 -> Failure Info + */ +int8_t bmi2_interface_init(struct bmi2_dev *bma, uint8_t intf); + +/*! + * @brief Prints the execution status of the APIs. + * + * @param[in] rslt : Error code returned by the API whose execution status has to be printed. + * + * @return void. + */ +void bmi2_error_codes_print_result(int8_t rslt); + +/*! + * @brief Deinitializes coines platform + * + * @return void. + */ +void bmi2_coines_deinit(void); + +#ifdef __cplusplus +} +#endif /* End of CPP guard */ + +#endif /* _COMMON_H */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/component_retrim/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/component_retrim/Makefile new file mode 100644 index 000000000..61db2e135 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/component_retrim/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= component_retrim.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_context.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/component_retrim/component_retrim.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/component_retrim/component_retrim.c new file mode 100644 index 000000000..94ed04377 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/component_retrim/component_retrim.c @@ -0,0 +1,52 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_context.h" +#include "common.h" + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_context. */ + rslt = bmi270_context_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* This API runs the CRT process. */ + rslt = bmi2_do_crt(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Do not move the board while doing CRT. If so, it will throw an abort error. */ + if (rslt == BMI2_OK) + { + printf("CRT successfully completed."); + } + } + + bmi2_coines_deinit(); + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_header_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_header_mode/Makefile new file mode 100644 index 000000000..2d328d5f8 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_header_mode/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= fifo_full_header_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_context.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_header_mode/fifo_full_header_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_header_mode/fifo_full_header_mode.c new file mode 100644 index 000000000..3a5a810ec --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_header_mode/fifo_full_header_mode.c @@ -0,0 +1,315 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_context.h" +#include "common.h" + +/******************************************************************************/ +/*! Macros */ + +/*! Buffer size allocated to store raw FIFO data. */ +#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) + +/*! Length of data to be read from FIFO. */ +#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) + +/*! Number of accel frames to be extracted from FIFO. */ + +/*! Calculation for frame count: Total frame count = Fifo buffer size(2048)/ Total frames(6 Accel, 6 Gyro and 1 header, + * totaling to 13) which equals to 157. + */ +#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(157) + +/*! Number of gyro frames to be extracted from FIFO. */ +#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(157) + +/*! Macro to read sensortime byte in FIFO. */ +#define SENSORTIME_OVERHEAD_BYTE UINT8_C(3) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + * @param[in] dev : Structure instance of bmi2_dev. + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev); + +/******************************************************************************/ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + uint16_t index = 0; + uint16_t fifo_length = 0; + uint16_t config = 0; + + /* To read sensortime, extra 3 bytes are added to fifo buffer. */ + uint16_t fifo_buffer_size = BMI2_FIFO_RAW_DATA_BUFFER_SIZE + SENSORTIME_OVERHEAD_BYTE; + + /* Variable to get fifo full interrupt status. */ + uint16_t int_status = 0; + + uint16_t accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + + uint16_t gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + /* Variable to store the available frame length count in FIFO */ + uint8_t frame_count; + + int8_t try = 1; + + /* Number of bytes of FIFO data. */ + uint8_t fifo_data[fifo_buffer_size]; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Array of accelerometer frames -> Total bytes = + * 157 * (6 axes + 1 header bytes) = 1099 bytes */ + struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; + + /* Array of gyro frames -> Total bytes = + * 157 * (6 axes + 1 header bytes) = 1099 bytes */ + struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; + + /* Initialize FIFO frame structure. */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Accel and gyro sensor are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_context. */ + rslt = bmi270_context_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable accel and gyro sensor. */ + rslt = bmi270_context_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configuration settings for accel and gyro. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Before setting FIFO, disable the advance power save mode. */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_get_fifo_config(&config, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Update FIFO structure. */ + /* Mapping the buffer to store the fifo data. */ + fifoframe.data = fifo_data; + + /* Length of FIFO frame. */ + /* To read sensortime, extra 3 bytes are added to fifo user length. */ + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH + SENSORTIME_OVERHEAD_BYTE; + + /* Set FIFO configuration by enabling accel, gyro and timestamp. + * NOTE 1: The header mode is enabled by default. + * NOTE 2: By default the FIFO operating mode is in FIFO mode. + * NOTE 3: Sensortime is enabled by default */ + printf("FIFO is configured in header mode\n"); + rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Map FIFO full interrupt. */ + fifoframe.data_int_map = BMI2_FFULL_INT; + rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + while (try <= 10) + { + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if ((rslt == BMI2_OK) && (int_status & BMI2_FFULL_INT_STATUS_MASK)) + { + printf("\nIteration : %d\n", try); + + accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + + gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("\nFIFO data bytes available : %d \n", fifo_length); + printf("\nFIFO data bytes requested : %d \n", fifoframe.length); + + /* Read FIFO data. */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nFIFO accel frames requested : %d \n", accel_frame_length); + + /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ + rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); + + printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); + + /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ + rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); + + /* Calculating the frame count from the available bytes in FIFO + * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len + header_byte)) */ + frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH + 1)); + printf("\nAvailable frame count from available bytes: %d\n", frame_count); + + printf("\nExtracted accel frames\n"); + + if (accel_frame_length > frame_count) + { + accel_frame_length = frame_count; + } + + /* Print the parsed accelerometer data from the FIFO buffer. */ + for (index = 0; index < accel_frame_length; index++) + { + printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, + fifo_accel_data[index].y, fifo_accel_data[index].z); + } + + printf("\nExtracted gyro frames\n"); + + if (gyro_frame_length > frame_count) + { + gyro_frame_length = frame_count; + } + + /* Print the parsed gyro data from the FIFO buffer. */ + for (index = 0; index < gyro_frame_length; index++) + { + printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", + index, + fifo_gyro_data[index].x, + fifo_gyro_data[index].y, + fifo_gyro_data[index].z); + } + + /* Print control frames like sensor time and skipped frame count. */ + printf("\nSkipped frame count = %d\n", fifoframe.skipped_frame_count); + + printf("Sensor time = %lu\r\n", fifoframe.sensor_time); + + try++; + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accel and gyro configurations. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[0].type = BMI2_ACCEL; + config[1].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_context_get_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Accel configuration settings. */ + /* Set Output Data Rate */ + config[0].cfg.acc.odr = BMI2_ACC_ODR_25HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Gyro configuration settings. */ + /* Set Output Data Rate */ + config[1].cfg.gyr.odr = BMI2_GYR_ODR_50HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set new configurations. */ + rslt = bmi270_context_set_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_headerless_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_headerless_mode/Makefile new file mode 100644 index 000000000..8f1f9609d --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_headerless_mode/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= fifo_full_headerless_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_context.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_headerless_mode/fifo_full_headerless_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_headerless_mode/fifo_full_headerless_mode.c new file mode 100644 index 000000000..bfc11455a --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_headerless_mode/fifo_full_headerless_mode.c @@ -0,0 +1,304 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_context.h" +#include "common.h" + +/******************************************************************************/ +/*! Macros */ + +/*! Buffer size allocated to store raw FIFO data */ +#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) + +/*! Length of data to be read from FIFO */ +#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) + +/*! Number of accel frames to be extracted from FIFO */ + +/*! Calculation for frame count: Total frame count = Fifo buffer size(2048)/ Total frames(6 Accel, 6 Gyro totaling to + * 12) which equals to 170. + */ +#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(169) + +/*! Number of gyro frames to be extracted from FIFO */ +#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(169) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + * @param[in] dev : Structure instance of bmi2_dev. + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev); + +/******************************************************************************/ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + uint16_t index = 0; + + uint16_t fifo_length = 0; + + uint8_t try = 1; + + /* Variable to get fifo full interrupt status. */ + uint16_t int_status = 0; + + uint16_t accel_frame_length; + + uint16_t gyro_frame_length; + + /* Variable to store the available frame length count in FIFO */ + uint8_t frame_count; + + /* Number of bytes of FIFO data. */ + uint8_t fifo_data[BMI2_FIFO_RAW_DATA_BUFFER_SIZE] = { 0 }; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Array of accelerometer frames -> Total bytes = + * 170 * (6 axes bytes) = 1020 bytes */ + struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; + + /* Array of gyro frames -> Total bytes = + * 170 * (6 axes bytes) = 1020 bytes */ + struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; + + /* Initialize FIFO frame structure. */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Accel and gyro sensor are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_context. */ + rslt = bmi270_context_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable accel and gyro sensor. */ + rslt = bmi270_context_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configuration settings for accel and gyro. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Before setting FIFO, disable the advance power save mode. */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Update FIFO structure. */ + /* Mapping the buffer to store the fifo data. */ + fifoframe.data = fifo_data; + + /* Length of FIFO frame. */ + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; + + /* Set FIFO configuration by enabling accel, gyro. + * NOTE 1: The header mode is enabled by default. + * NOTE 2: By default the FIFO operating mode is FIFO mode. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("FIFO is configured in headerless mode\n"); + + /* To enable headerless mode, disable the header. */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_fifo_config(BMI2_FIFO_HEADER_EN, BMI2_DISABLE, &bmi2_dev); + } + + /* Map FIFO full interrupt. */ + fifoframe.data_int_map = BMI2_FFULL_INT; + rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + while (try <= 10) + { + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if ((rslt == BMI2_OK) && (int_status & BMI2_FFULL_INT_STATUS_MASK)) + { + printf("\nIteration : %d\n", try); + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + printf("\nFIFO data bytes available : %d \n", fifo_length); + printf("\nFIFO data bytes requested : %d \n", fifoframe.length); + + /* Read FIFO data. */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nFIFO accel frames requested : %d \n", accel_frame_length); + + /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ + rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); + + printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); + + /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ + rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); + + /* Calculating the frame count from the available bytes in FIFO + * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len)) */ + frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH)); + printf("\nAvailable frame count from available bytes: %d\n", frame_count); + + printf("\nExtracted accel frames\n"); + + if (accel_frame_length > frame_count) + { + accel_frame_length = frame_count; + } + + /* Print the parsed accelerometer data from the FIFO buffer. */ + for (index = 0; index < accel_frame_length; index++) + { + printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, + fifo_accel_data[index].y, fifo_accel_data[index].z); + } + + printf("\nExtracted gyro frames\n"); + + if (gyro_frame_length > frame_count) + { + gyro_frame_length = frame_count; + } + + /* Print the parsed gyro data from the FIFO buffer. */ + for (index = 0; index < gyro_frame_length; index++) + { + printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", + index, + fifo_gyro_data[index].x, + fifo_gyro_data[index].y, + fifo_gyro_data[index].z); + } + } + + try++; + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accel and gyro configurations. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[0].type = BMI2_ACCEL; + config[1].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_context_get_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Accel configuration settings. */ + /* Set Output Data Rate */ + config[0].cfg.acc.odr = BMI2_ACC_ODR_100HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Gyro configuration settings. */ + /* Set Output Data Rate */ + config[1].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set new configurations. */ + rslt = bmi270_context_set_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_header_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_header_mode/Makefile new file mode 100644 index 000000000..38c5ce6a5 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_header_mode/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= fifo_watermark_header_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_context.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_header_mode/fifo_watermark_header_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_header_mode/fifo_watermark_header_mode.c new file mode 100644 index 000000000..59ff8f758 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_header_mode/fifo_watermark_header_mode.c @@ -0,0 +1,335 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_context.h" +#include "common.h" + +/******************************************************************************/ +/*! Macros */ + +/*! Buffer size allocated to store raw FIFO data */ +#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) + +/*! Length of data to be read from FIFO */ +#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) + +/*! Number of accel frames to be extracted from FIFO */ + +/*! + * Calculation: + * fifo_watermark_level = 650, accel_frame_len = 6, gyro_frame_len = 6 header_byte = 1. + * fifo_accel_frame_count = (650 / (6 + 6 + 1 )) = 50 frames + * NOTE: Extra frames are read in order to get sensor time + */ +#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(55) + +/*! Number of gyro frames to be extracted from FIFO */ +#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(55) + +/*! Setting a watermark level in FIFO */ +#define BMI270_FIFO_WATERMARK_LEVEL UINT16_C(650) + +/*! Macro to read sensortime byte in FIFO */ +#define SENSORTIME_OVERHEAD_BYTE UINT8_C(3) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + * @param[in] dev : Structure instance of bmi2_dev. + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to store the available frame length count in FIFO */ + uint8_t frame_count; + + /* Variable to index bytes. */ + uint16_t index = 0; + + uint16_t fifo_length = 0; + + uint16_t accel_frame_length; + + uint16_t gyro_frame_length; + + uint8_t try = 1; + + /* To read sensortime, extra 3 bytes are added to fifo buffer */ + uint16_t fifo_buffer_size = BMI2_FIFO_RAW_DATA_BUFFER_SIZE + SENSORTIME_OVERHEAD_BYTE; + + /* Number of bytes of FIFO data. */ + uint8_t fifo_data[fifo_buffer_size]; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Array of accelerometer frames -> Total bytes = + * 55 * (6 axes bytes) = 330 bytes */ + struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; + + /* Array of gyro frames -> Total bytes = + * 55 * (6 axes bytes) = 330 bytes */ + struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; + + /* Initialize FIFO frame structure. */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Accel and gyro sensor are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Variable to get fifo water-mark interrupt status. */ + uint16_t int_status = 0; + + uint16_t watermark = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_context. */ + rslt = bmi270_context_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable accel and gyro sensor. */ + rslt = bmi270_context_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configuration settings for accel and gyro. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Before setting FIFO, disable the advance power save mode. */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Update FIFO structure. */ + /* Mapping the buffer to store the fifo data. */ + fifoframe.data = fifo_data; + + /* Length of FIFO frame. */ + /* To read sensortime, extra 3 bytes are added to fifo user length. */ + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH + SENSORTIME_OVERHEAD_BYTE; + + /* Set FIFO configuration by enabling accel, gyro and timestamp. + * NOTE 1: The header mode is enabled by default. + * NOTE 2: By default the FIFO operating mode is FIFO mode. + * NOTE 3: Sensortime is enabled by default */ + printf("FIFO is configured in header mode\n"); + rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* FIFO water-mark interrupt is enabled. */ + fifoframe.data_int_map = BMI2_FWM_INT; + + /* Map water-mark interrupt to the required interrupt pin. */ + rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Set water-mark level. */ + fifoframe.wm_lvl = BMI270_FIFO_WATERMARK_LEVEL; + + /* Set the water-mark level if water-mark interrupt is mapped. */ + rslt = bmi2_set_fifo_wm(fifoframe.wm_lvl, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + rslt = bmi2_get_fifo_wm(&watermark, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + printf("\nFIFO watermark level is %d\n", watermark); + + while (try <= 10) + { + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the status of FIFO watermark interrupt. */ + if ((rslt == BMI2_OK) && (int_status & BMI2_FWM_INT_STATUS_MASK)) + { + printf("\nIteration : %d\n", try); + + printf("\nWatermark interrupt occurred\n"); + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + printf("\nFIFO data bytes available : %d \n", fifo_length); + printf("\nFIFO data bytes requested : %d \n", fifoframe.length); + + /* Read FIFO data. */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nFIFO accel frames requested : %d \n", accel_frame_length); + + /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ + rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); + + printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); + + /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ + rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); + + /* Calculating the frame count from the available bytes in FIFO + * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len + header_byte)) */ + frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH + 1)); + printf("\nAvailable frame count from available bytes: %d\n", frame_count); + + printf("\nExracted accel frames\n"); + + if (accel_frame_length > frame_count) + { + accel_frame_length = frame_count; + } + + /* Print the parsed accelerometer data from the FIFO buffer. */ + for (index = 0; index < accel_frame_length; index++) + { + printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, + fifo_accel_data[index].y, fifo_accel_data[index].z); + } + + printf("\nExtracted gyro frames\n"); + + if (gyro_frame_length > frame_count) + { + gyro_frame_length = frame_count; + } + + /* Print the parsed gyro data from the FIFO buffer. */ + for (index = 0; index < gyro_frame_length; index++) + { + printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", + index, + fifo_gyro_data[index].x, + fifo_gyro_data[index].y, + fifo_gyro_data[index].z); + } + + /* Print control frames like sensor time and skipped frame count. */ + printf("Skipped frame count = %d\n", fifoframe.skipped_frame_count); + printf("Sensor time = %lu\r\n", fifoframe.sensor_time); + + try++; + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accel and gyro configurations. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[0].type = BMI2_ACCEL; + config[1].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_context_get_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Accel configuration settings. */ + /* Set Output Data Rate */ + config[0].cfg.acc.odr = BMI2_ACC_ODR_25HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Gyro configuration settings. */ + /* Set Output Data Rate */ + config[1].cfg.gyr.odr = BMI2_GYR_ODR_25HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set new configurations. */ + rslt = bmi270_context_set_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_headerless_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_headerless_mode/Makefile new file mode 100644 index 000000000..9d5e86e56 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_headerless_mode/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= fifo_watermark_headerless_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_context.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c new file mode 100644 index 000000000..878fe43bb --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c @@ -0,0 +1,331 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_context.h" +#include "common.h" + +/******************************************************************************/ +/*! Macros */ + +/*! Buffer size allocated to store raw FIFO data */ +#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) + +/*! Length of data to be read from FIFO */ +#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) + +/*! Number of accel frames to be extracted from FIFO */ + +/*! + * Calculation: + * fifo_watermark_level = 650, accel_frame_len = 6, gyro_frame_len = 6. + * fifo_accel_frame_count = (650 / (6 + 6 )) = 54 frames + */ +#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(55) + +/*! Number of gyro frames to be extracted from FIFO */ +#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(55) + +/*! Setting a watermark level in FIFO */ +#define BMI270_FIFO_WATERMARK_LEVEL UINT16_C(650) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + * @param[in] dev : Structure instance of bmi2_dev. + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to index bytes. */ + uint16_t index = 0; + + /* Variable to store the available frame length count in FIFO */ + uint8_t frame_count; + + uint16_t fifo_length = 0; + + uint16_t accel_frame_length; + + uint16_t gyro_frame_length; + + uint8_t try = 1; + + /* Number of bytes of FIFO data. */ + uint8_t fifo_data[BMI2_FIFO_RAW_DATA_BUFFER_SIZE] = { 0 }; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Array of accelerometer frames -> Total bytes = + * 50 * (6 axes bytes) = 300 bytes */ + struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; + + /* Array of gyro frames -> Total bytes = + * 50 * (6 axes bytes) = 300 bytes */ + struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; + + /* Initialize FIFO frame structure. */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Accel and gyro sensor are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Variable to get fifo water-mark interrupt status. */ + uint16_t int_status = 0; + + uint16_t watermark = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_context. */ + rslt = bmi270_context_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable accel and gyro sensor. */ + rslt = bmi270_context_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configuration settings for accel and gyro. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Before setting FIFO, disable the advance power save mode. */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Update FIFO structure. */ + /* Mapping the buffer to store the fifo data. */ + fifoframe.data = fifo_data; + + /* Length of FIFO frame. */ + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; + + /* Set FIFO configuration by enabling accel, gyro. + * NOTE 1: The header mode is enabled by default. + * NOTE 2: By default the FIFO operating mode is FIFO mode. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("FIFO is configured in headerless mode\n"); + + /* To enable headerless mode, disable the header. */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_fifo_config(BMI2_FIFO_HEADER_EN, BMI2_DISABLE, &bmi2_dev); + } + + /* FIFO water-mark interrupt is enabled. */ + fifoframe.data_int_map = BMI2_FWM_INT; + + /* Map water-mark interrupt to the required interrupt pin. */ + rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Set water-mark level. */ + fifoframe.wm_lvl = BMI270_FIFO_WATERMARK_LEVEL; + + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; + + /* Set the water-mark level if water-mark interrupt is mapped. */ + rslt = bmi2_set_fifo_wm(fifoframe.wm_lvl, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + rslt = bmi2_get_fifo_wm(&watermark, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + printf("\nFIFO watermark level is %d\n", watermark); + + while (try <= 10) + { + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the status of FIFO watermark interrupt. */ + if ((rslt == BMI2_OK) && (int_status & BMI2_FWM_INT_STATUS_MASK)) + { + printf("\nIteration : %d\n", try); + + printf("\nWatermark interrupt occurred\n"); + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + printf("\nFIFO data bytes available : %d \n", fifo_length); + printf("\nFIFO data bytes requested : %d \n", fifoframe.length); + + /* Read FIFO data. */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nFIFO accel frames requested : %d \n", accel_frame_length); + + /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ + rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); + + printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); + + /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ + rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); + + /* Calculating the frame count from the available bytes in FIFO + * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len)) */ + frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH)); + printf("\nAvailable frame count from available bytes: %d\n", frame_count); + + printf("\nExracted accel frames\n"); + + if (accel_frame_length > frame_count) + { + accel_frame_length = frame_count; + } + + /* Print the parsed accelerometer data from the FIFO buffer. */ + for (index = 0; index < accel_frame_length; index++) + { + printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, + fifo_accel_data[index].y, fifo_accel_data[index].z); + } + + printf("\nExtracted gyro frames\n"); + + if (gyro_frame_length > frame_count) + { + gyro_frame_length = frame_count; + } + + /* Print the parsed gyro data from the FIFO buffer. */ + for (index = 0; index < gyro_frame_length; index++) + { + printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", + index, + fifo_gyro_data[index].x, + fifo_gyro_data[index].y, + fifo_gyro_data[index].z); + } + } + + try++; + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accel and gyro configurations. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[0].type = BMI2_ACCEL; + config[1].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_context_get_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameter according to their requirement. */ + /* Accel configuration settings. */ + /* Set Output Data Rate */ + config[0].cfg.acc.odr = BMI2_ACC_ODR_100HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Gyro configuration settings. */ + /* Set Output Data Rate */ + config[1].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set new configurations. */ + rslt = bmi270_context_set_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/gyro/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/gyro/Makefile new file mode 100644 index 000000000..135fe5ba3 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/gyro/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= gyro.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_context.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/gyro/gyro.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/gyro/gyro.c new file mode 100644 index 000000000..0a057e464 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/gyro/gyro.c @@ -0,0 +1,195 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_context.h" +#include "common.h" + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for gyro. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_gyro_config(struct bmi2_dev *dev); + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + * + * @param[in] val : LSB from each axis. + * @param[in] dps : Degree per second. + * @param[in] bit_width : Resolution for gyro. + * + * @return Degree per second. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to define limit to print gyro data. */ + uint8_t limit = 10; + + uint8_t indx = 0; + + float x = 0, y = 0, z = 0; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Create an instance of sensor data structure. */ + struct bmi2_sensor_data sensor_data = { 0 }; + + /* Assign gyro sensor to variable. */ + uint8_t sens_list = BMI2_GYRO; + + /* Initialize the interrupt status of gyro. */ + uint16_t int_status = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_context. */ + rslt = bmi270_context_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the selected sensors. */ + rslt = bmi270_context_sensor_enable(&sens_list, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Gyro configuration settings. */ + rslt = set_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Select gyro sensor. */ + sensor_data.type = BMI2_GYRO; + printf("Gyro and DPS data\n"); + printf("Gyro data collected at Range 2000 dps with 16-bit resolution\n"); + + /* Loop to print gyro data when interrupt occurs. */ + while (indx <= limit) + { + /* To get the data ready interrupt status of gyro. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the data ready interrupt status and print the status for 10 samples. */ + if (int_status & BMI2_GYR_DRDY_INT_MASK) + { + /* Get gyro data for x, y and z axis. */ + rslt = bmi270_context_get_sensor_data(&sensor_data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("\nGyr_X = %d\t", sensor_data.sens_data.gyr.x); + printf("Gyr_Y = %d\t", sensor_data.sens_data.gyr.y); + printf("Gyr_Z = %d\r\n", sensor_data.sens_data.gyr.z); + + /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ + x = lsb_to_dps(sensor_data.sens_data.gyr.x, 2000, bmi2_dev.resolution); + y = lsb_to_dps(sensor_data.sens_data.gyr.y, 2000, bmi2_dev.resolution); + z = lsb_to_dps(sensor_data.sens_data.gyr.z, 2000, bmi2_dev.resolution); + + /* Print the data in dps. */ + printf("Gyr_DPS_X = %4.2f, Gyr_DPS_Y = %4.2f, Gyr_DPS_Z = %4.2f\n", x, y, z); + + indx++; + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for gyro. + */ +static int8_t set_gyro_config(struct bmi2_dev *dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define the type of sensor and its configurations. */ + struct bmi2_sens_config config; + + /* Configure the type of feature. */ + config.type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_context_get_sensor_config(&config, 1, dev); + bmi2_error_codes_print_result(rslt); + + /* Map data ready interrupt to interrupt pin. */ + rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT2, dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config.cfg.gyr.odr = BMI2_GYR_ODR_200HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config.cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config.cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config.cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config.cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set the gyro configurations. */ + rslt = bmi270_context_set_sensor_config(&config, 1, dev); + } + + return rslt; +} + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/step_counter/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/step_counter/Makefile new file mode 100644 index 000000000..1e8be48cd --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/step_counter/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= step_counter.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_context.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/step_counter/step_counter.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/step_counter/step_counter.c new file mode 100644 index 000000000..2513b028a --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/step_counter/step_counter.c @@ -0,0 +1,146 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_context.h" +#include "common.h" + +/******************************************************************************/ +/*! Static function declaration */ + +/*! + * @brief This internal API is used to set configurations for step counter. + * + * @param[in] bmi2_dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_feature_config(struct bmi2_dev *bmi2_dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Structure to define type of sensor and their respective data. */ + struct bmi2_sensor_data sensor_data; + + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Accel sensor and step counter feature are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_STEP_COUNTER }; + + /* Variable to get step counter interrupt status. */ + uint16_t int_status = 0; + + /* Select features and their pins to be mapped to. */ + struct bmi2_sens_int_config sens_int = { .type = BMI2_STEP_COUNTER, .hw_int_pin = BMI2_INT2 }; + + /* Type of sensor to get step counter data. */ + sensor_data.type = BMI2_STEP_COUNTER; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_context. */ + rslt = bmi270_context_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the selected sensor. */ + rslt = bmi270_context_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Set the feature configuration for step counter. */ + rslt = set_feature_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("Step counter watermark level set to 1 (20 steps)\n"); + + if (rslt == BMI2_OK) + { + /* Map the step counter feature interrupt. */ + rslt = bmi270_context_map_feat_int(&sens_int, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Move the board in steps for 20 times to get step counter interrupt. */ + printf("Move the board in steps\n"); + + /* Loop to get number of steps counted. */ + do + { + /* To get the interrupt status of the step counter. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the interrupt status of the step counter. */ + if (int_status & BMI270_CONTEXT_STEP_CNT_STATUS_MASK) + { + printf("Step counter interrupt occurred when watermark level (20 steps) is reached\n"); + + /* Get step counter output. */ + rslt = bmi270_context_get_sensor_data(&sensor_data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Print the step counter output. */ + printf("No of steps counted = %ld", sensor_data.sens_data.step_counter_output); + break; + } + } while (rslt == BMI2_OK); + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for step counter. + */ +static int8_t set_feature_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define the type of sensor and its configurations. */ + struct bmi2_sens_config config; + + /* Configure the type of sensor. */ + config.type = BMI2_STEP_COUNTER; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_context_get_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Setting water-mark level to 1 for step counter to get interrupt after 20 step counts. Every 20 steps once + * output triggers. */ + config.cfg.step_counter.watermark_level = 1; + + /* Set new configuration. */ + rslt = bmi270_context_set_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel/Makefile new file mode 100644 index 000000000..d4cc64caf --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= accel.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_hc.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel/accel.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel/accel.c new file mode 100644 index 000000000..553db3772 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel/accel.c @@ -0,0 +1,201 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_hc.h" +#include "common.h" + +/******************************************************************************/ +/*! Macro definition */ + +/*! Earth's gravity in m/s^2 */ +#define GRAVITY_EARTH (9.80665f) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_accel_config(struct bmi2_dev *bmi2_dev); + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + * + * @param[in] val : LSB from each axis. + * @param[in] g_range : Gravity range. + * @param[in] bit_width : Resolution for accel. + * + * @return Gravity. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to define limit to print accel data. */ + uint8_t limit = 20; + + /* Assign accel sensor to variable. */ + uint8_t sensor_list = BMI2_ACCEL; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Create an instance of sensor data structure. */ + struct bmi2_sensor_data sensor_data = { 0 }; + + /* Initialize the interrupt status of accel. */ + uint16_t int_status = 0; + + uint8_t indx = 0; + float x = 0, y = 0, z = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_hc. */ + rslt = bmi270_hc_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the accel sensor. */ + rslt = bmi270_hc_sensor_enable(&sensor_list, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Accel configuration settings. */ + rslt = set_accel_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Assign accel sensor. */ + sensor_data.type = BMI2_ACCEL; + printf("Accel and m/s2 data \n"); + printf("Accel data collected at 2G Range with 16-bit resolution\n"); + + /* Loop to print the accel data when interrupt occurs. */ + while (indx <= limit) + { + /* To get the status of accel data ready interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the accel data ready interrupt status and print the status for 10 samples. */ + if (int_status & BMI2_ACC_DRDY_INT_MASK) + { + /* Get accelerometer data for x, y and z axis. */ + rslt = bmi270_hc_get_sensor_data(&sensor_data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + printf("\nAcc_X = %d\t", sensor_data.sens_data.acc.x); + printf("Acc_Y = %d\t", sensor_data.sens_data.acc.y); + printf("Acc_Z = %d\r\n", sensor_data.sens_data.acc.z); + + /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ + x = lsb_to_mps2(sensor_data.sens_data.acc.x, 2, bmi2_dev.resolution); + y = lsb_to_mps2(sensor_data.sens_data.acc.y, 2, bmi2_dev.resolution); + z = lsb_to_mps2(sensor_data.sens_data.acc.z, 2, bmi2_dev.resolution); + + /* Print the data in m/s2. */ + printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); + + indx++; + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel. + */ +static int8_t set_accel_config(struct bmi2_dev *bmi2_dev) +{ + + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accelerometer configuration. */ + struct bmi2_sens_config config; + + /* Configure the type of feature. */ + config.type = BMI2_ACCEL; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_hc_get_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config.cfg.acc.odr = BMI2_ACC_ODR_200HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config.cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples. + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config.cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config.cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set the accel configurations. */ + rslt = bmi270_hc_set_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Map data ready interrupt to interrupt pin. */ + rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (GRAVITY_EARTH * val * g_range) / half_scale; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel_gyro/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel_gyro/Makefile new file mode 100644 index 000000000..19eb609ea --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel_gyro/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= accel_gyro.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_hc.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel_gyro/accel_gyro.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel_gyro/accel_gyro.c new file mode 100644 index 000000000..c92a1b82d --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel_gyro/accel_gyro.c @@ -0,0 +1,268 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_hc.h" +#include "common.h" + +/******************************************************************************/ +/*! Macro definition */ + +/*! Earth's gravity in m/s^2 */ +#define GRAVITY_EARTH (9.80665f) + +/*! Macros to select the sensors */ +#define ACCEL UINT8_C(0x00) +#define GYRO UINT8_C(0x01) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev); + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + * + * @param[in] val : LSB from each axis. + * @param[in] g_range : Gravity range. + * @param[in] bit_width : Resolution for accel. + * + * @return Gravity. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + * + * @param[in] val : LSB from each axis. + * @param[in] dps : Degree per second. + * @param[in] bit_width : Resolution for gyro. + * + * @return Degree per second. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to define limit to print accel data. */ + uint8_t limit = 10; + + /* Assign accel and gyro sensor to variable. */ + uint8_t sensor_list[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Create an instance of sensor data structure. */ + struct bmi2_sensor_data sensor_data[2] = { { 0 } }; + + /* Initialize the interrupt status of accel and gyro. */ + uint16_t int_status = 0; + + uint8_t indx = 1; + + float x = 0, y = 0, z = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_hc. */ + rslt = bmi270_hc_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the accel and gyro sensor. */ + rslt = bmi270_hc_sensor_enable(sensor_list, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Accel and gyro configuration settings. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Assign accel and gyro sensor. */ + sensor_data[ACCEL].type = BMI2_ACCEL; + sensor_data[GYRO].type = BMI2_GYRO; + + /* Loop to print accel and gyro data when interrupt occurs. */ + while (indx <= limit) + { + /* To get the data ready interrupt status of accel and gyro. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the data ready interrupt status and print the status for 10 samples. */ + if ((int_status & BMI2_ACC_DRDY_INT_MASK) && (int_status & BMI2_GYR_DRDY_INT_MASK)) + { + /* Get accel and gyro data for x, y and z axis. */ + rslt = bmi270_hc_get_sensor_data(sensor_data, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("\n******* Accel(Raw and m/s2) Gyro(Raw and dps) data : %d *******\n", indx); + + printf("\nAcc_x = %d\t", sensor_data[ACCEL].sens_data.acc.x); + printf("Acc_y = %d\t", sensor_data[ACCEL].sens_data.acc.y); + printf("Acc_z = %d", sensor_data[ACCEL].sens_data.acc.z); + + /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ + x = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.x, 2, bmi2_dev.resolution); + y = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.y, 2, bmi2_dev.resolution); + z = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.z, 2, bmi2_dev.resolution); + + /* Print the data in m/s2. */ + printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); + + printf("\nGyr_X = %d\t", sensor_data[GYRO].sens_data.gyr.x); + printf("Gyr_Y = %d\t", sensor_data[GYRO].sens_data.gyr.y); + printf("Gyr_Z = %d\n", sensor_data[GYRO].sens_data.gyr.z); + + /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ + x = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.x, 2000, bmi2_dev.resolution); + y = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.y, 2000, bmi2_dev.resolution); + z = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.z, 2000, bmi2_dev.resolution); + + /* Print the data in dps. */ + printf("Gyro_DPS_X = %4.2f, Gyro_DPS_Y = %4.2f, Gyro_DPS_Z = %4.2f\n", x, y, z); + + indx++; + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accelerometer and gyro configuration. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[ACCEL].type = BMI2_ACCEL; + config[GYRO].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_hc_get_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Map data ready interrupt to interrupt pin. */ + rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config[ACCEL].cfg.acc.odr = BMI2_ACC_ODR_200HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[ACCEL].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples. + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[ACCEL].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[ACCEL].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config[GYRO].cfg.gyr.odr = BMI2_GYR_ODR_200HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[GYRO].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[GYRO].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[GYRO].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[GYRO].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set the accel and gyro configurations. */ + rslt = bmi270_hc_set_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (GRAVITY_EARTH * val * g_range) / half_scale; +} + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/activity_recognition/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/activity_recognition/Makefile new file mode 100644 index 000000000..ae036b47c --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/activity_recognition/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= activity_recognition.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_hc.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/activity_recognition/activity_recognition.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/activity_recognition/activity_recognition.c new file mode 100644 index 000000000..11750ca53 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/activity_recognition/activity_recognition.c @@ -0,0 +1,154 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_hc.h" +#include "common.h" + +/******************************************************************************/ +/*! Static function declaration */ + +/*! + * @brief This internal API is used to set configurations for step counter. + * + * @param[in] bmi2_dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_config(struct bmi2_dev *bmi2_dev); + +/******************************************************************************/ +/*! Functions */ +/* This function starts the execution of program. */ +int main(void) +{ + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to define index for FIFO data */ + uint8_t count; + + /* Various Activity recognitions are listed in array. */ + const char *activity_reg_output[6] = { "OTHERS", "STILL", "WALKING", "RUNNING", "ON_BICYCLE", "IN_VEHICLE" }; + + /* Accel sensor and step activity feature are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_ACTIVITY_RECOGNITION }; + + /* Array to define FIFO data */ + uint8_t fifo_data[516] = { 0 }; + + /* Variable to store number of activity frames */ + uint16_t act_frames; + + /* Structure to define a FIFO */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Structure to define output for activity recognition */ + struct bmi2_act_recog_output act_recog_data[5] = { { 0 } }; + + uint16_t fifo_length; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_hc. */ + rslt = bmi270_hc_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the selected sensor. */ + rslt = bmi270_hc_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + rslt = set_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Update FIFO structure */ + fifoframe.data = fifo_data; + fifoframe.length = 516; + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + printf("Fifo length = %d \n", fifo_length); + + /* Read FIFO data */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + while (rslt != BMI2_W_FIFO_EMPTY) + { + /* Provide the number of frames to be read */ + act_frames = 5; + + printf("Requested FIFO data frames : %d\n", act_frames); + + /* Get the activity output */ + rslt = bmi270_hc_get_act_recog_output(act_recog_data, &act_frames, &fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("Parsed FIFO data frames : %d\r\n", act_frames); + + for (count = 0; count < act_frames; count++) + { + printf( + "Activity Recognition output[%d]:Sensor time: %lu\t Previous activity: %s\t current: %s\n", + count, + act_recog_data[count].time_stamp, + activity_reg_output[act_recog_data[count].prev_act], + activity_reg_output[act_recog_data[count].curr_act]); + } + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +static int8_t set_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + uint8_t temp_data[2]; + + /* Disable advanced power save */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Reset FIFO */ + rslt = bmi2_set_command_register(BMI2_FIFO_FLUSH_CMD, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("Move the board for activity recognition for 30 sec :\n"); + bmi2_dev->delay_us(30000000, bmi2_dev->intf_ptr); + + if (rslt == BMI2_OK) + { + rslt = bmi2_get_regs(BMI2_FIFO_CONFIG_0_ADDR, temp_data, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/common/common.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/common/common.c new file mode 100644 index 000000000..afff37bc8 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/common/common.c @@ -0,0 +1,372 @@ +/** + * Copyright (C) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +#include +#include +#include + +#include "coines.h" +#include "bmi2_defs.h" + +/******************************************************************************/ +/*! Macro definitions */ +#define BMI2XY_SHUTTLE_ID UINT16_C(0x1B8) + +/*! Macro that defines read write length */ +#define READ_WRITE_LEN UINT8_C(46) + +/******************************************************************************/ +/*! Static variable definition */ + +/*! Variable that holds the I2C device address or SPI chip selection */ +static uint8_t dev_addr; + +/******************************************************************************/ +/*! User interface functions */ + +/*! + * I2C read function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_read_i2c(dev_addr, reg_addr, reg_data, (uint16_t)len); +} + +/*! + * I2C write function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_write_i2c(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len); +} + +/*! + * SPI read function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_read_spi(dev_addr, reg_addr, reg_data, (uint16_t)len); +} + +/*! + * SPI write function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_write_spi(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len); +} + +/*! + * Delay function map to COINES platform + */ +void bmi2_delay_us(uint32_t period, void *intf_ptr) +{ + coines_delay_usec(period); +} + +/*! + * @brief Function to select the interface between SPI and I2C. + * Also to initialize coines platform + */ +int8_t bmi2_interface_init(struct bmi2_dev *bmi, uint8_t intf) +{ + int8_t rslt = BMI2_OK; + struct coines_board_info board_info; + + if (bmi != NULL) + { + int16_t result = coines_open_comm_intf(COINES_COMM_INTF_USB); + if (result < COINES_SUCCESS) + { + printf( + "\n Unable to connect with Application Board ! \n" " 1. Check if the board is connected and powered on. \n" " 2. Check if Application Board USB driver is installed. \n" + " 3. Check if board is in use by another application. (Insufficient permissions to access USB) \n"); + exit(result); + } + + result = coines_get_board_info(&board_info); + +#if defined(PC) + setbuf(stdout, NULL); +#endif + + if (result == COINES_SUCCESS) + { + if ((board_info.shuttle_id != BMI2XY_SHUTTLE_ID)) + { + printf("! Warning invalid sensor shuttle \n ," "This application will not support this sensor \n"); + exit(COINES_E_FAILURE); + } + } + + coines_set_shuttleboard_vdd_vddio_config(0, 0); + coines_delay_msec(100); + + /* Bus configuration : I2C */ + if (intf == BMI2_I2C_INTF) + { + printf("I2C Interface \n"); + + /* To initialize the user I2C function */ + dev_addr = BMI2_I2C_PRIM_ADDR; + bmi->intf = BMI2_I2C_INTF; + bmi->read = bmi2_i2c_read; + bmi->write = bmi2_i2c_write; + coines_config_i2c_bus(COINES_I2C_BUS_0, COINES_I2C_FAST_MODE); + } + /* Bus configuration : SPI */ + else if (intf == BMI2_SPI_INTF) + { + printf("SPI Interface \n"); + + /* To initialize the user SPI function */ + dev_addr = COINES_SHUTTLE_PIN_7; + bmi->intf = BMI2_SPI_INTF; + bmi->read = bmi2_spi_read; + bmi->write = bmi2_spi_write; + coines_config_spi_bus(COINES_SPI_BUS_0, COINES_SPI_SPEED_5_MHZ, COINES_SPI_MODE3); + coines_set_pin_config(COINES_SHUTTLE_PIN_7, COINES_PIN_DIRECTION_OUT, COINES_PIN_VALUE_HIGH); + } + + /* Assign device address to interface pointer */ + bmi->intf_ptr = &dev_addr; + + /* Configure delay in microseconds */ + bmi->delay_us = bmi2_delay_us; + + /* Configure max read/write length (in bytes) ( Supported length depends on target machine) */ + bmi->read_write_len = READ_WRITE_LEN; + + /* Assign to NULL to load the default config file. */ + bmi->config_file_ptr = NULL; + + coines_delay_usec(10000); + + coines_set_shuttleboard_vdd_vddio_config(3300, 3300); + + coines_delay_usec(10000); + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; + +} + +/*! + * @brief Prints the execution status of the APIs. + */ +void bmi2_error_codes_print_result(int8_t rslt) +{ + switch (rslt) + { + case BMI2_OK: + + /* Do nothing */ + break; + + case BMI2_W_FIFO_EMPTY: + printf("Warning [%d] : FIFO empty\r\n", rslt); + break; + case BMI2_W_PARTIAL_READ: + printf("Warning [%d] : FIFO partial read\r\n", rslt); + break; + case BMI2_E_NULL_PTR: + printf( + "Error [%d] : Null pointer error. It occurs when the user tries to assign value (not address) to a pointer," " which has been initialized to NULL.\r\n", + rslt); + break; + + case BMI2_E_COM_FAIL: + printf( + "Error [%d] : Communication failure error. It occurs due to read/write operation failure and also due " "to power failure during communication\r\n", + rslt); + break; + + case BMI2_E_DEV_NOT_FOUND: + printf("Error [%d] : Device not found error. It occurs when the device chip id is incorrectly read\r\n", + rslt); + break; + + case BMI2_E_INVALID_SENSOR: + printf( + "Error [%d] : Invalid sensor error. It occurs when there is a mismatch in the requested feature with the " "available one\r\n", + rslt); + break; + + case BMI2_E_SELF_TEST_FAIL: + printf( + "Error [%d] : Self-test failed error. It occurs when the validation of accel self-test data is " "not satisfied\r\n", + rslt); + break; + + case BMI2_E_INVALID_INT_PIN: + printf( + "Error [%d] : Invalid interrupt pin error. It occurs when the user tries to configure interrupt pins " "apart from INT1 and INT2\r\n", + rslt); + break; + + case BMI2_E_OUT_OF_RANGE: + printf( + "Error [%d] : Out of range error. It occurs when the data exceeds from filtered or unfiltered data from " "fifo and also when the range exceeds the maximum range for accel and gyro while performing FOC\r\n", + rslt); + break; + + case BMI2_E_ACC_INVALID_CFG: + printf( + "Error [%d] : Invalid Accel configuration error. It occurs when there is an error in accel configuration" " register which could be one among range, BW or filter performance in reg address 0x40\r\n", + rslt); + break; + + case BMI2_E_GYRO_INVALID_CFG: + printf( + "Error [%d] : Invalid Gyro configuration error. It occurs when there is a error in gyro configuration" "register which could be one among range, BW or filter performance in reg address 0x42\r\n", + rslt); + break; + + case BMI2_E_ACC_GYR_INVALID_CFG: + printf( + "Error [%d] : Invalid Accel-Gyro configuration error. It occurs when there is a error in accel and gyro" " configuration registers which could be one among range, BW or filter performance in reg address 0x40 " "and 0x42\r\n", + rslt); + break; + + case BMI2_E_CONFIG_LOAD: + printf( + "Error [%d] : Configuration load error. It occurs when failure observed while loading the configuration " "into the sensor\r\n", + rslt); + break; + + case BMI2_E_INVALID_PAGE: + printf( + "Error [%d] : Invalid page error. It occurs due to failure in writing the correct feature configuration " "from selected page\r\n", + rslt); + break; + + case BMI2_E_SET_APS_FAIL: + printf( + "Error [%d] : APS failure error. It occurs due to failure in write of advance power mode configuration " "register\r\n", + rslt); + break; + + case BMI2_E_AUX_INVALID_CFG: + printf( + "Error [%d] : Invalid AUX configuration error. It occurs when the auxiliary interface settings are not " "enabled properly\r\n", + rslt); + break; + + case BMI2_E_AUX_BUSY: + printf( + "Error [%d] : AUX busy error. It occurs when the auxiliary interface buses are engaged while configuring" " the AUX\r\n", + rslt); + break; + + case BMI2_E_REMAP_ERROR: + printf( + "Error [%d] : Remap error. It occurs due to failure in assigning the remap axes data for all the axes " "after change in axis position\r\n", + rslt); + break; + + case BMI2_E_GYR_USER_GAIN_UPD_FAIL: + printf( + "Error [%d] : Gyro user gain update fail error. It occurs when the reading of user gain update status " "fails\r\n", + rslt); + break; + + case BMI2_E_SELF_TEST_NOT_DONE: + printf( + "Error [%d] : Self-test not done error. It occurs when the self-test process is ongoing or not " "completed\r\n", + rslt); + break; + + case BMI2_E_INVALID_INPUT: + printf("Error [%d] : Invalid input error. It occurs when the sensor input validity fails\r\n", rslt); + break; + + case BMI2_E_INVALID_STATUS: + printf("Error [%d] : Invalid status error. It occurs when the feature/sensor validity fails\r\n", rslt); + break; + + case BMI2_E_CRT_ERROR: + printf("Error [%d] : CRT error. It occurs when the CRT test has failed\r\n", rslt); + break; + + case BMI2_E_ST_ALREADY_RUNNING: + printf( + "Error [%d] : Self-test already running error. It occurs when the self-test is already running and " "another has been initiated\r\n", + rslt); + break; + + case BMI2_E_CRT_READY_FOR_DL_FAIL_ABORT: + printf( + "Error [%d] : CRT ready for download fail abort error. It occurs when download in CRT fails due to wrong " "address location\r\n", + rslt); + break; + + case BMI2_E_DL_ERROR: + printf( + "Error [%d] : Download error. It occurs when write length exceeds that of the maximum burst length\r\n", + rslt); + break; + + case BMI2_E_PRECON_ERROR: + printf( + "Error [%d] : Pre-conditional error. It occurs when precondition to start the feature was not " "completed\r\n", + rslt); + break; + + case BMI2_E_ABORT_ERROR: + printf("Error [%d] : Abort error. It occurs when the device was shaken during CRT test\r\n", rslt); + break; + + case BMI2_E_WRITE_CYCLE_ONGOING: + printf( + "Error [%d] : Write cycle ongoing error. It occurs when the write cycle is already running and another " "has been initiated\r\n", + rslt); + break; + + case BMI2_E_ST_NOT_RUNING: + printf( + "Error [%d] : Self-test is not running error. It occurs when self-test running is disabled while it's " "running\r\n", + rslt); + break; + + case BMI2_E_DATA_RDY_INT_FAILED: + printf( + "Error [%d] : Data ready interrupt error. It occurs when the sample count exceeds the FOC sample limit " "and data ready status is not updated\r\n", + rslt); + break; + + case BMI2_E_INVALID_FOC_POSITION: + printf( + "Error [%d] : Invalid FOC position error. It occurs when average FOC data is obtained for the wrong" " axes\r\n", + rslt); + break; + + default: + printf("Error [%d] : Unknown error code\r\n", rslt); + break; + } +} + +/*! + * @brief Deinitializes coines platform + * + * @return void. + */ +void bmi2_coines_deinit(void) +{ + coines_close_comm_intf(COINES_COMM_INTF_USB); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/common/common.h b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/common/common.h new file mode 100644 index 000000000..763983da4 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/common/common.h @@ -0,0 +1,122 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +#ifndef _COMMON_H +#define _COMMON_H + +/*! CPP guard */ +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include "bmi2.h" + +/*! + * @brief Function for reading the sensor's registers through I2C bus. + * + * @param[in] reg_addr : Register address. + * @param[out] reg_data : Pointer to the data buffer to store the read data. + * @param[in] length : No of bytes to read. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief Function for writing the sensor's registers through I2C bus. + * + * @param[in] reg_addr : Register address. + * @param[in] reg_data : Pointer to the data buffer whose value is to be written. + * @param[in] length : No of bytes to write. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief Function for reading the sensor's registers through SPI bus. + * + * @param[in] reg_addr : Register address. + * @param[out] reg_data : Pointer to the data buffer to store the read data. + * @param[in] length : No of bytes to read. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief Function for writing the sensor's registers through SPI bus. + * + * @param[in] reg_addr : Register address. + * @param[in] reg_data : Pointer to the data buffer whose data has to be written. + * @param[in] length : No of bytes to write. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief This function provides the delay for required time (Microsecond) as per the input provided in some of the + * APIs. + * + * @param[in] period_us : The required wait time in microsecond. + * @param[in] intf_ptr : Interface pointer + * + * @return void. + * + */ +void bmi2_delay_us(uint32_t period, void *intf_ptr); + +/*! + * @brief Function to select the interface between SPI and I2C. + * + * @param[in] bma : Structure instance of bmi2_dev + * @param[in] intf : Interface selection parameter + * + * @return Status of execution + * @retval 0 -> Success + * @retval < 0 -> Failure Info + */ +int8_t bmi2_interface_init(struct bmi2_dev *bma, uint8_t intf); + +/*! + * @brief Prints the execution status of the APIs. + * + * @param[in] rslt : Error code returned by the API whose execution status has to be printed. + * + * @return void. + */ +void bmi2_error_codes_print_result(int8_t rslt); + +/*! + * @brief Deinitializes coines platform + * + * @return void. + */ +void bmi2_coines_deinit(void); + +#ifdef __cplusplus +} +#endif /* End of CPP guard */ + +#endif /* _COMMON_H */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/component_retrim/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/component_retrim/Makefile new file mode 100644 index 000000000..633a81729 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/component_retrim/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= component_retrim.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_hc.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/component_retrim/component_retrim.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/component_retrim/component_retrim.c new file mode 100644 index 000000000..6084574f4 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/component_retrim/component_retrim.c @@ -0,0 +1,52 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_hc.h" +#include "common.h" + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_hc. */ + rslt = bmi270_hc_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* This API runs the CRT process. */ + rslt = bmi2_do_crt(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Do not move the board while doing CRT. If so, it will throw an abort error. */ + if (rslt == BMI2_OK) + { + printf("CRT successfully completed."); + } + } + + bmi2_coines_deinit(); + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_header_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_header_mode/Makefile new file mode 100644 index 000000000..00794a37a --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_header_mode/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= fifo_full_header_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_hc.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_header_mode/fifo_full_header_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_header_mode/fifo_full_header_mode.c new file mode 100644 index 000000000..6f1164284 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_header_mode/fifo_full_header_mode.c @@ -0,0 +1,316 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_hc.h" +#include "common.h" + +/******************************************************************************/ +/*! Macros */ + +/*! Buffer size allocated to store raw FIFO data. */ +#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) + +/*! Length of data to be read from FIFO. */ +#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) + +/*! Number of accel frames to be extracted from FIFO. */ + +/*! Calculation for frame count: Total frame count = Fifo buffer size(2048)/ Total frames(6 Accel, 6 Gyro and 1 header, + * totaling to 13) which equals to 157. + */ +#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(157) + +/*! Number of gyro frames to be extracted from FIFO. */ +#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(157) + +/*! Macro to read sensortime byte in FIFO. */ +#define SENSORTIME_OVERHEAD_BYTE UINT8_C(3) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + * @param[in] dev : Structure instance of bmi2_dev. + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev); + +/******************************************************************************/ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + uint16_t index = 0; + uint16_t fifo_length = 0; + uint16_t config = 0; + + /* To read sensortime, extra 3 bytes are added to fifo buffer. */ + uint16_t fifo_buffer_size = BMI2_FIFO_RAW_DATA_BUFFER_SIZE + SENSORTIME_OVERHEAD_BYTE; + + /* Variable to get fifo full interrupt status. */ + uint16_t int_status = 0; + + uint16_t accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + + uint16_t gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + /* Variable to store the available frame length count in FIFO */ + uint8_t frame_count; + + int8_t try = 1; + + /* Number of bytes of FIFO data. */ + uint8_t fifo_data[fifo_buffer_size]; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Array of accelerometer frames -> Total bytes = + * 157 * (6 axes + 1 header bytes) = 1099 bytes */ + struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; + + /* Array of gyro frames -> Total bytes = + * 157 * (6 axes + 1 header bytes) = 1099 bytes */ + struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; + + /* Initialize FIFO frame structure. */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Accel and gyro sensor are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_hc. */ + rslt = bmi270_hc_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable accel and gyro sensor. */ + rslt = bmi270_hc_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configuration settings for accel and gyro. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Before setting FIFO, disable the advance power save mode. */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_get_fifo_config(&config, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Update FIFO structure. */ + /* Mapping the buffer to store the fifo data. */ + fifoframe.data = fifo_data; + + /* Length of FIFO frame. */ + /* To read sensortime, extra 3 bytes are added to fifo user length. */ + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH + SENSORTIME_OVERHEAD_BYTE; + + /* Set FIFO configuration by enabling accel, gyro and timestamp. + * NOTE 1: The header mode is enabled by default. + * NOTE 2: By default the FIFO operating mode is in FIFO mode. + * NOTE 3: Sensortime is enabled by default */ + printf("FIFO is configured in header mode\n"); + rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Map FIFO full interrupt. */ + fifoframe.data_int_map = BMI2_FFULL_INT; + rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + while (try <= 10) + { + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if ((rslt == BMI2_OK) && (int_status & BMI2_FFULL_INT_STATUS_MASK)) + { + printf("\nIteration : %d\n", try); + + accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + + gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("\nFIFO data bytes available : %d \n", fifo_length); + printf("\nFIFO data bytes requested : %d \n", fifoframe.length); + + /* Read FIFO data. */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nFIFO accel frames requested : %d \n", accel_frame_length); + + /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ + rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); + + printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); + + /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ + rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); + + /* Calculating the frame count from the available bytes in FIFO + * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len + header_byte)) */ + frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH + 1)); + printf("\nAvailable frame count from available bytes: %d\n", frame_count); + + printf("\nExtracted accel frames\n"); + + if (accel_frame_length > frame_count) + { + accel_frame_length = frame_count; + } + + /* Print the parsed accelerometer data from the FIFO buffer. */ + for (index = 0; index < accel_frame_length; index++) + { + printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, + fifo_accel_data[index].y, fifo_accel_data[index].z); + } + + printf("\nExtracted gyro frames\n"); + + if (gyro_frame_length > frame_count) + { + gyro_frame_length = frame_count; + } + + /* Print the parsed gyro data from the FIFO buffer. */ + for (index = 0; index < gyro_frame_length; index++) + { + printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", + index, + fifo_gyro_data[index].x, + fifo_gyro_data[index].y, + fifo_gyro_data[index].z); + } + + /* Print control frames like sensor time and skipped frame count. */ + printf("\nSkipped frame count = %d\n", fifoframe.skipped_frame_count); + + printf("Sensor time = %lu\r\n", fifoframe.sensor_time); + + try++; + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accel and gyro configurations. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[0].type = BMI2_ACCEL; + config[1].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_hc_get_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Accel configuration settings. */ + /* Set Output Data Rate */ + config[0].cfg.acc.odr = BMI2_ACC_ODR_25HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Gyro configuration settings. */ + /* Set Output Data Rate */ + config[1].cfg.gyr.odr = BMI2_GYR_ODR_25HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set new configurations. */ + rslt = bmi270_hc_set_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_headerless_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_headerless_mode/Makefile new file mode 100644 index 000000000..5eefc71ab --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_headerless_mode/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= fifo_full_headerless_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_hc.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_headerless_mode/fifo_full_headerless_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_headerless_mode/fifo_full_headerless_mode.c new file mode 100644 index 000000000..1bce488be --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_headerless_mode/fifo_full_headerless_mode.c @@ -0,0 +1,304 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_hc.h" +#include "common.h" + +/******************************************************************************/ +/*! Macros */ + +/*! Buffer size allocated to store raw FIFO data */ +#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) + +/*! Length of data to be read from FIFO */ +#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) + +/*! Number of accel frames to be extracted from FIFO */ + +/*! Calculation for frame count: Total frame count = Fifo buffer size(2048)/ Total frames(6 Accel, 6 Gyro totaling to + * 12) which equals to 170. + */ +#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(169) + +/*! Number of gyro frames to be extracted from FIFO */ +#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(169) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + * @param[in] dev : Structure instance of bmi2_dev. + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev); + +/******************************************************************************/ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + uint16_t index = 0; + + uint16_t fifo_length = 0; + + uint8_t try = 1; + + /* Variable to get fifo full interrupt status. */ + uint16_t int_status = 0; + + uint16_t accel_frame_length; + + uint16_t gyro_frame_length; + + /* Variable to store the available frame length count in FIFO */ + uint8_t frame_count; + + /* Number of bytes of FIFO data. */ + uint8_t fifo_data[BMI2_FIFO_RAW_DATA_BUFFER_SIZE] = { 0 }; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Array of accelerometer frames -> Total bytes = + * 170 * (6 axes bytes) = 1020 bytes */ + struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; + + /* Array of gyro frames -> Total bytes = + * 170 * (6 axes bytes) = 1020 bytes */ + struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; + + /* Initialize FIFO frame structure. */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Accel and gyro sensor are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_hc. */ + rslt = bmi270_hc_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable accel and gyro sensor. */ + rslt = bmi270_hc_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configuration settings for accel and gyro. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Before setting FIFO, disable the advance power save mode. */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Update FIFO structure. */ + /* Mapping the buffer to store the fifo data. */ + fifoframe.data = fifo_data; + + /* Length of FIFO frame. */ + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; + + /* Set FIFO configuration by enabling accel, gyro. + * NOTE 1: The header mode is enabled by default. + * NOTE 2: By default the FIFO operating mode is FIFO mode. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("FIFO is configured in headerless mode\n"); + + /* To enable headerless mode, disable the header. */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_fifo_config(BMI2_FIFO_HEADER_EN, BMI2_DISABLE, &bmi2_dev); + } + + /* Map FIFO full interrupt. */ + fifoframe.data_int_map = BMI2_FFULL_INT; + rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + while (try <= 10) + { + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if ((rslt == BMI2_OK) && (int_status & BMI2_FFULL_INT_STATUS_MASK)) + { + printf("\nIteration : %d\n", try); + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + printf("\nFIFO data bytes available : %d \n", fifo_length); + printf("\nFIFO data bytes requested : %d \n", fifoframe.length); + + /* Read FIFO data. */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nFIFO accel frames requested : %d \n", accel_frame_length); + + /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ + rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); + + printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); + + /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ + rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); + + /* Calculating the frame count from the available bytes in FIFO + * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len)) */ + frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH)); + printf("\nAvailable frame count from available bytes: %d\n", frame_count); + + printf("\nExtracted accel frames\n"); + + if (accel_frame_length > frame_count) + { + accel_frame_length = frame_count; + } + + /* Print the parsed accelerometer data from the FIFO buffer. */ + for (index = 0; index < accel_frame_length; index++) + { + printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, + fifo_accel_data[index].y, fifo_accel_data[index].z); + } + + printf("\nExtracted gyro frames\n"); + + if (gyro_frame_length > frame_count) + { + gyro_frame_length = frame_count; + } + + /* Print the parsed gyro data from the FIFO buffer. */ + for (index = 0; index < gyro_frame_length; index++) + { + printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", + index, + fifo_gyro_data[index].x, + fifo_gyro_data[index].y, + fifo_gyro_data[index].z); + } + } + + try++; + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accel and gyro configurations. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[0].type = BMI2_ACCEL; + config[1].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_hc_get_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Accel configuration settings. */ + /* Set Output Data Rate */ + config[0].cfg.acc.odr = BMI2_ACC_ODR_100HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Gyro configuration settings. */ + /* Set Output Data Rate */ + config[1].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set new configurations. */ + rslt = bmi270_hc_set_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_header_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_header_mode/Makefile new file mode 100644 index 000000000..8dea084da --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_header_mode/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= fifo_watermark_header_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_hc.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_header_mode/fifo_watermark_header_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_header_mode/fifo_watermark_header_mode.c new file mode 100644 index 000000000..46599f21c --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_header_mode/fifo_watermark_header_mode.c @@ -0,0 +1,335 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_hc.h" +#include "common.h" + +/******************************************************************************/ +/*! Macros */ + +/*! Buffer size allocated to store raw FIFO data */ +#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) + +/*! Length of data to be read from FIFO */ +#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) + +/*! Number of accel frames to be extracted from FIFO */ + +/*! + * Calculation: + * fifo_watermark_level = 650, accel_frame_len = 6, gyro_frame_len = 6 header_byte = 1. + * fifo_accel_frame_count = (650 / (6 + 6 + 1 )) = 50 frames + * NOTE: Extra frames are read in order to get sensor time + */ +#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(55) + +/*! Number of gyro frames to be extracted from FIFO */ +#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(55) + +/*! Setting a watermark level in FIFO */ +#define BMI270_FIFO_WATERMARK_LEVEL UINT16_C(650) + +/*! Macro to read sensortime byte in FIFO */ +#define SENSORTIME_OVERHEAD_BYTE UINT8_C(3) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + * @param[in] dev : Structure instance of bmi2_dev. + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to store the available frame length count in FIFO */ + uint8_t frame_count; + + /* Variable to index bytes. */ + uint16_t index = 0; + + uint16_t fifo_length = 0; + + uint16_t accel_frame_length; + + uint16_t gyro_frame_length; + + uint8_t try = 1; + + /* To read sensortime, extra 3 bytes are added to fifo buffer */ + uint16_t fifo_buffer_size = BMI2_FIFO_RAW_DATA_BUFFER_SIZE + SENSORTIME_OVERHEAD_BYTE; + + /* Number of bytes of FIFO data. */ + uint8_t fifo_data[fifo_buffer_size]; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Array of accelerometer frames -> Total bytes = + * 55 * (6 axes bytes) = 330 bytes */ + struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; + + /* Array of gyro frames -> Total bytes = + * 55 * (6 axes bytes) = 330 bytes */ + struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; + + /* Initialize FIFO frame structure. */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Accel and gyro sensor are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Variable to get fifo water-mark interrupt status. */ + uint16_t int_status = 0; + + uint16_t watermark = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_hc. */ + rslt = bmi270_hc_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable accel and gyro sensor. */ + rslt = bmi270_hc_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configuration settings for accel and gyro. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Before setting FIFO, disable the advance power save mode. */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Update FIFO structure. */ + /* Mapping the buffer to store the fifo data. */ + fifoframe.data = fifo_data; + + /* Length of FIFO frame. */ + /* To read sensortime, extra 3 bytes are added to fifo user length. */ + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH + SENSORTIME_OVERHEAD_BYTE; + + /* Set FIFO configuration by enabling accel, gyro and timestamp. + * NOTE 1: The header mode is enabled by default. + * NOTE 2: By default the FIFO operating mode is FIFO mode. + * NOTE 3: Sensortime is enabled by default */ + printf("FIFO is configured in header mode\n"); + rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* FIFO water-mark interrupt is enabled. */ + fifoframe.data_int_map = BMI2_FWM_INT; + + /* Map water-mark interrupt to the required interrupt pin. */ + rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Set water-mark level. */ + fifoframe.wm_lvl = BMI270_FIFO_WATERMARK_LEVEL; + + /* Set the water-mark level if water-mark interrupt is mapped. */ + rslt = bmi2_set_fifo_wm(fifoframe.wm_lvl, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + rslt = bmi2_get_fifo_wm(&watermark, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + printf("\nFIFO watermark level is %d\n", watermark); + + while (try <= 10) + { + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the status of FIFO watermark interrupt. */ + if ((rslt == BMI2_OK) && (int_status & BMI2_FWM_INT_STATUS_MASK)) + { + printf("\nIteration : %d\n", try); + + printf("\nWatermark interrupt occurred\n"); + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + printf("\nFIFO data bytes available : %d \n", fifo_length); + printf("\nFIFO data bytes requested : %d \n", fifoframe.length); + + /* Read FIFO data. */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nFIFO accel frames requested : %d \n", accel_frame_length); + + /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ + rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); + + printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); + + /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ + rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); + + /* Calculating the frame count from the available bytes in FIFO + * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len + header_byte)) */ + frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH + 1)); + printf("\nAvailable frame count from available bytes: %d\n", frame_count); + + printf("\nExracted accel frames\n"); + + if (accel_frame_length > frame_count) + { + accel_frame_length = frame_count; + } + + /* Print the parsed accelerometer data from the FIFO buffer. */ + for (index = 0; index < accel_frame_length; index++) + { + printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, + fifo_accel_data[index].y, fifo_accel_data[index].z); + } + + printf("\nExtracted gyro frames\n"); + + if (gyro_frame_length > frame_count) + { + gyro_frame_length = frame_count; + } + + /* Print the parsed gyro data from the FIFO buffer. */ + for (index = 0; index < gyro_frame_length; index++) + { + printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", + index, + fifo_gyro_data[index].x, + fifo_gyro_data[index].y, + fifo_gyro_data[index].z); + } + + /* Print control frames like sensor time and skipped frame count. */ + printf("Skipped frame count = %d\n", fifoframe.skipped_frame_count); + printf("Sensor time = %lu\r\n", fifoframe.sensor_time); + + try++; + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accel and gyro configurations. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[0].type = BMI2_ACCEL; + config[1].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_hc_get_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Accel configuration settings. */ + /* Set Output Data Rate */ + config[0].cfg.acc.odr = BMI2_ACC_ODR_25HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Gyro configuration settings. */ + /* Set Output Data Rate */ + config[1].cfg.gyr.odr = BMI2_GYR_ODR_25HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set new configurations. */ + rslt = bmi270_hc_set_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_headerless_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_headerless_mode/Makefile new file mode 100644 index 000000000..bd842a817 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_headerless_mode/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= fifo_watermark_headerless_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_hc.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c new file mode 100644 index 000000000..bb0bfd111 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c @@ -0,0 +1,331 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_hc.h" +#include "common.h" + +/******************************************************************************/ +/*! Macros */ + +/*! Buffer size allocated to store raw FIFO data */ +#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) + +/*! Length of data to be read from FIFO */ +#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) + +/*! Number of accel frames to be extracted from FIFO */ + +/*! + * Calculation: + * fifo_watermark_level = 650, accel_frame_len = 6, gyro_frame_len = 6. + * fifo_accel_frame_count = (650 / (6 + 6 )) = 54 frames + */ +#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(55) + +/*! Number of gyro frames to be extracted from FIFO */ +#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(55) + +/*! Setting a watermark level in FIFO */ +#define BMI270_FIFO_WATERMARK_LEVEL UINT16_C(650) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + * @param[in] dev : Structure instance of bmi2_dev. + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to index bytes. */ + uint16_t index = 0; + + /* Variable to store the available frame length count in FIFO */ + uint8_t frame_count; + + uint16_t fifo_length = 0; + + uint16_t accel_frame_length; + + uint16_t gyro_frame_length; + + uint8_t try = 1; + + /* Number of bytes of FIFO data. */ + uint8_t fifo_data[BMI2_FIFO_RAW_DATA_BUFFER_SIZE] = { 0 }; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Array of accelerometer frames -> Total bytes = + * 50 * (6 axes bytes) = 300 bytes */ + struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; + + /* Array of gyro frames -> Total bytes = + * 50 * (6 axes bytes) = 300 bytes */ + struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; + + /* Initialize FIFO frame structure. */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Accel and gyro sensor are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Variable to get fifo water-mark interrupt status. */ + uint16_t int_status = 0; + + uint16_t watermark = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_hc. */ + rslt = bmi270_hc_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable accel and gyro sensor. */ + rslt = bmi270_hc_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configuration settings for accel and gyro. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Before setting FIFO, disable the advance power save mode. */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Update FIFO structure. */ + /* Mapping the buffer to store the fifo data. */ + fifoframe.data = fifo_data; + + /* Length of FIFO frame. */ + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; + + /* Set FIFO configuration by enabling accel, gyro. + * NOTE 1: The header mode is enabled by default. + * NOTE 2: By default the FIFO operating mode is FIFO mode. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("FIFO is configured in headerless mode\n"); + + /* To enable headerless mode, disable the header. */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_fifo_config(BMI2_FIFO_HEADER_EN, BMI2_DISABLE, &bmi2_dev); + } + + /* FIFO water-mark interrupt is enabled. */ + fifoframe.data_int_map = BMI2_FWM_INT; + + /* Map water-mark interrupt to the required interrupt pin. */ + rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Set water-mark level. */ + fifoframe.wm_lvl = BMI270_FIFO_WATERMARK_LEVEL; + + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; + + /* Set the water-mark level if water-mark interrupt is mapped. */ + rslt = bmi2_set_fifo_wm(fifoframe.wm_lvl, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + rslt = bmi2_get_fifo_wm(&watermark, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + printf("\nFIFO watermark level is %d\n", watermark); + + while (try <= 10) + { + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the status of FIFO watermark interrupt. */ + if ((rslt == BMI2_OK) && (int_status & BMI2_FWM_INT_STATUS_MASK)) + { + printf("\nIteration : %d\n", try); + + printf("\nWatermark interrupt occurred\n"); + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + printf("\nFIFO data bytes available : %d \n", fifo_length); + printf("\nFIFO data bytes requested : %d \n", fifoframe.length); + + /* Read FIFO data. */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nFIFO accel frames requested : %d \n", accel_frame_length); + + /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ + rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); + + printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); + + /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ + rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); + + /* Calculating the frame count from the available bytes in FIFO + * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len)) */ + frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH)); + printf("\nAvailable frame count from available bytes: %d\n", frame_count); + + printf("\nExracted accel frames\n"); + + if (accel_frame_length > frame_count) + { + accel_frame_length = frame_count; + } + + /* Print the parsed accelerometer data from the FIFO buffer. */ + for (index = 0; index < accel_frame_length; index++) + { + printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, + fifo_accel_data[index].y, fifo_accel_data[index].z); + } + + printf("\nExtracted gyro frames\n"); + + if (gyro_frame_length > frame_count) + { + gyro_frame_length = frame_count; + } + + /* Print the parsed gyro data from the FIFO buffer. */ + for (index = 0; index < gyro_frame_length; index++) + { + printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", + index, + fifo_gyro_data[index].x, + fifo_gyro_data[index].y, + fifo_gyro_data[index].z); + } + } + + try++; + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accel and gyro configurations. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[0].type = BMI2_ACCEL; + config[1].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_hc_get_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameter according to their requirement. */ + /* Accel configuration settings. */ + /* Set Output Data Rate */ + config[0].cfg.acc.odr = BMI2_ACC_ODR_100HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Gyro configuration settings. */ + /* Set Output Data Rate */ + config[1].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set new configurations. */ + rslt = bmi270_hc_set_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/gyro/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/gyro/Makefile new file mode 100644 index 000000000..cb6bfaa3e --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/gyro/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= gyro.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_hc.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/gyro/gyro.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/gyro/gyro.c new file mode 100644 index 000000000..c7c50277f --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/gyro/gyro.c @@ -0,0 +1,195 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_hc.h" +#include "common.h" + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for gyro. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_gyro_config(struct bmi2_dev *dev); + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + * + * @param[in] val : LSB from each axis. + * @param[in] dps : Degree per second. + * @param[in] bit_width : Resolution for gyro. + * + * @return Degree per second. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to define limit to print gyro data. */ + uint8_t limit = 10; + + uint8_t indx = 0; + + float x = 0, y = 0, z = 0; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Create an instance of sensor data structure. */ + struct bmi2_sensor_data sensor_data = { 0 }; + + /* Assign gyro sensor to variable. */ + uint8_t sens_list = BMI2_GYRO; + + /* Initialize the interrupt status of gyro. */ + uint16_t int_status = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_hc. */ + rslt = bmi270_hc_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the selected sensors. */ + rslt = bmi270_hc_sensor_enable(&sens_list, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Gyro configuration settings. */ + rslt = set_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Select gyro sensor. */ + sensor_data.type = BMI2_GYRO; + printf("Gyro and DPS data\n"); + printf("Gyro data collected at Range 2000 dps with 16-bit resolution\n"); + + /* Loop to print gyro data when interrupt occurs. */ + while (indx <= limit) + { + /* To get the data ready interrupt status of gyro. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the data ready interrupt status and print the status for 10 samples. */ + if (int_status & BMI2_GYR_DRDY_INT_MASK) + { + /* Get gyro data for x, y and z axis. */ + rslt = bmi270_hc_get_sensor_data(&sensor_data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("\nGyr_X = %d\t", sensor_data.sens_data.gyr.x); + printf("Gyr_Y = %d\t", sensor_data.sens_data.gyr.y); + printf("Gyr_Z = %d\r\n", sensor_data.sens_data.gyr.z); + + /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ + x = lsb_to_dps(sensor_data.sens_data.gyr.x, 2000, bmi2_dev.resolution); + y = lsb_to_dps(sensor_data.sens_data.gyr.y, 2000, bmi2_dev.resolution); + z = lsb_to_dps(sensor_data.sens_data.gyr.z, 2000, bmi2_dev.resolution); + + /* Print the data in dps. */ + printf("Gyr_DPS_X = %4.2f, Gyr_DPS_Y = %4.2f, Gyr_DPS_Z = %4.2f\n", x, y, z); + + indx++; + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for gyro. + */ +static int8_t set_gyro_config(struct bmi2_dev *dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define the type of sensor and its configurations. */ + struct bmi2_sens_config config; + + /* Configure the type of feature. */ + config.type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_hc_get_sensor_config(&config, 1, dev); + bmi2_error_codes_print_result(rslt); + + /* Map data ready interrupt to interrupt pin. */ + rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT2, dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config.cfg.gyr.odr = BMI2_GYR_ODR_200HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config.cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config.cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config.cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config.cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set the gyro configurations. */ + rslt = bmi270_hc_set_sensor_config(&config, 1, dev); + } + + return rslt; +} + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/step_counter/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/step_counter/Makefile new file mode 100644 index 000000000..fc47b9bb1 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/step_counter/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= step_counter.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_hc.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/step_counter/step_counter.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/step_counter/step_counter.c new file mode 100644 index 000000000..785dc28ff --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/step_counter/step_counter.c @@ -0,0 +1,146 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_hc.h" +#include "common.h" + +/******************************************************************************/ +/*! Static function declaration */ + +/*! + * @brief This internal API is used to set configurations for step counter. + * + * @param[in] bmi2_dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_feature_config(struct bmi2_dev *bmi2_dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Structure to define type of sensor and their respective data. */ + struct bmi2_sensor_data sensor_data; + + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Accel sensor and step counter feature are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_STEP_COUNTER }; + + /* Variable to get step counter interrupt status. */ + uint16_t int_status = 0; + + /* Select features and their pins to be mapped to. */ + struct bmi2_sens_int_config sens_int = { .type = BMI2_STEP_COUNTER, .hw_int_pin = BMI2_INT2 }; + + /* Type of sensor to get step counter data. */ + sensor_data.type = BMI2_STEP_COUNTER; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_hc. */ + rslt = bmi270_hc_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the selected sensor. */ + rslt = bmi270_hc_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Set the feature configuration for step counter. */ + rslt = set_feature_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("Step counter watermark level set to 1 (20 steps)\n"); + + if (rslt == BMI2_OK) + { + /* Map the step counter feature interrupt. */ + rslt = bmi270_hc_map_feat_int(&sens_int, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Move the board in steps for 20 times to get step counter interrupt. */ + printf("Move the board in steps\n"); + + /* Loop to get number of steps counted. */ + do + { + /* To get the interrupt status of the step counter. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the interrupt status of the step counter. */ + if (int_status & BMI270_HC_STEP_CNT_STATUS_MASK) + { + printf("Step counter interrupt occurred when watermark level (20 steps) is reached\n"); + + /* Get step counter output. */ + rslt = bmi270_hc_get_sensor_data(&sensor_data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Print the step counter output. */ + printf("No of steps counted = %ld", sensor_data.sens_data.step_counter_output); + break; + } + } while (rslt == BMI2_OK); + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for step counter. + */ +static int8_t set_feature_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define the type of sensor and its configurations. */ + struct bmi2_sens_config config; + + /* Configure the type of sensor. */ + config.type = BMI2_STEP_COUNTER; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_hc_get_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Setting water-mark level to 1 for step counter to get interrupt after 20 step counts. Every 20 steps once + * output triggers. */ + config.cfg.step_counter.watermark_level = 1; + + /* Set new configuration. */ + rslt = bmi270_hc_set_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel/Makefile new file mode 100644 index 000000000..e4400a45b --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= accel.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_maximum_fifo.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel/accel.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel/accel.c new file mode 100644 index 000000000..c32a43a41 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel/accel.c @@ -0,0 +1,200 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_maximum_fifo.h" +#include "common.h" + +/******************************************************************************/ +/*! Macro definition */ + +/*! Earth's gravity in m/s^2 */ +#define GRAVITY_EARTH (9.80665f) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_accel_config(struct bmi2_dev *bmi2_dev); + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + * + * @param[in] val : LSB from each axis. + * @param[in] g_range : Gravity range. + * @param[in] bit_width : Resolution for accel. + * + * @return Gravity. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to define limit to print accel data. */ + uint8_t limit = 20; + + /* Assign accel sensor to variable. */ + uint8_t sensor_list = BMI2_ACCEL; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Create an instance of sensor data structure. */ + struct bmi2_sensor_data sensor_data = { 0 }; + + /* Initialize the interrupt status of accel. */ + uint16_t int_status = 0; + + uint8_t indx = 0; + float x = 0, y = 0, z = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_maximum_fifo. */ + rslt = bmi270_maximum_fifo_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the accel sensor. */ + rslt = bmi2_sensor_enable(&sensor_list, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Accel configuration settings. */ + rslt = set_accel_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Assign accel sensor. */ + sensor_data.type = BMI2_ACCEL; + printf("Accel and m/s2 data \n"); + printf("Accel data collected at 2G Range with 16-bit resolution\n"); + + /* Loop to print the accel data when interrupt occurs. */ + while (indx <= limit) + { + /* To get the status of accel data ready interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the accel data ready interrupt status and print the status for 10 samples. */ + if (int_status & BMI2_ACC_DRDY_INT_MASK) + { + /* Get accelerometer data for x, y and z axis. */ + rslt = bmi2_get_sensor_data(&sensor_data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + printf("\nAcc_X = %d\t", sensor_data.sens_data.acc.x); + printf("Acc_Y = %d\t", sensor_data.sens_data.acc.y); + printf("Acc_Z = %d\r\n", sensor_data.sens_data.acc.z); + + /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ + x = lsb_to_mps2(sensor_data.sens_data.acc.x, 2, bmi2_dev.resolution); + y = lsb_to_mps2(sensor_data.sens_data.acc.y, 2, bmi2_dev.resolution); + z = lsb_to_mps2(sensor_data.sens_data.acc.z, 2, bmi2_dev.resolution); + + /* Print the data in m/s2. */ + printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); + + indx++; + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel. + */ +static int8_t set_accel_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accelerometer configuration. */ + struct bmi2_sens_config config; + + /* Configure the type of feature. */ + config.type = BMI2_ACCEL; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi2_get_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config.cfg.acc.odr = BMI2_ACC_ODR_200HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config.cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples. + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config.cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config.cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set the accel configurations. */ + rslt = bmi2_set_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Map data ready interrupt to interrupt pin. */ + rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (GRAVITY_EARTH * val * g_range) / half_scale; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel_gyro/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel_gyro/Makefile new file mode 100644 index 000000000..a33f36617 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel_gyro/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= accel_gyro.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_maximum_fifo.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel_gyro/accel_gyro.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel_gyro/accel_gyro.c new file mode 100644 index 000000000..b77ea1d81 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel_gyro/accel_gyro.c @@ -0,0 +1,268 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_maximum_fifo.h" +#include "common.h" + +/******************************************************************************/ +/*! Macro definition */ + +/*! Earth's gravity in m/s^2 */ +#define GRAVITY_EARTH (9.80665f) + +/*! Macros to select the sensors */ +#define ACCEL UINT8_C(0x00) +#define GYRO UINT8_C(0x01) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev); + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + * + * @param[in] val : LSB from each axis. + * @param[in] g_range : Gravity range. + * @param[in] bit_width : Resolution for accel. + * + * @return Gravity. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + * + * @param[in] val : LSB from each axis. + * @param[in] dps : Degree per second. + * @param[in] bit_width : Resolution for gyro. + * + * @return Degree per second. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to define limit to print accel data. */ + uint8_t limit = 10; + + /* Assign accel and gyro sensor to variable. */ + uint8_t sensor_list[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Create an instance of sensor data structure. */ + struct bmi2_sensor_data sensor_data[2] = { { 0 } }; + + /* Initialize the interrupt status of accel and gyro. */ + uint16_t int_status = 0; + + uint8_t indx = 1; + + float x = 0, y = 0, z = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_maximum_fifo. */ + rslt = bmi270_maximum_fifo_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the accel and gyro sensor. */ + rslt = bmi2_sensor_enable(sensor_list, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Accel and gyro configuration settings. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Assign accel and gyro sensor. */ + sensor_data[ACCEL].type = BMI2_ACCEL; + sensor_data[GYRO].type = BMI2_GYRO; + + /* Loop to print accel and gyro data when interrupt occurs. */ + while (indx <= limit) + { + /* To get the data ready interrupt status of accel and gyro. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the data ready interrupt status and print the status for 10 samples. */ + if ((int_status & BMI2_ACC_DRDY_INT_MASK) && (int_status & BMI2_GYR_DRDY_INT_MASK)) + { + /* Get accel and gyro data for x, y and z axis. */ + rslt = bmi2_get_sensor_data(sensor_data, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("\n******* Accel(Raw and m/s2) Gyro(Raw and dps) data : %d *******\n", indx); + + printf("\nAcc_x = %d\t", sensor_data[ACCEL].sens_data.acc.x); + printf("Acc_y = %d\t", sensor_data[ACCEL].sens_data.acc.y); + printf("Acc_z = %d", sensor_data[ACCEL].sens_data.acc.z); + + /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ + x = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.x, 2, bmi2_dev.resolution); + y = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.y, 2, bmi2_dev.resolution); + z = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.z, 2, bmi2_dev.resolution); + + /* Print the data in m/s2. */ + printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); + + printf("\nGyr_X = %d\t", sensor_data[GYRO].sens_data.gyr.x); + printf("Gyr_Y = %d\t", sensor_data[GYRO].sens_data.gyr.y); + printf("Gyr_Z= %d\n", sensor_data[GYRO].sens_data.gyr.z); + + /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ + x = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.x, 2000, bmi2_dev.resolution); + y = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.y, 2000, bmi2_dev.resolution); + z = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.z, 2000, bmi2_dev.resolution); + + /* Print the data in dps. */ + printf("Gyro_DPS_X = %4.2f, Gyro_DPS_Y = %4.2f, Gyro_DPS_Z = %4.2f\n", x, y, z); + + indx++; + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accelerometer and gyro configuration. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[ACCEL].type = BMI2_ACCEL; + config[GYRO].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi2_get_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Map data ready interrupt to interrupt pin. */ + rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config[ACCEL].cfg.acc.odr = BMI2_ACC_ODR_200HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[ACCEL].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples. + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[ACCEL].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[ACCEL].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config[GYRO].cfg.gyr.odr = BMI2_GYR_ODR_200HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[GYRO].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[GYRO].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[GYRO].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[GYRO].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set the accel and gyro configurations. */ + rslt = bmi2_set_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (GRAVITY_EARTH * val * g_range) / half_scale; +} + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/common/common.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/common/common.c new file mode 100644 index 000000000..afff37bc8 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/common/common.c @@ -0,0 +1,372 @@ +/** + * Copyright (C) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +#include +#include +#include + +#include "coines.h" +#include "bmi2_defs.h" + +/******************************************************************************/ +/*! Macro definitions */ +#define BMI2XY_SHUTTLE_ID UINT16_C(0x1B8) + +/*! Macro that defines read write length */ +#define READ_WRITE_LEN UINT8_C(46) + +/******************************************************************************/ +/*! Static variable definition */ + +/*! Variable that holds the I2C device address or SPI chip selection */ +static uint8_t dev_addr; + +/******************************************************************************/ +/*! User interface functions */ + +/*! + * I2C read function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_read_i2c(dev_addr, reg_addr, reg_data, (uint16_t)len); +} + +/*! + * I2C write function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_write_i2c(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len); +} + +/*! + * SPI read function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_read_spi(dev_addr, reg_addr, reg_data, (uint16_t)len); +} + +/*! + * SPI write function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_write_spi(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len); +} + +/*! + * Delay function map to COINES platform + */ +void bmi2_delay_us(uint32_t period, void *intf_ptr) +{ + coines_delay_usec(period); +} + +/*! + * @brief Function to select the interface between SPI and I2C. + * Also to initialize coines platform + */ +int8_t bmi2_interface_init(struct bmi2_dev *bmi, uint8_t intf) +{ + int8_t rslt = BMI2_OK; + struct coines_board_info board_info; + + if (bmi != NULL) + { + int16_t result = coines_open_comm_intf(COINES_COMM_INTF_USB); + if (result < COINES_SUCCESS) + { + printf( + "\n Unable to connect with Application Board ! \n" " 1. Check if the board is connected and powered on. \n" " 2. Check if Application Board USB driver is installed. \n" + " 3. Check if board is in use by another application. (Insufficient permissions to access USB) \n"); + exit(result); + } + + result = coines_get_board_info(&board_info); + +#if defined(PC) + setbuf(stdout, NULL); +#endif + + if (result == COINES_SUCCESS) + { + if ((board_info.shuttle_id != BMI2XY_SHUTTLE_ID)) + { + printf("! Warning invalid sensor shuttle \n ," "This application will not support this sensor \n"); + exit(COINES_E_FAILURE); + } + } + + coines_set_shuttleboard_vdd_vddio_config(0, 0); + coines_delay_msec(100); + + /* Bus configuration : I2C */ + if (intf == BMI2_I2C_INTF) + { + printf("I2C Interface \n"); + + /* To initialize the user I2C function */ + dev_addr = BMI2_I2C_PRIM_ADDR; + bmi->intf = BMI2_I2C_INTF; + bmi->read = bmi2_i2c_read; + bmi->write = bmi2_i2c_write; + coines_config_i2c_bus(COINES_I2C_BUS_0, COINES_I2C_FAST_MODE); + } + /* Bus configuration : SPI */ + else if (intf == BMI2_SPI_INTF) + { + printf("SPI Interface \n"); + + /* To initialize the user SPI function */ + dev_addr = COINES_SHUTTLE_PIN_7; + bmi->intf = BMI2_SPI_INTF; + bmi->read = bmi2_spi_read; + bmi->write = bmi2_spi_write; + coines_config_spi_bus(COINES_SPI_BUS_0, COINES_SPI_SPEED_5_MHZ, COINES_SPI_MODE3); + coines_set_pin_config(COINES_SHUTTLE_PIN_7, COINES_PIN_DIRECTION_OUT, COINES_PIN_VALUE_HIGH); + } + + /* Assign device address to interface pointer */ + bmi->intf_ptr = &dev_addr; + + /* Configure delay in microseconds */ + bmi->delay_us = bmi2_delay_us; + + /* Configure max read/write length (in bytes) ( Supported length depends on target machine) */ + bmi->read_write_len = READ_WRITE_LEN; + + /* Assign to NULL to load the default config file. */ + bmi->config_file_ptr = NULL; + + coines_delay_usec(10000); + + coines_set_shuttleboard_vdd_vddio_config(3300, 3300); + + coines_delay_usec(10000); + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; + +} + +/*! + * @brief Prints the execution status of the APIs. + */ +void bmi2_error_codes_print_result(int8_t rslt) +{ + switch (rslt) + { + case BMI2_OK: + + /* Do nothing */ + break; + + case BMI2_W_FIFO_EMPTY: + printf("Warning [%d] : FIFO empty\r\n", rslt); + break; + case BMI2_W_PARTIAL_READ: + printf("Warning [%d] : FIFO partial read\r\n", rslt); + break; + case BMI2_E_NULL_PTR: + printf( + "Error [%d] : Null pointer error. It occurs when the user tries to assign value (not address) to a pointer," " which has been initialized to NULL.\r\n", + rslt); + break; + + case BMI2_E_COM_FAIL: + printf( + "Error [%d] : Communication failure error. It occurs due to read/write operation failure and also due " "to power failure during communication\r\n", + rslt); + break; + + case BMI2_E_DEV_NOT_FOUND: + printf("Error [%d] : Device not found error. It occurs when the device chip id is incorrectly read\r\n", + rslt); + break; + + case BMI2_E_INVALID_SENSOR: + printf( + "Error [%d] : Invalid sensor error. It occurs when there is a mismatch in the requested feature with the " "available one\r\n", + rslt); + break; + + case BMI2_E_SELF_TEST_FAIL: + printf( + "Error [%d] : Self-test failed error. It occurs when the validation of accel self-test data is " "not satisfied\r\n", + rslt); + break; + + case BMI2_E_INVALID_INT_PIN: + printf( + "Error [%d] : Invalid interrupt pin error. It occurs when the user tries to configure interrupt pins " "apart from INT1 and INT2\r\n", + rslt); + break; + + case BMI2_E_OUT_OF_RANGE: + printf( + "Error [%d] : Out of range error. It occurs when the data exceeds from filtered or unfiltered data from " "fifo and also when the range exceeds the maximum range for accel and gyro while performing FOC\r\n", + rslt); + break; + + case BMI2_E_ACC_INVALID_CFG: + printf( + "Error [%d] : Invalid Accel configuration error. It occurs when there is an error in accel configuration" " register which could be one among range, BW or filter performance in reg address 0x40\r\n", + rslt); + break; + + case BMI2_E_GYRO_INVALID_CFG: + printf( + "Error [%d] : Invalid Gyro configuration error. It occurs when there is a error in gyro configuration" "register which could be one among range, BW or filter performance in reg address 0x42\r\n", + rslt); + break; + + case BMI2_E_ACC_GYR_INVALID_CFG: + printf( + "Error [%d] : Invalid Accel-Gyro configuration error. It occurs when there is a error in accel and gyro" " configuration registers which could be one among range, BW or filter performance in reg address 0x40 " "and 0x42\r\n", + rslt); + break; + + case BMI2_E_CONFIG_LOAD: + printf( + "Error [%d] : Configuration load error. It occurs when failure observed while loading the configuration " "into the sensor\r\n", + rslt); + break; + + case BMI2_E_INVALID_PAGE: + printf( + "Error [%d] : Invalid page error. It occurs due to failure in writing the correct feature configuration " "from selected page\r\n", + rslt); + break; + + case BMI2_E_SET_APS_FAIL: + printf( + "Error [%d] : APS failure error. It occurs due to failure in write of advance power mode configuration " "register\r\n", + rslt); + break; + + case BMI2_E_AUX_INVALID_CFG: + printf( + "Error [%d] : Invalid AUX configuration error. It occurs when the auxiliary interface settings are not " "enabled properly\r\n", + rslt); + break; + + case BMI2_E_AUX_BUSY: + printf( + "Error [%d] : AUX busy error. It occurs when the auxiliary interface buses are engaged while configuring" " the AUX\r\n", + rslt); + break; + + case BMI2_E_REMAP_ERROR: + printf( + "Error [%d] : Remap error. It occurs due to failure in assigning the remap axes data for all the axes " "after change in axis position\r\n", + rslt); + break; + + case BMI2_E_GYR_USER_GAIN_UPD_FAIL: + printf( + "Error [%d] : Gyro user gain update fail error. It occurs when the reading of user gain update status " "fails\r\n", + rslt); + break; + + case BMI2_E_SELF_TEST_NOT_DONE: + printf( + "Error [%d] : Self-test not done error. It occurs when the self-test process is ongoing or not " "completed\r\n", + rslt); + break; + + case BMI2_E_INVALID_INPUT: + printf("Error [%d] : Invalid input error. It occurs when the sensor input validity fails\r\n", rslt); + break; + + case BMI2_E_INVALID_STATUS: + printf("Error [%d] : Invalid status error. It occurs when the feature/sensor validity fails\r\n", rslt); + break; + + case BMI2_E_CRT_ERROR: + printf("Error [%d] : CRT error. It occurs when the CRT test has failed\r\n", rslt); + break; + + case BMI2_E_ST_ALREADY_RUNNING: + printf( + "Error [%d] : Self-test already running error. It occurs when the self-test is already running and " "another has been initiated\r\n", + rslt); + break; + + case BMI2_E_CRT_READY_FOR_DL_FAIL_ABORT: + printf( + "Error [%d] : CRT ready for download fail abort error. It occurs when download in CRT fails due to wrong " "address location\r\n", + rslt); + break; + + case BMI2_E_DL_ERROR: + printf( + "Error [%d] : Download error. It occurs when write length exceeds that of the maximum burst length\r\n", + rslt); + break; + + case BMI2_E_PRECON_ERROR: + printf( + "Error [%d] : Pre-conditional error. It occurs when precondition to start the feature was not " "completed\r\n", + rslt); + break; + + case BMI2_E_ABORT_ERROR: + printf("Error [%d] : Abort error. It occurs when the device was shaken during CRT test\r\n", rslt); + break; + + case BMI2_E_WRITE_CYCLE_ONGOING: + printf( + "Error [%d] : Write cycle ongoing error. It occurs when the write cycle is already running and another " "has been initiated\r\n", + rslt); + break; + + case BMI2_E_ST_NOT_RUNING: + printf( + "Error [%d] : Self-test is not running error. It occurs when self-test running is disabled while it's " "running\r\n", + rslt); + break; + + case BMI2_E_DATA_RDY_INT_FAILED: + printf( + "Error [%d] : Data ready interrupt error. It occurs when the sample count exceeds the FOC sample limit " "and data ready status is not updated\r\n", + rslt); + break; + + case BMI2_E_INVALID_FOC_POSITION: + printf( + "Error [%d] : Invalid FOC position error. It occurs when average FOC data is obtained for the wrong" " axes\r\n", + rslt); + break; + + default: + printf("Error [%d] : Unknown error code\r\n", rslt); + break; + } +} + +/*! + * @brief Deinitializes coines platform + * + * @return void. + */ +void bmi2_coines_deinit(void) +{ + coines_close_comm_intf(COINES_COMM_INTF_USB); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/common/common.h b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/common/common.h new file mode 100644 index 000000000..763983da4 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/common/common.h @@ -0,0 +1,122 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +#ifndef _COMMON_H +#define _COMMON_H + +/*! CPP guard */ +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include "bmi2.h" + +/*! + * @brief Function for reading the sensor's registers through I2C bus. + * + * @param[in] reg_addr : Register address. + * @param[out] reg_data : Pointer to the data buffer to store the read data. + * @param[in] length : No of bytes to read. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief Function for writing the sensor's registers through I2C bus. + * + * @param[in] reg_addr : Register address. + * @param[in] reg_data : Pointer to the data buffer whose value is to be written. + * @param[in] length : No of bytes to write. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief Function for reading the sensor's registers through SPI bus. + * + * @param[in] reg_addr : Register address. + * @param[out] reg_data : Pointer to the data buffer to store the read data. + * @param[in] length : No of bytes to read. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief Function for writing the sensor's registers through SPI bus. + * + * @param[in] reg_addr : Register address. + * @param[in] reg_data : Pointer to the data buffer whose data has to be written. + * @param[in] length : No of bytes to write. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief This function provides the delay for required time (Microsecond) as per the input provided in some of the + * APIs. + * + * @param[in] period_us : The required wait time in microsecond. + * @param[in] intf_ptr : Interface pointer + * + * @return void. + * + */ +void bmi2_delay_us(uint32_t period, void *intf_ptr); + +/*! + * @brief Function to select the interface between SPI and I2C. + * + * @param[in] bma : Structure instance of bmi2_dev + * @param[in] intf : Interface selection parameter + * + * @return Status of execution + * @retval 0 -> Success + * @retval < 0 -> Failure Info + */ +int8_t bmi2_interface_init(struct bmi2_dev *bma, uint8_t intf); + +/*! + * @brief Prints the execution status of the APIs. + * + * @param[in] rslt : Error code returned by the API whose execution status has to be printed. + * + * @return void. + */ +void bmi2_error_codes_print_result(int8_t rslt); + +/*! + * @brief Deinitializes coines platform + * + * @return void. + */ +void bmi2_coines_deinit(void); + +#ifdef __cplusplus +} +#endif /* End of CPP guard */ + +#endif /* _COMMON_H */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/component_retrim/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/component_retrim/Makefile new file mode 100644 index 000000000..0729972a2 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/component_retrim/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= component_retrim.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_maximum_fifo.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/component_retrim/component_retrim.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/component_retrim/component_retrim.c new file mode 100644 index 000000000..ea24ab2e9 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/component_retrim/component_retrim.c @@ -0,0 +1,52 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_maximum_fifo.h" +#include "common.h" + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_maximum_fifo. */ + rslt = bmi270_maximum_fifo_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* This API runs the CRT process. */ + rslt = bmi2_do_crt(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Do not move the board while doing CRT. If so, it will throw an abort error. */ + if (rslt == BMI2_OK) + { + printf("CRT successfully completed."); + } + } + + bmi2_coines_deinit(); + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_header_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_header_mode/Makefile new file mode 100644 index 000000000..131538f30 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_header_mode/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= fifo_watermark_header_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_maximum_fifo.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_header_mode/fifo_watermark_header_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_header_mode/fifo_watermark_header_mode.c new file mode 100644 index 000000000..f031379f7 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_header_mode/fifo_watermark_header_mode.c @@ -0,0 +1,335 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_maximum_fifo.h" +#include "common.h" + +/******************************************************************************/ +/*! Macros */ + +/*! Buffer size allocated to store raw FIFO data */ +#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) + +/*! Length of data to be read from FIFO */ +#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) + +/*! Number of accel frames to be extracted from FIFO */ + +/*! + * Calculation: + * fifo_watermark_level = 650, accel_frame_len = 6, gyro_frame_len = 6 header_byte = 1. + * fifo_accel_frame_count = (650 / (6 + 6 + 1 )) = 50 frames + * NOTE: Extra frames are read in order to get sensor time + */ +#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(55) + +/*! Number of gyro frames to be extracted from FIFO */ +#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(55) + +/*! Setting a watermark level in FIFO */ +#define BMI270_FIFO_WATERMARK_LEVEL UINT16_C(650) + +/*! Macro to read sensortime byte in FIFO */ +#define SENSORTIME_OVERHEAD_BYTE UINT8_C(3) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + * @param[in] dev : Structure instance of bmi2_dev. + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to store the available frame length count in FIFO */ + uint8_t frame_count; + + /* Variable to index bytes. */ + uint16_t index = 0; + + uint16_t fifo_length = 0; + + uint16_t accel_frame_length; + + uint16_t gyro_frame_length; + + uint8_t try = 1; + + /* To read sensortime, extra 3 bytes are added to fifo buffer */ + uint16_t fifo_buffer_size = BMI2_FIFO_RAW_DATA_BUFFER_SIZE + SENSORTIME_OVERHEAD_BYTE; + + /* Number of bytes of FIFO data. */ + uint8_t fifo_data[fifo_buffer_size]; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Array of accelerometer frames -> Total bytes = + * 55 * (6 axes bytes) = 330 bytes */ + struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; + + /* Array of gyro frames -> Total bytes = + * 55 * (6 axes bytes) = 330 bytes */ + struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; + + /* Initialize FIFO frame structure. */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Accel and gyro sensor are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Variable to get fifo water-mark interrupt status. */ + uint16_t int_status = 0; + + uint16_t watermark = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_maximum_fifo. */ + rslt = bmi270_maximum_fifo_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable accel and gyro sensor. */ + rslt = bmi2_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configuration settings for accel and gyro. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Before setting FIFO, disable the advance power save mode. */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Update FIFO structure. */ + /* Mapping the buffer to store the fifo data. */ + fifoframe.data = fifo_data; + + /* Length of FIFO frame. */ + /* To read sensortime, extra 3 bytes are added to fifo user length. */ + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH + SENSORTIME_OVERHEAD_BYTE; + + /* Set FIFO configuration by enabling accel, gyro and timestamp. + * NOTE 1: The header mode is enabled by default. + * NOTE 2: By default the FIFO operating mode is FIFO mode. + * NOTE 3: Sensortime is enabled by default */ + printf("FIFO is configured in header mode\n"); + rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* FIFO water-mark interrupt is enabled. */ + fifoframe.data_int_map = BMI2_FWM_INT; + + /* Map water-mark interrupt to the required interrupt pin. */ + rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Set water-mark level. */ + fifoframe.wm_lvl = BMI270_FIFO_WATERMARK_LEVEL; + + /* Set the water-mark level if water-mark interrupt is mapped. */ + rslt = bmi2_set_fifo_wm(fifoframe.wm_lvl, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + rslt = bmi2_get_fifo_wm(&watermark, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + printf("\nFIFO watermark level is %d\n", watermark); + + while (try <= 10) + { + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the status of FIFO watermark interrupt. */ + if ((rslt == BMI2_OK) && (int_status & BMI2_FWM_INT_STATUS_MASK)) + { + printf("\nIteration : %d\n", try); + + printf("\nWatermark interrupt occurred\n"); + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + printf("\nFIFO data bytes available : %d \n", fifo_length); + printf("\nFIFO data bytes requested : %d \n", fifoframe.length); + + /* Read FIFO data. */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nFIFO accel frames requested : %d \n", accel_frame_length); + + /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ + rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); + + printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); + + /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ + rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); + + /* Calculating the frame count from the available bytes in FIFO + * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len + header_byte)) */ + frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH + 1)); + printf("\nAvailable frame count from available bytes: %d\n", frame_count); + + printf("\nExracted accel frames\n"); + + if (accel_frame_length > frame_count) + { + accel_frame_length = frame_count; + } + + /* Print the parsed accelerometer data from the FIFO buffer. */ + for (index = 0; index < accel_frame_length; index++) + { + printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, + fifo_accel_data[index].y, fifo_accel_data[index].z); + } + + printf("\nExtracted gyro frames\n"); + + if (gyro_frame_length > frame_count) + { + gyro_frame_length = frame_count; + } + + /* Print the parsed gyro data from the FIFO buffer. */ + for (index = 0; index < gyro_frame_length; index++) + { + printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", + index, + fifo_gyro_data[index].x, + fifo_gyro_data[index].y, + fifo_gyro_data[index].z); + } + + /* Print control frames like sensor time and skipped frame count. */ + printf("Skipped frame count = %d\n", fifoframe.skipped_frame_count); + printf("Sensor time = %lu\r\n", fifoframe.sensor_time); + + try++; + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accel and gyro configurations. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[0].type = BMI2_ACCEL; + config[1].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi2_get_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Accel configuration settings. */ + /* Set Output Data Rate */ + config[0].cfg.acc.odr = BMI2_ACC_ODR_25HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Gyro configuration settings. */ + /* Set Output Data Rate */ + config[1].cfg.gyr.odr = BMI2_GYR_ODR_25HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set new configurations. */ + rslt = bmi2_set_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_headerless_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_headerless_mode/Makefile new file mode 100644 index 000000000..9790a92d4 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_headerless_mode/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= fifo_watermark_headerless_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_maximum_fifo.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c new file mode 100644 index 000000000..541ff1116 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c @@ -0,0 +1,331 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_maximum_fifo.h" +#include "common.h" + +/******************************************************************************/ +/*! Macros */ + +/*! Buffer size allocated to store raw FIFO data */ +#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) + +/*! Length of data to be read from FIFO */ +#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) + +/*! Number of accel frames to be extracted from FIFO */ + +/*! + * Calculation: + * fifo_watermark_level = 650, accel_frame_len = 6, gyro_frame_len = 6. + * fifo_accel_frame_count = (650 / (6 + 6 )) = 54 frames + */ +#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(55) + +/*! Number of gyro frames to be extracted from FIFO */ +#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(55) + +/*! Setting a watermark level in FIFO */ +#define BMI270_FIFO_WATERMARK_LEVEL UINT16_C(650) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + * @param[in] dev : Structure instance of bmi2_dev. + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to index bytes. */ + uint16_t index = 0; + + /* Variable to store the available frame length count in FIFO */ + uint8_t frame_count; + + uint16_t fifo_length = 0; + + uint16_t accel_frame_length; + + uint16_t gyro_frame_length; + + uint8_t try = 1; + + /* Number of bytes of FIFO data. */ + uint8_t fifo_data[BMI2_FIFO_RAW_DATA_BUFFER_SIZE] = { 0 }; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Array of accelerometer frames -> Total bytes = + * 50 * (6 axes bytes) = 300 bytes */ + struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; + + /* Array of gyro frames -> Total bytes = + * 50 * (6 axes bytes) = 300 bytes */ + struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; + + /* Initialize FIFO frame structure. */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Accel and gyro sensor are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Variable to get fifo water-mark interrupt status. */ + uint16_t int_status = 0; + + uint16_t watermark = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_maximum_fifo_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable accel and gyro sensor. */ + rslt = bmi2_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configuration settings for accel and gyro. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Before setting FIFO, disable the advance power save mode. */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Update FIFO structure. */ + /* Mapping the buffer to store the fifo data. */ + fifoframe.data = fifo_data; + + /* Length of FIFO frame. */ + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; + + /* Set FIFO configuration by enabling accel, gyro. + * NOTE 1: The header mode is enabled by default. + * NOTE 2: By default the FIFO operating mode is FIFO mode. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("FIFO is configured in headerless mode\n"); + + /* To enable headerless mode, disable the header. */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_fifo_config(BMI2_FIFO_HEADER_EN, BMI2_DISABLE, &bmi2_dev); + } + + /* FIFO water-mark interrupt is enabled. */ + fifoframe.data_int_map = BMI2_FWM_INT; + + /* Map water-mark interrupt to the required interrupt pin. */ + rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Set water-mark level. */ + fifoframe.wm_lvl = BMI270_FIFO_WATERMARK_LEVEL; + + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; + + /* Set the water-mark level if water-mark interrupt is mapped. */ + rslt = bmi2_set_fifo_wm(fifoframe.wm_lvl, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + rslt = bmi2_get_fifo_wm(&watermark, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + printf("\nFIFO watermark level is %d\n", watermark); + + while (try <= 10) + { + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the status of FIFO watermark interrupt. */ + if ((rslt == BMI2_OK) && (int_status & BMI2_FWM_INT_STATUS_MASK)) + { + printf("\nIteration : %d\n", try); + + printf("\nWatermark interrupt occurred\n"); + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + printf("\nFIFO data bytes available : %d \n", fifo_length); + printf("\nFIFO data bytes requested : %d \n", fifoframe.length); + + /* Read FIFO data. */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nFIFO accel frames requested : %d \n", accel_frame_length); + + /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ + rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); + + printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); + + /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ + rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); + + /* Calculating the frame count from the available bytes in FIFO + * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len)) */ + frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH)); + printf("\nAvailable frame count from available bytes: %d\n", frame_count); + + printf("\nExracted accel frames\n"); + + if (accel_frame_length > frame_count) + { + accel_frame_length = frame_count; + } + + /* Print the parsed accelerometer data from the FIFO buffer. */ + for (index = 0; index < accel_frame_length; index++) + { + printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, + fifo_accel_data[index].y, fifo_accel_data[index].z); + } + + printf("\nExtracted gyro frames\n"); + + if (gyro_frame_length > frame_count) + { + gyro_frame_length = frame_count; + } + + /* Print the parsed gyro data from the FIFO buffer. */ + for (index = 0; index < gyro_frame_length; index++) + { + printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", + index, + fifo_gyro_data[index].x, + fifo_gyro_data[index].y, + fifo_gyro_data[index].z); + } + } + + try++; + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accel and gyro configurations. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[0].type = BMI2_ACCEL; + config[1].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi2_get_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameter according to their requirement. */ + /* Accel configuration settings. */ + /* Set Output Data Rate */ + config[0].cfg.acc.odr = BMI2_ACC_ODR_100HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Gyro configuration settings. */ + /* Set Output Data Rate */ + config[1].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set new configurations. */ + rslt = bmi2_set_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/gyro/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/gyro/Makefile new file mode 100644 index 000000000..5ab6e7fd7 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/gyro/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= gyro.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_maximum_fifo.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/gyro/gyro.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/gyro/gyro.c new file mode 100644 index 000000000..8b56f0842 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/gyro/gyro.c @@ -0,0 +1,195 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_maximum_fifo.h" +#include "common.h" + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for gyro. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_gyro_config(struct bmi2_dev *dev); + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + * + * @param[in] val : LSB from each axis. + * @param[in] dps : Degree per second. + * @param[in] bit_width : Resolution for gyro. + * + * @return Degree per second. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to define limit to print gyro data. */ + uint8_t limit = 10; + + uint8_t indx = 0; + + float x = 0, y = 0, z = 0; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Create an instance of sensor data structure. */ + struct bmi2_sensor_data sensor_data = { 0 }; + + /* Assign gyro sensor to variable. */ + uint8_t sens_list = BMI2_GYRO; + + /* Initialize the interrupt status of gyro. */ + uint16_t int_status = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_maximum_fifo_init */ + rslt = bmi270_maximum_fifo_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the selected sensors. */ + rslt = bmi2_sensor_enable(&sens_list, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Gyro configuration settings. */ + rslt = set_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Select gyro sensor. */ + sensor_data.type = BMI2_GYRO; + printf("Gyro and DPS data\n"); + printf("Gyro data collected at Range 2000 dps with 16-bit resolution\n"); + + /* Loop to print gyro data when interrupt occurs. */ + while (indx <= limit) + { + /* To get the data ready interrupt status of gyro. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the data ready interrupt status and print the status for 10 samples. */ + if (int_status & BMI2_GYR_DRDY_INT_MASK) + { + /* Get gyro data for x, y and z axis. */ + rslt = bmi2_get_sensor_data(&sensor_data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("\nGyr_X = %d\t", sensor_data.sens_data.gyr.x); + printf("Gyr_Y = %d\t", sensor_data.sens_data.gyr.y); + printf("Gyr_Z = %d\r\n", sensor_data.sens_data.gyr.z); + + /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ + x = lsb_to_dps(sensor_data.sens_data.gyr.x, 2000, bmi2_dev.resolution); + y = lsb_to_dps(sensor_data.sens_data.gyr.y, 2000, bmi2_dev.resolution); + z = lsb_to_dps(sensor_data.sens_data.gyr.z, 2000, bmi2_dev.resolution); + + /* Print the data in dps. */ + printf("Gyr_DPS_X = %4.2f, Gyr_DPS_Y = %4.2f, Gyr_DPS_Z = %4.2f\n", x, y, z); + + indx++; + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for gyro. + */ +static int8_t set_gyro_config(struct bmi2_dev *dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define the type of sensor and its configurations. */ + struct bmi2_sens_config config; + + /* Configure the type of feature. */ + config.type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi2_get_sensor_config(&config, 1, dev); + bmi2_error_codes_print_result(rslt); + + /* Map data ready interrupt to interrupt pin. */ + rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT2, dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config.cfg.gyr.odr = BMI2_GYR_ODR_200HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config.cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config.cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config.cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config.cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set the gyro configurations. */ + rslt = bmi2_set_sensor_config(&config, 1, dev); + } + + return rslt; +} + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel/Makefile new file mode 100644 index 000000000..3ab55f00e --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= accel.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_wh.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel/accel.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel/accel.c new file mode 100644 index 000000000..cf6825229 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel/accel.c @@ -0,0 +1,201 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_wh.h" +#include "common.h" + +/******************************************************************************/ +/*! Macro definition */ + +/*! Earth's gravity in m/s^2 */ +#define GRAVITY_EARTH (9.80665f) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_accel_config(struct bmi2_dev *bmi2_dev); + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + * + * @param[in] val : LSB from each axis. + * @param[in] g_range : Gravity range. + * @param[in] bit_width : Resolution for accel. + * + * @return Gravity. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to define limit to print accel data. */ + uint8_t limit = 20; + + /* Assign accel sensor to variable. */ + uint8_t sensor_list = BMI2_ACCEL; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Create an instance of sensor data structure. */ + struct bmi2_sensor_data sensor_data = { 0 }; + + /* Initialize the interrupt status of accel. */ + uint16_t int_status = 0; + + uint8_t indx = 0; + float x = 0, y = 0, z = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_wh. */ + rslt = bmi270_wh_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the accel sensor. */ + rslt = bmi270_wh_sensor_enable(&sensor_list, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Accel configuration settings. */ + rslt = set_accel_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Assign accel sensor. */ + sensor_data.type = BMI2_ACCEL; + printf("Accel and m/s2 data \n"); + printf("Accel data collected at 2G Range with 16-bit resolution\n"); + + /* Loop to print the accel data when interrupt occurs. */ + while (indx <= limit) + { + /* To get the status of accel data ready interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the accel data ready interrupt status and print the status for 10 samples. */ + if (int_status & BMI2_ACC_DRDY_INT_MASK) + { + /* Get accelerometer data for x, y and z axis. */ + rslt = bmi270_wh_get_sensor_data(&sensor_data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + printf("\nAcc_X = %d\t", sensor_data.sens_data.acc.x); + printf("Acc_Y = %d\t", sensor_data.sens_data.acc.y); + printf("Acc_Z = %d\r\n", sensor_data.sens_data.acc.z); + + /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ + x = lsb_to_mps2(sensor_data.sens_data.acc.x, 2, bmi2_dev.resolution); + y = lsb_to_mps2(sensor_data.sens_data.acc.y, 2, bmi2_dev.resolution); + z = lsb_to_mps2(sensor_data.sens_data.acc.z, 2, bmi2_dev.resolution); + + /* Print the data in m/s2. */ + printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); + + indx++; + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel. + */ +static int8_t set_accel_config(struct bmi2_dev *bmi2_dev) +{ + + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accelerometer configuration. */ + struct bmi2_sens_config config; + + /* Configure the type of feature. */ + config.type = BMI2_ACCEL; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_wh_get_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config.cfg.acc.odr = BMI2_ACC_ODR_200HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config.cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples. + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config.cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config.cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set the accel configurations. */ + rslt = bmi270_wh_set_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Map data ready interrupt to interrupt pin. */ + rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (GRAVITY_EARTH * val * g_range) / half_scale; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel_gyro/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel_gyro/Makefile new file mode 100644 index 000000000..1d3ea28c5 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel_gyro/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= accel_gyro.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_wh.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel_gyro/accel_gyro.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel_gyro/accel_gyro.c new file mode 100644 index 000000000..e543cca7c --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel_gyro/accel_gyro.c @@ -0,0 +1,268 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_wh.h" +#include "common.h" + +/******************************************************************************/ +/*! Macro definition */ + +/*! Earth's gravity in m/s^2 */ +#define GRAVITY_EARTH (9.80665f) + +/*! Macros to select the sensors */ +#define ACCEL UINT8_C(0x00) +#define GYRO UINT8_C(0x01) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev); + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + * + * @param[in] val : LSB from each axis. + * @param[in] g_range : Gravity range. + * @param[in] bit_width : Resolution for accel. + * + * @return Gravity. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + * + * @param[in] val : LSB from each axis. + * @param[in] dps : Degree per second. + * @param[in] bit_width : Resolution for gyro. + * + * @return Degree per second. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to define limit to print accel data. */ + uint8_t limit = 10; + + /* Assign accel and gyro sensor to variable. */ + uint8_t sensor_list[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Create an instance of sensor data structure. */ + struct bmi2_sensor_data sensor_data[2] = { { 0 } }; + + /* Initialize the interrupt status of accel and gyro. */ + uint16_t int_status = 0; + + uint8_t indx = 1; + + float x = 0, y = 0, z = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_wh_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the accel and gyro sensor. */ + rslt = bmi270_wh_sensor_enable(sensor_list, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Accel and gyro configuration settings. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Assign accel and gyro sensor. */ + sensor_data[ACCEL].type = BMI2_ACCEL; + sensor_data[GYRO].type = BMI2_GYRO; + + /* Loop to print accel and gyro data when interrupt occurs. */ + while (indx <= limit) + { + /* To get the data ready interrupt status of accel and gyro. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the data ready interrupt status and print the status for 10 samples. */ + if ((int_status & BMI2_ACC_DRDY_INT_MASK) && (int_status & BMI2_GYR_DRDY_INT_MASK)) + { + /* Get accel and gyro data for x, y and z axis. */ + rslt = bmi270_wh_get_sensor_data(sensor_data, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("\n******* Accel(Raw and m/s2) Gyro(Raw and dps) data : %d *******\n", indx); + + printf("\nAcc_x = %d\t", sensor_data[ACCEL].sens_data.acc.x); + printf("Acc_y = %d\t", sensor_data[ACCEL].sens_data.acc.y); + printf("Acc_z = %d", sensor_data[ACCEL].sens_data.acc.z); + + /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ + x = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.x, 2, bmi2_dev.resolution); + y = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.y, 2, bmi2_dev.resolution); + z = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.z, 2, bmi2_dev.resolution); + + /* Print the data in m/s2. */ + printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); + + printf("\nGyr_X = %d\t", sensor_data[GYRO].sens_data.gyr.x); + printf("Gyr_Y = %d\t", sensor_data[GYRO].sens_data.gyr.y); + printf("Gyr_Z= %d\n", sensor_data[GYRO].sens_data.gyr.z); + + /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ + x = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.x, 2000, bmi2_dev.resolution); + y = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.y, 2000, bmi2_dev.resolution); + z = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.z, 2000, bmi2_dev.resolution); + + /* Print the data in dps. */ + printf("Gyro_DPS_X = %4.2f, Gyro_DPS_Y = %4.2f, Gyro_DPS_Z = %4.2f\n", x, y, z); + + indx++; + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accelerometer and gyro configuration. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[ACCEL].type = BMI2_ACCEL; + config[GYRO].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_wh_get_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Map data ready interrupt to interrupt pin. */ + rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config[ACCEL].cfg.acc.odr = BMI2_ACC_ODR_200HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[ACCEL].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples. + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[ACCEL].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[ACCEL].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config[GYRO].cfg.gyr.odr = BMI2_GYR_ODR_200HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[GYRO].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[GYRO].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[GYRO].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[GYRO].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set the accel and gyro configurations. */ + rslt = bmi270_wh_set_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (GRAVITY_EARTH * val * g_range) / half_scale; +} + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/any_motion_interrupt/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/any_motion_interrupt/Makefile new file mode 100644 index 000000000..1598c62cb --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/any_motion_interrupt/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= any_motion_interrupt.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_wh.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/any_motion_interrupt/any_motion_interrupt.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/any_motion_interrupt/any_motion_interrupt.c new file mode 100644 index 000000000..d1953e322 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/any_motion_interrupt/any_motion_interrupt.c @@ -0,0 +1,134 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_wh.h" +#include "common.h" + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for any-motion. + * + * @param[in] bmi2_dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_feature_config(struct bmi2_dev *bmi2_dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Accel sensor and any-motion feature are listed in array. */ + uint8_t sens_list[2] = { BMI2_ACCEL, BMI2_ANY_MOTION }; + + /* Variable to get any-motion interrupt status. */ + uint16_t int_status = 0; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Select features and their pins to be mapped to. */ + struct bmi2_sens_int_config sens_int = { .type = BMI2_ANY_MOTION, .hw_int_pin = BMI2_INT1 }; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_wh. */ + rslt = bmi270_wh_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the selected sensors. */ + rslt = bmi270_wh_sensor_enable(sens_list, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Set feature configurations for any-motion. */ + rslt = set_feature_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Map the feature interrupt for any-motion. */ + rslt = bmi270_wh_map_feat_int(&sens_int, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + printf("Move the board\n"); + + /* Loop to get any-motion interrupt. */ + do + { + /* Clear buffer. */ + int_status = 0; + + /* To get the interrupt status of any-motion. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the interrupt status of any-motion. */ + if (int_status & BMI270_WH_ANY_MOT_STATUS_MASK) + { + printf("Any-motion interrupt is generated\n"); + break; + } + } while (rslt == BMI2_OK); + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for any-motion. + */ +static int8_t set_feature_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define the type of sensor and its configurations. */ + struct bmi2_sens_config config; + + /* Configure the type of feature. */ + config.type = BMI2_ANY_MOTION; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_wh_get_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* 1LSB equals 20ms. Default is 100ms, setting to 80ms. */ + config.cfg.any_motion.duration = 0x04; + + /* 1LSB equals to 0.48mg. Default is 83mg, setting to 50mg. */ + config.cfg.any_motion.threshold = 0x68; + + /* Set new configurations. */ + rslt = bmi270_wh_set_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/common/common.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/common/common.c new file mode 100644 index 000000000..afff37bc8 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/common/common.c @@ -0,0 +1,372 @@ +/** + * Copyright (C) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +#include +#include +#include + +#include "coines.h" +#include "bmi2_defs.h" + +/******************************************************************************/ +/*! Macro definitions */ +#define BMI2XY_SHUTTLE_ID UINT16_C(0x1B8) + +/*! Macro that defines read write length */ +#define READ_WRITE_LEN UINT8_C(46) + +/******************************************************************************/ +/*! Static variable definition */ + +/*! Variable that holds the I2C device address or SPI chip selection */ +static uint8_t dev_addr; + +/******************************************************************************/ +/*! User interface functions */ + +/*! + * I2C read function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_read_i2c(dev_addr, reg_addr, reg_data, (uint16_t)len); +} + +/*! + * I2C write function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_write_i2c(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len); +} + +/*! + * SPI read function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_read_spi(dev_addr, reg_addr, reg_data, (uint16_t)len); +} + +/*! + * SPI write function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_write_spi(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len); +} + +/*! + * Delay function map to COINES platform + */ +void bmi2_delay_us(uint32_t period, void *intf_ptr) +{ + coines_delay_usec(period); +} + +/*! + * @brief Function to select the interface between SPI and I2C. + * Also to initialize coines platform + */ +int8_t bmi2_interface_init(struct bmi2_dev *bmi, uint8_t intf) +{ + int8_t rslt = BMI2_OK; + struct coines_board_info board_info; + + if (bmi != NULL) + { + int16_t result = coines_open_comm_intf(COINES_COMM_INTF_USB); + if (result < COINES_SUCCESS) + { + printf( + "\n Unable to connect with Application Board ! \n" " 1. Check if the board is connected and powered on. \n" " 2. Check if Application Board USB driver is installed. \n" + " 3. Check if board is in use by another application. (Insufficient permissions to access USB) \n"); + exit(result); + } + + result = coines_get_board_info(&board_info); + +#if defined(PC) + setbuf(stdout, NULL); +#endif + + if (result == COINES_SUCCESS) + { + if ((board_info.shuttle_id != BMI2XY_SHUTTLE_ID)) + { + printf("! Warning invalid sensor shuttle \n ," "This application will not support this sensor \n"); + exit(COINES_E_FAILURE); + } + } + + coines_set_shuttleboard_vdd_vddio_config(0, 0); + coines_delay_msec(100); + + /* Bus configuration : I2C */ + if (intf == BMI2_I2C_INTF) + { + printf("I2C Interface \n"); + + /* To initialize the user I2C function */ + dev_addr = BMI2_I2C_PRIM_ADDR; + bmi->intf = BMI2_I2C_INTF; + bmi->read = bmi2_i2c_read; + bmi->write = bmi2_i2c_write; + coines_config_i2c_bus(COINES_I2C_BUS_0, COINES_I2C_FAST_MODE); + } + /* Bus configuration : SPI */ + else if (intf == BMI2_SPI_INTF) + { + printf("SPI Interface \n"); + + /* To initialize the user SPI function */ + dev_addr = COINES_SHUTTLE_PIN_7; + bmi->intf = BMI2_SPI_INTF; + bmi->read = bmi2_spi_read; + bmi->write = bmi2_spi_write; + coines_config_spi_bus(COINES_SPI_BUS_0, COINES_SPI_SPEED_5_MHZ, COINES_SPI_MODE3); + coines_set_pin_config(COINES_SHUTTLE_PIN_7, COINES_PIN_DIRECTION_OUT, COINES_PIN_VALUE_HIGH); + } + + /* Assign device address to interface pointer */ + bmi->intf_ptr = &dev_addr; + + /* Configure delay in microseconds */ + bmi->delay_us = bmi2_delay_us; + + /* Configure max read/write length (in bytes) ( Supported length depends on target machine) */ + bmi->read_write_len = READ_WRITE_LEN; + + /* Assign to NULL to load the default config file. */ + bmi->config_file_ptr = NULL; + + coines_delay_usec(10000); + + coines_set_shuttleboard_vdd_vddio_config(3300, 3300); + + coines_delay_usec(10000); + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; + +} + +/*! + * @brief Prints the execution status of the APIs. + */ +void bmi2_error_codes_print_result(int8_t rslt) +{ + switch (rslt) + { + case BMI2_OK: + + /* Do nothing */ + break; + + case BMI2_W_FIFO_EMPTY: + printf("Warning [%d] : FIFO empty\r\n", rslt); + break; + case BMI2_W_PARTIAL_READ: + printf("Warning [%d] : FIFO partial read\r\n", rslt); + break; + case BMI2_E_NULL_PTR: + printf( + "Error [%d] : Null pointer error. It occurs when the user tries to assign value (not address) to a pointer," " which has been initialized to NULL.\r\n", + rslt); + break; + + case BMI2_E_COM_FAIL: + printf( + "Error [%d] : Communication failure error. It occurs due to read/write operation failure and also due " "to power failure during communication\r\n", + rslt); + break; + + case BMI2_E_DEV_NOT_FOUND: + printf("Error [%d] : Device not found error. It occurs when the device chip id is incorrectly read\r\n", + rslt); + break; + + case BMI2_E_INVALID_SENSOR: + printf( + "Error [%d] : Invalid sensor error. It occurs when there is a mismatch in the requested feature with the " "available one\r\n", + rslt); + break; + + case BMI2_E_SELF_TEST_FAIL: + printf( + "Error [%d] : Self-test failed error. It occurs when the validation of accel self-test data is " "not satisfied\r\n", + rslt); + break; + + case BMI2_E_INVALID_INT_PIN: + printf( + "Error [%d] : Invalid interrupt pin error. It occurs when the user tries to configure interrupt pins " "apart from INT1 and INT2\r\n", + rslt); + break; + + case BMI2_E_OUT_OF_RANGE: + printf( + "Error [%d] : Out of range error. It occurs when the data exceeds from filtered or unfiltered data from " "fifo and also when the range exceeds the maximum range for accel and gyro while performing FOC\r\n", + rslt); + break; + + case BMI2_E_ACC_INVALID_CFG: + printf( + "Error [%d] : Invalid Accel configuration error. It occurs when there is an error in accel configuration" " register which could be one among range, BW or filter performance in reg address 0x40\r\n", + rslt); + break; + + case BMI2_E_GYRO_INVALID_CFG: + printf( + "Error [%d] : Invalid Gyro configuration error. It occurs when there is a error in gyro configuration" "register which could be one among range, BW or filter performance in reg address 0x42\r\n", + rslt); + break; + + case BMI2_E_ACC_GYR_INVALID_CFG: + printf( + "Error [%d] : Invalid Accel-Gyro configuration error. It occurs when there is a error in accel and gyro" " configuration registers which could be one among range, BW or filter performance in reg address 0x40 " "and 0x42\r\n", + rslt); + break; + + case BMI2_E_CONFIG_LOAD: + printf( + "Error [%d] : Configuration load error. It occurs when failure observed while loading the configuration " "into the sensor\r\n", + rslt); + break; + + case BMI2_E_INVALID_PAGE: + printf( + "Error [%d] : Invalid page error. It occurs due to failure in writing the correct feature configuration " "from selected page\r\n", + rslt); + break; + + case BMI2_E_SET_APS_FAIL: + printf( + "Error [%d] : APS failure error. It occurs due to failure in write of advance power mode configuration " "register\r\n", + rslt); + break; + + case BMI2_E_AUX_INVALID_CFG: + printf( + "Error [%d] : Invalid AUX configuration error. It occurs when the auxiliary interface settings are not " "enabled properly\r\n", + rslt); + break; + + case BMI2_E_AUX_BUSY: + printf( + "Error [%d] : AUX busy error. It occurs when the auxiliary interface buses are engaged while configuring" " the AUX\r\n", + rslt); + break; + + case BMI2_E_REMAP_ERROR: + printf( + "Error [%d] : Remap error. It occurs due to failure in assigning the remap axes data for all the axes " "after change in axis position\r\n", + rslt); + break; + + case BMI2_E_GYR_USER_GAIN_UPD_FAIL: + printf( + "Error [%d] : Gyro user gain update fail error. It occurs when the reading of user gain update status " "fails\r\n", + rslt); + break; + + case BMI2_E_SELF_TEST_NOT_DONE: + printf( + "Error [%d] : Self-test not done error. It occurs when the self-test process is ongoing or not " "completed\r\n", + rslt); + break; + + case BMI2_E_INVALID_INPUT: + printf("Error [%d] : Invalid input error. It occurs when the sensor input validity fails\r\n", rslt); + break; + + case BMI2_E_INVALID_STATUS: + printf("Error [%d] : Invalid status error. It occurs when the feature/sensor validity fails\r\n", rslt); + break; + + case BMI2_E_CRT_ERROR: + printf("Error [%d] : CRT error. It occurs when the CRT test has failed\r\n", rslt); + break; + + case BMI2_E_ST_ALREADY_RUNNING: + printf( + "Error [%d] : Self-test already running error. It occurs when the self-test is already running and " "another has been initiated\r\n", + rslt); + break; + + case BMI2_E_CRT_READY_FOR_DL_FAIL_ABORT: + printf( + "Error [%d] : CRT ready for download fail abort error. It occurs when download in CRT fails due to wrong " "address location\r\n", + rslt); + break; + + case BMI2_E_DL_ERROR: + printf( + "Error [%d] : Download error. It occurs when write length exceeds that of the maximum burst length\r\n", + rslt); + break; + + case BMI2_E_PRECON_ERROR: + printf( + "Error [%d] : Pre-conditional error. It occurs when precondition to start the feature was not " "completed\r\n", + rslt); + break; + + case BMI2_E_ABORT_ERROR: + printf("Error [%d] : Abort error. It occurs when the device was shaken during CRT test\r\n", rslt); + break; + + case BMI2_E_WRITE_CYCLE_ONGOING: + printf( + "Error [%d] : Write cycle ongoing error. It occurs when the write cycle is already running and another " "has been initiated\r\n", + rslt); + break; + + case BMI2_E_ST_NOT_RUNING: + printf( + "Error [%d] : Self-test is not running error. It occurs when self-test running is disabled while it's " "running\r\n", + rslt); + break; + + case BMI2_E_DATA_RDY_INT_FAILED: + printf( + "Error [%d] : Data ready interrupt error. It occurs when the sample count exceeds the FOC sample limit " "and data ready status is not updated\r\n", + rslt); + break; + + case BMI2_E_INVALID_FOC_POSITION: + printf( + "Error [%d] : Invalid FOC position error. It occurs when average FOC data is obtained for the wrong" " axes\r\n", + rslt); + break; + + default: + printf("Error [%d] : Unknown error code\r\n", rslt); + break; + } +} + +/*! + * @brief Deinitializes coines platform + * + * @return void. + */ +void bmi2_coines_deinit(void) +{ + coines_close_comm_intf(COINES_COMM_INTF_USB); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/common/common.h b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/common/common.h new file mode 100644 index 000000000..763983da4 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/common/common.h @@ -0,0 +1,122 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +#ifndef _COMMON_H +#define _COMMON_H + +/*! CPP guard */ +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include "bmi2.h" + +/*! + * @brief Function for reading the sensor's registers through I2C bus. + * + * @param[in] reg_addr : Register address. + * @param[out] reg_data : Pointer to the data buffer to store the read data. + * @param[in] length : No of bytes to read. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief Function for writing the sensor's registers through I2C bus. + * + * @param[in] reg_addr : Register address. + * @param[in] reg_data : Pointer to the data buffer whose value is to be written. + * @param[in] length : No of bytes to write. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief Function for reading the sensor's registers through SPI bus. + * + * @param[in] reg_addr : Register address. + * @param[out] reg_data : Pointer to the data buffer to store the read data. + * @param[in] length : No of bytes to read. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief Function for writing the sensor's registers through SPI bus. + * + * @param[in] reg_addr : Register address. + * @param[in] reg_data : Pointer to the data buffer whose data has to be written. + * @param[in] length : No of bytes to write. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief This function provides the delay for required time (Microsecond) as per the input provided in some of the + * APIs. + * + * @param[in] period_us : The required wait time in microsecond. + * @param[in] intf_ptr : Interface pointer + * + * @return void. + * + */ +void bmi2_delay_us(uint32_t period, void *intf_ptr); + +/*! + * @brief Function to select the interface between SPI and I2C. + * + * @param[in] bma : Structure instance of bmi2_dev + * @param[in] intf : Interface selection parameter + * + * @return Status of execution + * @retval 0 -> Success + * @retval < 0 -> Failure Info + */ +int8_t bmi2_interface_init(struct bmi2_dev *bma, uint8_t intf); + +/*! + * @brief Prints the execution status of the APIs. + * + * @param[in] rslt : Error code returned by the API whose execution status has to be printed. + * + * @return void. + */ +void bmi2_error_codes_print_result(int8_t rslt); + +/*! + * @brief Deinitializes coines platform + * + * @return void. + */ +void bmi2_coines_deinit(void); + +#ifdef __cplusplus +} +#endif /* End of CPP guard */ + +#endif /* _COMMON_H */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_header_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_header_mode/Makefile new file mode 100644 index 000000000..bde7c1962 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_header_mode/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= fifo_full_header_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_wh.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_header_mode/fifo_full_header_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_header_mode/fifo_full_header_mode.c new file mode 100644 index 000000000..ab331eaee --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_header_mode/fifo_full_header_mode.c @@ -0,0 +1,316 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_wh.h" +#include "common.h" + +/******************************************************************************/ +/*! Macros */ + +/*! Buffer size allocated to store raw FIFO data. */ +#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) + +/*! Length of data to be read from FIFO. */ +#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) + +/*! Number of accel frames to be extracted from FIFO. */ + +/*! Calculation for frame count: Total frame count = Fifo buffer size(2048)/ Total frames(6 Accel, 6 Gyro and 1 header, + * totaling to 13) which equals to 157. + */ +#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(157) + +/*! Number of gyro frames to be extracted from FIFO. */ +#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(157) + +/*! Macro to read sensortime byte in FIFO. */ +#define SENSORTIME_OVERHEAD_BYTE UINT8_C(3) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + * @param[in] dev : Structure instance of bmi2_dev. + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev); + +/******************************************************************************/ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + uint16_t index = 0; + uint16_t fifo_length = 0; + uint16_t config = 0; + + /* To read sensortime, extra 3 bytes are added to fifo buffer. */ + uint16_t fifo_buffer_size = BMI2_FIFO_RAW_DATA_BUFFER_SIZE + SENSORTIME_OVERHEAD_BYTE; + + /* Variable to get fifo full interrupt status. */ + uint16_t int_status = 0; + + uint16_t accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + + uint16_t gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + /* Variable to store the available frame length count in FIFO */ + uint8_t frame_count; + + int8_t try = 1; + + /* Number of bytes of FIFO data. */ + uint8_t fifo_data[fifo_buffer_size]; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Array of accelerometer frames -> Total bytes = + * 157 * (6 axes + 1 header bytes) = 1099 bytes */ + struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; + + /* Array of gyro frames -> Total bytes = + * 157 * (6 axes + 1 header bytes) = 1099 bytes */ + struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; + + /* Initialize FIFO frame structure. */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Accel and gyro sensor are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_wh. */ + rslt = bmi270_wh_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable accel and gyro sensor. */ + rslt = bmi270_wh_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configuration settings for accel and gyro. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Before setting FIFO, disable the advance power save mode. */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_get_fifo_config(&config, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Update FIFO structure. */ + /* Mapping the buffer to store the fifo data. */ + fifoframe.data = fifo_data; + + /* Length of FIFO frame. */ + /* To read sensortime, extra 3 bytes are added to fifo user length. */ + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH + SENSORTIME_OVERHEAD_BYTE; + + /* Set FIFO configuration by enabling accel, gyro and timestamp. + * NOTE 1: The header mode is enabled by default. + * NOTE 2: By default the FIFO operating mode is in FIFO mode. + * NOTE 3: Sensortime is enabled by default */ + printf("FIFO is configured in header mode\n"); + rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Map FIFO full interrupt. */ + fifoframe.data_int_map = BMI2_FFULL_INT; + rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + while (try <= 10) + { + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if ((rslt == BMI2_OK) && (int_status & BMI2_FFULL_INT_STATUS_MASK)) + { + printf("\nIteration : %d\n", try); + + accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + + gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("\nFIFO data bytes available : %d \n", fifo_length); + printf("\nFIFO data bytes requested : %d \n", fifoframe.length); + + /* Read FIFO data. */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nFIFO accel frames requested : %d \n", accel_frame_length); + + /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ + rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); + + printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); + + /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ + rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); + + /* Calculating the frame count from the available bytes in FIFO + * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len + header_byte)) */ + frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH + 1)); + printf("\nAvailable frame count from available bytes: %d\n", frame_count); + + printf("\nExtracted accel frames\n"); + + if (accel_frame_length > frame_count) + { + accel_frame_length = frame_count; + } + + /* Print the parsed accelerometer data from the FIFO buffer. */ + for (index = 0; index < accel_frame_length; index++) + { + printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, + fifo_accel_data[index].y, fifo_accel_data[index].z); + } + + printf("\nExtracted gyro frames\n"); + + if (gyro_frame_length > frame_count) + { + gyro_frame_length = frame_count; + } + + /* Print the parsed gyro data from the FIFO buffer. */ + for (index = 0; index < gyro_frame_length; index++) + { + printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", + index, + fifo_gyro_data[index].x, + fifo_gyro_data[index].y, + fifo_gyro_data[index].z); + } + + /* Print control frames like sensor time and skipped frame count. */ + printf("\nSkipped frame count = %d\n", fifoframe.skipped_frame_count); + + printf("Sensor time = %lu\r\n", fifoframe.sensor_time); + + try++; + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accel and gyro configurations. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[0].type = BMI2_ACCEL; + config[1].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_wh_get_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Accel configuration settings. */ + /* Set Output Data Rate */ + config[0].cfg.acc.odr = BMI2_ACC_ODR_25HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Gyro configuration settings. */ + /* Set Output Data Rate */ + config[1].cfg.gyr.odr = BMI2_GYR_ODR_25HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set new configurations. */ + rslt = bmi270_wh_set_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_headerless_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_headerless_mode/Makefile new file mode 100644 index 000000000..5c407e909 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_headerless_mode/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= fifo_full_headerless_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_wh.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_headerless_mode/fifo_full_headerless_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_headerless_mode/fifo_full_headerless_mode.c new file mode 100644 index 000000000..14b0b22b3 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_headerless_mode/fifo_full_headerless_mode.c @@ -0,0 +1,304 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_wh.h" +#include "common.h" + +/******************************************************************************/ +/*! Macros */ + +/*! Buffer size allocated to store raw FIFO data */ +#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) + +/*! Length of data to be read from FIFO */ +#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) + +/*! Number of accel frames to be extracted from FIFO */ + +/*! Calculation for frame count: Total frame count = Fifo buffer size(2048)/ Total frames(6 Accel, 6 Gyro totaling to + * 12) which equals to 170. + */ +#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(169) + +/*! Number of gyro frames to be extracted from FIFO */ +#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(169) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + * @param[in] dev : Structure instance of bmi2_dev. + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev); + +/******************************************************************************/ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + uint16_t index = 0; + + uint16_t fifo_length = 0; + + uint8_t try = 1; + + /* Variable to get fifo full interrupt status. */ + uint16_t int_status = 0; + + uint16_t accel_frame_length; + + uint16_t gyro_frame_length; + + /* Variable to store the available frame length count in FIFO */ + uint8_t frame_count; + + /* Number of bytes of FIFO data. */ + uint8_t fifo_data[BMI2_FIFO_RAW_DATA_BUFFER_SIZE] = { 0 }; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Array of accelerometer frames -> Total bytes = + * 170 * (6 axes bytes) = 1020 bytes */ + struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; + + /* Array of gyro frames -> Total bytes = + * 170 * (6 axes bytes) = 1020 bytes */ + struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; + + /* Initialize FIFO frame structure. */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Accel and gyro sensor are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_wh. */ + rslt = bmi270_wh_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable accel and gyro sensor. */ + rslt = bmi270_wh_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configuration settings for accel and gyro. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Before setting FIFO, disable the advance power save mode. */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Update FIFO structure. */ + /* Mapping the buffer to store the fifo data. */ + fifoframe.data = fifo_data; + + /* Length of FIFO frame. */ + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; + + /* Set FIFO configuration by enabling accel, gyro. + * NOTE 1: The header mode is enabled by default. + * NOTE 2: By default the FIFO operating mode is FIFO mode. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("FIFO is configured in headerless mode\n"); + + /* To enable headerless mode, disable the header. */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_fifo_config(BMI2_FIFO_HEADER_EN, BMI2_DISABLE, &bmi2_dev); + } + + /* Map FIFO full interrupt. */ + fifoframe.data_int_map = BMI2_FFULL_INT; + rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + while (try <= 10) + { + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if ((rslt == BMI2_OK) && (int_status & BMI2_FFULL_INT_STATUS_MASK)) + { + printf("\nIteration : %d\n", try); + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + printf("\nFIFO data bytes available : %d \n", fifo_length); + printf("\nFIFO data bytes requested : %d \n", fifoframe.length); + + /* Read FIFO data. */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nFIFO accel frames requested : %d \n", accel_frame_length); + + /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ + rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); + + printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); + + /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ + rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); + + /* Calculating the frame count from the available bytes in FIFO + * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len)) */ + frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH)); + printf("\nAvailable frame count from available bytes: %d\n", frame_count); + + printf("\nExtracted accel frames\n"); + + if (accel_frame_length > frame_count) + { + accel_frame_length = frame_count; + } + + /* Print the parsed accelerometer data from the FIFO buffer. */ + for (index = 0; index < accel_frame_length; index++) + { + printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, + fifo_accel_data[index].y, fifo_accel_data[index].z); + } + + printf("\nExtracted gyro frames\n"); + + if (gyro_frame_length > frame_count) + { + gyro_frame_length = frame_count; + } + + /* Print the parsed gyro data from the FIFO buffer. */ + for (index = 0; index < gyro_frame_length; index++) + { + printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", + index, + fifo_gyro_data[index].x, + fifo_gyro_data[index].y, + fifo_gyro_data[index].z); + } + } + + try++; + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accel and gyro configurations. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[0].type = BMI2_ACCEL; + config[1].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_wh_get_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Accel configuration settings. */ + /* Set Output Data Rate */ + config[0].cfg.acc.odr = BMI2_ACC_ODR_100HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Gyro configuration settings. */ + /* Set Output Data Rate */ + config[1].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set new configurations. */ + rslt = bmi270_wh_set_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_header_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_header_mode/Makefile new file mode 100644 index 000000000..e0a42e369 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_header_mode/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= fifo_watermark_header_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_wh.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_header_mode/fifo_watermark_header_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_header_mode/fifo_watermark_header_mode.c new file mode 100644 index 000000000..f33bb5ebf --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_header_mode/fifo_watermark_header_mode.c @@ -0,0 +1,335 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_wh.h" +#include "common.h" + +/******************************************************************************/ +/*! Macros */ + +/*! Buffer size allocated to store raw FIFO data */ +#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) + +/*! Length of data to be read from FIFO */ +#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) + +/*! Number of accel frames to be extracted from FIFO */ + +/*! + * Calculation: + * fifo_watermark_level = 650, accel_frame_len = 6, gyro_frame_len = 6 header_byte = 1. + * fifo_accel_frame_count = (650 / (6 + 6 + 1 )) = 50 frames + * NOTE: Extra frames are read in order to get sensor time + */ +#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(55) + +/*! Number of gyro frames to be extracted from FIFO */ +#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(55) + +/*! Setting a watermark level in FIFO */ +#define BMI270_FIFO_WATERMARK_LEVEL UINT16_C(650) + +/*! Macro to read sensortime byte in FIFO */ +#define SENSORTIME_OVERHEAD_BYTE UINT8_C(3) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + * @param[in] dev : Structure instance of bmi2_dev. + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to store the available frame length count in FIFO */ + uint8_t frame_count; + + /* Variable to index bytes. */ + uint16_t index = 0; + + uint16_t fifo_length = 0; + + uint16_t accel_frame_length; + + uint16_t gyro_frame_length; + + uint8_t try = 1; + + /* To read sensortime, extra 3 bytes are added to fifo buffer */ + uint16_t fifo_buffer_size = BMI2_FIFO_RAW_DATA_BUFFER_SIZE + SENSORTIME_OVERHEAD_BYTE; + + /* Number of bytes of FIFO data. */ + uint8_t fifo_data[fifo_buffer_size]; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Array of accelerometer frames -> Total bytes = + * 55 * (6 axes bytes) = 330 bytes */ + struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; + + /* Array of gyro frames -> Total bytes = + * 55 * (6 axes bytes) = 330 bytes */ + struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; + + /* Initialize FIFO frame structure. */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Accel and gyro sensor are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Variable to get fifo water-mark interrupt status. */ + uint16_t int_status = 0; + + uint16_t watermark = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_wh. */ + rslt = bmi270_wh_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable accel and gyro sensor. */ + rslt = bmi270_wh_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configuration settings for accel and gyro. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Before setting FIFO, disable the advance power save mode. */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Update FIFO structure. */ + /* Mapping the buffer to store the fifo data. */ + fifoframe.data = fifo_data; + + /* Length of FIFO frame. */ + /* To read sensortime, extra 3 bytes are added to fifo user length. */ + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH + SENSORTIME_OVERHEAD_BYTE; + + /* Set FIFO configuration by enabling accel, gyro and timestamp. + * NOTE 1: The header mode is enabled by default. + * NOTE 2: By default the FIFO operating mode is FIFO mode. + * NOTE 3: Sensortime is enabled by default */ + printf("FIFO is configured in header mode\n"); + rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* FIFO water-mark interrupt is enabled. */ + fifoframe.data_int_map = BMI2_FWM_INT; + + /* Map water-mark interrupt to the required interrupt pin. */ + rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Set water-mark level. */ + fifoframe.wm_lvl = BMI270_FIFO_WATERMARK_LEVEL; + + /* Set the water-mark level if water-mark interrupt is mapped. */ + rslt = bmi2_set_fifo_wm(fifoframe.wm_lvl, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + rslt = bmi2_get_fifo_wm(&watermark, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + printf("\nFIFO watermark level is %d\n", watermark); + + while (try <= 10) + { + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the status of FIFO watermark interrupt. */ + if ((rslt == BMI2_OK) && (int_status & BMI2_FWM_INT_STATUS_MASK)) + { + printf("\nIteration : %d\n", try); + + printf("\nWatermark interrupt occurred\n"); + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + printf("\nFIFO data bytes available : %d \n", fifo_length); + printf("\nFIFO data bytes requested : %d \n", fifoframe.length); + + /* Read FIFO data. */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nFIFO accel frames requested : %d \n", accel_frame_length); + + /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ + rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); + + printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); + + /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ + rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); + + /* Calculating the frame count from the available bytes in FIFO + * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len + header_byte)) */ + frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH + 1)); + printf("\nAvailable frame count from available bytes: %d\n", frame_count); + + printf("\nExracted accel frames\n"); + + if (accel_frame_length > frame_count) + { + accel_frame_length = frame_count; + } + + /* Print the parsed accelerometer data from the FIFO buffer. */ + for (index = 0; index < accel_frame_length; index++) + { + printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, + fifo_accel_data[index].y, fifo_accel_data[index].z); + } + + printf("\nExtracted gyro frames\n"); + + if (gyro_frame_length > frame_count) + { + gyro_frame_length = frame_count; + } + + /* Print the parsed gyro data from the FIFO buffer. */ + for (index = 0; index < gyro_frame_length; index++) + { + printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", + index, + fifo_gyro_data[index].x, + fifo_gyro_data[index].y, + fifo_gyro_data[index].z); + } + + /* Print control frames like sensor time and skipped frame count. */ + printf("Skipped frame count = %d\n", fifoframe.skipped_frame_count); + printf("Sensor time = %lu\r\n", fifoframe.sensor_time); + + try++; + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accel and gyro configurations. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[0].type = BMI2_ACCEL; + config[1].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_wh_get_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Accel configuration settings. */ + /* Set Output Data Rate */ + config[0].cfg.acc.odr = BMI2_ACC_ODR_25HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Gyro configuration settings. */ + /* Set Output Data Rate */ + config[1].cfg.gyr.odr = BMI2_GYR_ODR_25HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set new configurations. */ + rslt = bmi270_wh_set_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_headerless_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_headerless_mode/Makefile new file mode 100644 index 000000000..ce97737ea --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_headerless_mode/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= fifo_watermark_headerless_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_wh.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c new file mode 100644 index 000000000..160c01de5 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c @@ -0,0 +1,331 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_wh.h" +#include "common.h" + +/******************************************************************************/ +/*! Macros */ + +/*! Buffer size allocated to store raw FIFO data */ +#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) + +/*! Length of data to be read from FIFO */ +#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) + +/*! Number of accel frames to be extracted from FIFO */ + +/*! + * Calculation: + * fifo_watermark_level = 650, accel_frame_len = 6, gyro_frame_len = 6. + * fifo_accel_frame_count = (650 / (6 + 6 )) = 54 frames + */ +#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(55) + +/*! Number of gyro frames to be extracted from FIFO */ +#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(55) + +/*! Setting a watermark level in FIFO */ +#define BMI270_FIFO_WATERMARK_LEVEL UINT16_C(650) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + * @param[in] dev : Structure instance of bmi2_dev. + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to index bytes. */ + uint16_t index = 0; + + /* Variable to store the available frame length count in FIFO */ + uint8_t frame_count; + + uint16_t fifo_length = 0; + + uint16_t accel_frame_length; + + uint16_t gyro_frame_length; + + uint8_t try = 1; + + /* Number of bytes of FIFO data. */ + uint8_t fifo_data[BMI2_FIFO_RAW_DATA_BUFFER_SIZE] = { 0 }; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Array of accelerometer frames -> Total bytes = + * 50 * (6 axes bytes) = 300 bytes */ + struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; + + /* Array of gyro frames -> Total bytes = + * 50 * (6 axes bytes) = 300 bytes */ + struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; + + /* Initialize FIFO frame structure. */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Accel and gyro sensor are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Variable to get fifo water-mark interrupt status. */ + uint16_t int_status = 0; + + uint16_t watermark = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_wh. */ + rslt = bmi270_wh_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable accel and gyro sensor. */ + rslt = bmi270_wh_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configuration settings for accel and gyro. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Before setting FIFO, disable the advance power save mode. */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Update FIFO structure. */ + /* Mapping the buffer to store the fifo data. */ + fifoframe.data = fifo_data; + + /* Length of FIFO frame. */ + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; + + /* Set FIFO configuration by enabling accel, gyro. + * NOTE 1: The header mode is enabled by default. + * NOTE 2: By default the FIFO operating mode is FIFO mode. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("FIFO is configured in headerless mode\n"); + + /* To enable headerless mode, disable the header. */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_fifo_config(BMI2_FIFO_HEADER_EN, BMI2_DISABLE, &bmi2_dev); + } + + /* FIFO water-mark interrupt is enabled. */ + fifoframe.data_int_map = BMI2_FWM_INT; + + /* Map water-mark interrupt to the required interrupt pin. */ + rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Set water-mark level. */ + fifoframe.wm_lvl = BMI270_FIFO_WATERMARK_LEVEL; + + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; + + /* Set the water-mark level if water-mark interrupt is mapped. */ + rslt = bmi2_set_fifo_wm(fifoframe.wm_lvl, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + rslt = bmi2_get_fifo_wm(&watermark, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + printf("\nFIFO watermark level is %d\n", watermark); + + while (try <= 10) + { + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the status of FIFO watermark interrupt. */ + if ((rslt == BMI2_OK) && (int_status & BMI2_FWM_INT_STATUS_MASK)) + { + printf("\nIteration : %d\n", try); + + printf("\nWatermark interrupt occurred\n"); + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + printf("\nFIFO data bytes available : %d \n", fifo_length); + printf("\nFIFO data bytes requested : %d \n", fifoframe.length); + + /* Read FIFO data. */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nFIFO accel frames requested : %d \n", accel_frame_length); + + /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ + rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); + + printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); + + /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ + rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); + + /* Calculating the frame count from the available bytes in FIFO + * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len)) */ + frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH)); + printf("\nAvailable frame count from available bytes: %d\n", frame_count); + + printf("\nExracted accel frames\n"); + + if (accel_frame_length > frame_count) + { + accel_frame_length = frame_count; + } + + /* Print the parsed accelerometer data from the FIFO buffer. */ + for (index = 0; index < accel_frame_length; index++) + { + printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, + fifo_accel_data[index].y, fifo_accel_data[index].z); + } + + printf("\nExtracted gyro frames\n"); + + if (gyro_frame_length > frame_count) + { + gyro_frame_length = frame_count; + } + + /* Print the parsed gyro data from the FIFO buffer. */ + for (index = 0; index < gyro_frame_length; index++) + { + printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", + index, + fifo_gyro_data[index].x, + fifo_gyro_data[index].y, + fifo_gyro_data[index].z); + } + } + + try++; + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accel and gyro configurations. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[0].type = BMI2_ACCEL; + config[1].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_wh_get_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameter according to their requirement. */ + /* Accel configuration settings. */ + /* Set Output Data Rate */ + config[0].cfg.acc.odr = BMI2_ACC_ODR_100HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Gyro configuration settings. */ + /* Set Output Data Rate */ + config[1].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set new configurations. */ + rslt = bmi270_wh_set_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/gyro/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/gyro/Makefile new file mode 100644 index 000000000..9192877d5 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/gyro/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= gyro.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_wh.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/gyro/gyro.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/gyro/gyro.c new file mode 100644 index 000000000..d1ce9fe16 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/gyro/gyro.c @@ -0,0 +1,195 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_wh.h" +#include "common.h" + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for gyro. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_gyro_config(struct bmi2_dev *dev); + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + * + * @param[in] val : LSB from each axis. + * @param[in] dps : Degree per second. + * @param[in] bit_width : Resolution for gyro. + * + * @return Degree per second. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to define limit to print gyro data. */ + uint8_t limit = 10; + + uint8_t indx = 0; + + float x = 0, y = 0, z = 0; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Create an instance of sensor data structure. */ + struct bmi2_sensor_data sensor_data = { 0 }; + + /* Assign gyro sensor to variable. */ + uint8_t sens_list = BMI2_GYRO; + + /* Initialize the interrupt status of gyro. */ + uint16_t int_status = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_wh. */ + rslt = bmi270_wh_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the selected sensors. */ + rslt = bmi270_wh_sensor_enable(&sens_list, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Gyro configuration settings. */ + rslt = set_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Select gyro sensor. */ + sensor_data.type = BMI2_GYRO; + printf("Gyro and DPS data\n"); + printf("Gyro data collected at Range 2000 dps with 16-bit resolution\n"); + + /* Loop to print gyro data when interrupt occurs. */ + while (indx <= limit) + { + /* To get the data ready interrupt status of gyro. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the data ready interrupt status and print the status for 10 samples. */ + if (int_status & BMI2_GYR_DRDY_INT_MASK) + { + /* Get gyro data for x, y and z axis. */ + rslt = bmi270_wh_get_sensor_data(&sensor_data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("\nGyr_X = %d\t", sensor_data.sens_data.gyr.x); + printf("Gyr_Y = %d\t", sensor_data.sens_data.gyr.y); + printf("Gyr_Z = %d\r\n", sensor_data.sens_data.gyr.z); + + /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ + x = lsb_to_dps(sensor_data.sens_data.gyr.x, 2000, bmi2_dev.resolution); + y = lsb_to_dps(sensor_data.sens_data.gyr.y, 2000, bmi2_dev.resolution); + z = lsb_to_dps(sensor_data.sens_data.gyr.z, 2000, bmi2_dev.resolution); + + /* Print the data in dps. */ + printf("Gyr_DPS_X = %4.2f, Gyr_DPS_Y = %4.2f, Gyr_DPS_Z = %4.2f\n", x, y, z); + + indx++; + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for gyro. + */ +static int8_t set_gyro_config(struct bmi2_dev *dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define the type of sensor and its configurations. */ + struct bmi2_sens_config config; + + /* Configure the type of feature. */ + config.type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_wh_get_sensor_config(&config, 1, dev); + bmi2_error_codes_print_result(rslt); + + /* Map data ready interrupt to interrupt pin. */ + rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT2, dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config.cfg.gyr.odr = BMI2_GYR_ODR_200HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config.cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config.cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config.cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config.cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set the gyro configurations. */ + rslt = bmi270_wh_set_sensor_config(&config, 1, dev); + } + + return rslt; +} + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/no_motion_interrupt/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/no_motion_interrupt/Makefile new file mode 100644 index 000000000..61b7016bb --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/no_motion_interrupt/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= no_motion_interrupt.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_wh.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/no_motion_interrupt/no_motion_interrupt.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/no_motion_interrupt/no_motion_interrupt.c new file mode 100644 index 000000000..5f584de28 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/no_motion_interrupt/no_motion_interrupt.c @@ -0,0 +1,135 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_wh.h" +#include "common.h" + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for no-motion. + * + * @param[in] bmi2_dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_feature_config(struct bmi2_dev *bmi2_dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Accel sensor and no-motion feature are listed in array. */ + uint8_t sens_list[2] = { BMI2_ACCEL, BMI2_NO_MOTION }; + + /* Variable to get no-motion interrupt status. */ + uint16_t int_status = 0; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Select features and their pins to be mapped to. */ + struct bmi2_sens_int_config sens_int = { .type = BMI2_NO_MOTION, .hw_int_pin = BMI2_INT1 }; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_wh. */ + rslt = bmi270_wh_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the selected sensors. */ + rslt = bmi270_wh_sensor_enable(sens_list, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Set feature configurations for no-motion. */ + rslt = set_feature_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Map the feature interrupt for no-motion. */ + rslt = bmi270_wh_map_feat_int(&sens_int, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + printf("Do not move the board\n"); + + /* Loop to get no-motion interrupt. */ + do + { + /* Clear buffer. */ + int_status = 0; + + /* To get the interrupt status of no-motion. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the interrupt status of no-motion. */ + if (int_status & BMI270_WH_NO_MOT_STATUS_MASK) + { + printf("No-motion interrupt is generated\n"); + break; + } + } while (rslt == BMI2_OK); + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for no-motion. + */ +static int8_t set_feature_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define the type of sensor and its configurations. */ + struct bmi2_sens_config config; + + /* Configure the type of feature. */ + config.type = BMI2_NO_MOTION; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_wh_get_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + if (rslt == BMI2_OK) + { + + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* 1LSB equals 20ms. Default is 100ms, setting to 80ms. */ + config.cfg.no_motion.duration = 0x04; + + /* 1LSB equals to 0.48mg. Default is 83mg, setting to 50mg. */ + config.cfg.no_motion.threshold = 0x68; + + /* Set new configurations. */ + rslt = bmi270_wh_set_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_data_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_data_mode/Makefile new file mode 100644 index 000000000..b995b42dd --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_data_mode/Makefile @@ -0,0 +1,23 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= read_aux_data_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +BMM150_SOURCE ?= ../../../bmm150 + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_wh.c \ +$(BMM150_SOURCE)/bmm150.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(BMM150_SOURCE) \ +$(COMMON_LOCATION)/common + + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_data_mode/read_aux_data_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_data_mode/read_aux_data_mode.c new file mode 100644 index 000000000..6d8cd67f3 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_data_mode/read_aux_data_mode.c @@ -0,0 +1,327 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_wh.h" +#include "bmm150.h" +#include "coines.h" +#include "common.h" + +/******************************************************************************/ +/*! Macro definition */ + +/*! Macros to select the sensors */ +#define ACCEL UINT8_C(0x00) +#define GYRO UINT8_C(0x01) +#define AUX UINT8_C(0x02) + +/*! Earth's gravity in m/s^2 */ +#define GRAVITY_EARTH (9.80665f) + +/*****************************************************************************/ +/*! Structure declaration */ + +/* Sensor initialization configuration. */ +struct bmi2_dev bmi2_dev; + +/*******************************************************************************/ +/*! Functions */ + +/** + * user_aux_read - Reads data from auxiliary sensor in manual mode. + * + * @param[in] reg_addr : Register address. + * @param[out] aux_data : Aux data pointer to store the read data. + * @param[in] length : No of bytes to read. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval 0 -> Success + * @retval < 0 -> Failure Info + */ +static int8_t user_aux_read(uint8_t reg_addr, uint8_t *aux_data, uint32_t len, void *intf_ptr); + +/** + * user_aux_write - Writes data to the auxiliary sensor in manual mode. + * + * @param[in] reg_addr : Register address. + * @param[out] aux_data : Aux data pointer to store the data being written. + * @param[in] length : No of bytes to read. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval 0 -> Success + * @retval < 0 -> Failure Info + */ +static int8_t user_aux_write(uint8_t reg_addr, const uint8_t *aux_data, uint32_t len, void *intf_ptr); + +/*! + * @brief This function provides the delay for required time (Microsecond) as per the input provided in some of the + * APIs. + * + * @param[in] period_us : The required wait time in microsecond. + * @param[in] intf_ptr : Interface pointer + * + * @return void. + */ +static void user_delay_us(uint32_t period, void *intf_ptr); + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + * + * @param[in] val : LSB from each axis. + * @param[in] g_range : Gravity range. + * @param[in] bit_width : Resolution for accel. + * + * @return Gravity. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + * + * @param[in] val : LSB from each axis. + * @param[in] dps : Degree per second. + * @param[in] bit_width : Resolution for gyro. + * + * @return Degree per second. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to select the pull-up resistor which is set to trim register */ + uint8_t regdata; + + /* Variable to define limit to print aux data. */ + uint8_t limit = 20; + + /* Accel, Gyro and Aux sensors are listed in array. */ + uint8_t sensor_list[3] = { BMI2_ACCEL, BMI2_GYRO, BMI2_AUX }; + + /* Structure to define the type of the sensor and its configurations. */ + struct bmi2_sens_config config[3]; + + /* Sensor initialization configuration. */ + struct bmm150_dev bmm150_dev; + + /* bmm150 settings configuration */ + struct bmm150_settings settings; + + /* bmm150 magnetometer data */ + struct bmm150_mag_data mag_data; + + /* Structure to define type of sensor and their respective data. */ + struct bmi2_sensor_data sensor_data[3]; + + /* Variables to define read the accel and gyro data in float */ + float x = 0, y = 0, z = 0; + + config[ACCEL].type = BMI2_ACCEL; + config[GYRO].type = BMI2_GYRO; + config[AUX].type = BMI2_AUX; + + /* To enable the i2c interface settings for bmm150. */ + uint8_t bmm150_dev_addr = BMM150_DEFAULT_I2C_ADDRESS; + bmm150_dev.intf_ptr = &bmm150_dev_addr; + bmm150_dev.read = user_aux_read; + bmm150_dev.write = user_aux_write; + bmm150_dev.delay_us = user_delay_us; + + /* As per datasheet, aux interface with bmi270_wh will support only for I2C */ + bmm150_dev.intf = BMM150_I2C_INTF; + + sensor_data[ACCEL].type = BMI2_ACCEL; + sensor_data[GYRO].type = BMI2_GYRO; + sensor_data[AUX].type = BMI2_AUX; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_wh. */ + rslt = bmi270_wh_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Pull-up resistor 2k is set to the trim regiter */ + regdata = BMI2_ASDA_PUPSEL_2K; + rslt = bmi2_set_regs(BMI2_AUX_IF_TRIM, ®data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable the accel, gyro and aux sensor. */ + rslt = bmi270_wh_sensor_enable(sensor_list, 3, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_wh_get_sensor_config(config, 3, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configurations for accel. */ + config[ACCEL].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + config[ACCEL].cfg.acc.bwp = BMI2_ACC_OSR2_AVG2; + config[ACCEL].cfg.acc.odr = BMI2_ACC_ODR_100HZ; + config[ACCEL].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* Configurations for gyro. */ + config[GYRO].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + config[GYRO].cfg.gyr.noise_perf = BMI2_GYR_RANGE_2000; + config[GYRO].cfg.gyr.bwp = BMI2_GYR_OSR2_MODE; + config[GYRO].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; + config[GYRO].cfg.gyr.range = BMI2_GYR_RANGE_2000; + config[GYRO].cfg.gyr.ois_range = BMI2_GYR_OIS_2000; + + /* Configurations for aux. */ + config[AUX].cfg.aux.odr = BMI2_AUX_ODR_100HZ; + config[AUX].cfg.aux.aux_en = BMI2_ENABLE; + config[AUX].cfg.aux.i2c_device_addr = BMM150_DEFAULT_I2C_ADDRESS; + config[AUX].cfg.aux.manual_en = BMI2_ENABLE; + config[AUX].cfg.aux.fcu_write_en = BMI2_ENABLE; + config[AUX].cfg.aux.man_rd_burst = BMI2_AUX_READ_LEN_3; + config[AUX].cfg.aux.read_addr = BMM150_REG_DATA_X_LSB; + + /* Set new configurations for accel, gyro and aux. */ + rslt = bmi270_wh_set_sensor_config(config, 3, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmm150. */ + rslt = bmm150_init(&bmm150_dev); + bmi2_error_codes_print_result(rslt); + + /* Set the power mode to normal mode. */ + settings.pwr_mode = BMM150_POWERMODE_NORMAL; + rslt = bmm150_set_op_mode(&settings, &bmm150_dev); + bmi2_error_codes_print_result(rslt); + + rslt = bmi270_wh_get_sensor_config(config, 3, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Disable manual mode so that the data mode is enabled. */ + config[AUX].cfg.aux.manual_en = BMI2_DISABLE; + + /* Set the aux configurations. */ + rslt = bmi270_wh_set_sensor_config(config, 3, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("MAGNETOMETER, ACCEL, AND GYRO DATA IN DATA MODE\n"); + + if (bmm150_dev.chip_id == BMM150_CHIP_ID) + { + while (limit--) + { + /* Delay has been added to get aux, accel and gyro data at the interval of every 0.05 second. */ + bmi2_dev.delay_us(50000, bmi2_dev.intf_ptr); + + rslt = bmi270_wh_get_sensor_data(sensor_data, 3, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nAcc_x = %d\t", sensor_data[ACCEL].sens_data.acc.x); + printf("Acc_y = %d\t", sensor_data[ACCEL].sens_data.acc.y); + printf("Acc_z = %d", sensor_data[ACCEL].sens_data.acc.z); + + /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ + x = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.x, 2, bmi2_dev.resolution); + y = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.y, 2, bmi2_dev.resolution); + z = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.z, 2, bmi2_dev.resolution); + + /* Print the data in m/s2. */ + printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); + + printf("\nGyr_X = %d\t", sensor_data[GYRO].sens_data.gyr.x); + printf("Gyr_Y = %d\t", sensor_data[GYRO].sens_data.gyr.y); + printf("Gyr_Z= %d", sensor_data[GYRO].sens_data.gyr.z); + + /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ + x = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.x, 2000, bmi2_dev.resolution); + y = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.y, 2000, bmi2_dev.resolution); + z = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.z, 2000, bmi2_dev.resolution); + + /* Print the data in dps. */ + printf("\nGyro_DPS_X = %4.2f, Gyro_DPS_Y = %4.2f, Gyro_DPS_Z = %4.2f\n", x, y, z); + + /* Compensating the raw auxiliary data available from the BMM150 API. */ + rslt = bmm150_aux_mag_data(sensor_data[AUX].sens_data.aux_data, &mag_data, &bmm150_dev); + bmi2_error_codes_print_result(rslt); + printf("\nMag_x_axis = %d \t Mag_y_axis = %d \t Mag_z_axis = %d \t\n", + mag_data.x, + mag_data.y, + mag_data.z); + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This function reads the data from auxiliary sensor in manual mode. + */ +static int8_t user_aux_read(uint8_t reg_addr, uint8_t *aux_data, uint32_t len, void *intf_ptr) +{ + int8_t rslt; + + /* Discarding the parameter id as it is redundant */ + rslt = bmi2_read_aux_man_mode(reg_addr, aux_data, len, &bmi2_dev); + + return rslt; +} + +/*! + * @brief This function writes the data to auxiliary sensor in manual mode. + */ +static int8_t user_aux_write(uint8_t reg_addr, const uint8_t *aux_data, uint32_t len, void *intf_ptr) +{ + int8_t rslt; + + /* Discarding the parameter id as it is redundant */ + rslt = bmi2_write_aux_man_mode(reg_addr, aux_data, len, &bmi2_dev); + + return rslt; +} + +/*! + * Delay function map to COINES platform + */ +static void user_delay_us(uint32_t period, void *intf_ptr) +{ + coines_delay_usec(period); +} + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (GRAVITY_EARTH * val * g_range) / half_scale; +} + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_manual_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_manual_mode/Makefile new file mode 100644 index 000000000..843a2ce8c --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_manual_mode/Makefile @@ -0,0 +1,23 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= read_aux_manual_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +BMM150_SOURCE ?= ../../../bmm150 + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_wh.c \ +$(BMM150_SOURCE)/bmm150.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(BMM150_SOURCE) \ +$(COMMON_LOCATION)/common + + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_manual_mode/read_aux_manual_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_manual_mode/read_aux_manual_mode.c new file mode 100644 index 000000000..524eb8d2f --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_manual_mode/read_aux_manual_mode.c @@ -0,0 +1,322 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_wh.h" +#include "bmm150.h" +#include "coines.h" +#include "common.h" + +/******************************************************************************/ +/*! Macro definition */ + +/*! Macros to select the sensors */ +#define ACCEL UINT8_C(0x00) +#define GYRO UINT8_C(0x01) +#define AUX UINT8_C(0x02) + +/*! Earth's gravity in m/s^2 */ +#define GRAVITY_EARTH (9.80665f) + +/*****************************************************************************/ +/*! Structure declaration */ + +/* Sensor initialization configuration. */ +struct bmi2_dev bmi2_dev; + +/******************************************************************************/ +/*! Functions */ + +/** + * user_aux_read - Reads data from auxiliary sensor in manual mode. + * + * @param[in] reg_addr : Register address. + * @param[out] aux_data : Aux data pointer to store the read data. + * @param[in] length : No of bytes to read. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval 0 -> Success + * @retval < 0 -> Failure Info + */ +static int8_t user_aux_read(uint8_t reg_addr, uint8_t *aux_data, uint32_t len, void *intf_ptr); + +/** + * user_aux_write - Writes data to the auxiliary sensor in manual mode. + * + * @param[in] reg_addr : Register address. + * @param[out] aux_data : Aux data pointer to store the data being written. + * @param[in] length : No of bytes to read. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval 0 -> Success + * @retval < 0 -> Failure Info + */ +static int8_t user_aux_write(uint8_t reg_addr, const uint8_t *aux_data, uint32_t len, void *intf_ptr); + +/*! + * @brief This function provides the delay for required time (Microsecond) as per the input provided in some of the + * APIs. + * + * @param[in] period_us : The required wait time in microsecond. + * @param[in] intf_ptr : Interface pointer + * + * @return void. + */ +static void user_delay_us(uint32_t period, void *intf_ptr); + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + * + * @param[in] val : LSB from each axis. + * @param[in] g_range : Gravity range. + * @param[in] bit_width : Resolution for accel. + * + * @return Gravity. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + * + * @param[in] val : LSB from each axis. + * @param[in] dps : Degree per second. + * @param[in] bit_width : Resolution for gyro. + * + * @return Degree per second. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to select the pull-up resistor which is set to trim register */ + uint8_t regdata; + + /* Variable to define limit to print aux data. */ + uint8_t limit = 20; + + /* Accel, Gyro and Aux sensors are listed in array. */ + uint8_t sensor_list[3] = { BMI2_ACCEL, BMI2_GYRO, BMI2_AUX }; + + /* Sensor initialization configuration. */ + struct bmm150_dev bmm150_dev; + + /* bmm150 settings configuration */ + struct bmm150_settings settings; + + /* bmm150 magnetometer data */ + struct bmm150_mag_data mag_data; + + /* Structure to define the type of the sensor and its configurations. */ + struct bmi2_sens_config config[3]; + + /* Structure to define type of sensor and their respective data. */ + struct bmi2_sensor_data sensor_data[2]; + + /* Variables to define read the accel and gyro data in float */ + float x = 0, y = 0, z = 0; + + config[ACCEL].type = BMI2_ACCEL; + config[GYRO].type = BMI2_GYRO; + config[AUX].type = BMI2_AUX; + + sensor_data[ACCEL].type = BMI2_ACCEL; + sensor_data[GYRO].type = BMI2_GYRO; + + /* Array of eight bytes to store x, y, z and r axis aux data. */ + uint8_t aux_data[8] = { 0 }; + + /* To enable the i2c interface settings for bmm150. */ + uint8_t bmm150_dev_addr = BMM150_DEFAULT_I2C_ADDRESS; + bmm150_dev.intf_ptr = &bmm150_dev_addr; + bmm150_dev.read = user_aux_read; + bmm150_dev.write = user_aux_write; + bmm150_dev.delay_us = user_delay_us; + + /* As per datasheet, aux interface with bmi270_wh will support only for I2C */ + bmm150_dev.intf = BMM150_I2C_INTF; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_SPI_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_wh. */ + rslt = bmi270_wh_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Pull-up resistor 2k is set to the trim register */ + regdata = BMI2_ASDA_PUPSEL_2K; + rslt = bmi2_set_regs(BMI2_AUX_IF_TRIM, ®data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable the accel, gyro and aux sensor. */ + rslt = bmi270_wh_sensor_enable(sensor_list, 3, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_wh_get_sensor_config(config, 3, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configurations for accel. */ + config[ACCEL].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + config[ACCEL].cfg.acc.bwp = BMI2_ACC_OSR2_AVG2; + config[ACCEL].cfg.acc.odr = BMI2_ACC_ODR_100HZ; + config[ACCEL].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* Configurations for gyro. */ + config[GYRO].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + config[GYRO].cfg.gyr.noise_perf = BMI2_GYR_RANGE_2000; + config[GYRO].cfg.gyr.bwp = BMI2_GYR_OSR2_MODE; + config[GYRO].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; + config[GYRO].cfg.gyr.range = BMI2_GYR_RANGE_2000; + config[GYRO].cfg.gyr.ois_range = BMI2_GYR_OIS_2000; + + /* Configurations for aux. */ + config[AUX].cfg.aux.odr = BMI2_AUX_ODR_100HZ; + config[AUX].cfg.aux.aux_en = BMI2_ENABLE; + config[AUX].cfg.aux.i2c_device_addr = BMM150_DEFAULT_I2C_ADDRESS; + config[AUX].cfg.aux.manual_en = BMI2_ENABLE; + config[AUX].cfg.aux.fcu_write_en = BMI2_ENABLE; + config[AUX].cfg.aux.man_rd_burst = BMI2_AUX_READ_LEN_3; + + /* Set new configurations for accel, gyro and aux. */ + rslt = bmi270_wh_set_sensor_config(config, 3, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmm150. */ + rslt = bmm150_init(&bmm150_dev); + bmi2_error_codes_print_result(rslt); + + /* Set the power mode to normal mode. */ + settings.pwr_mode = BMM150_POWERMODE_NORMAL; + rslt = bmm150_set_op_mode(&settings, &bmm150_dev); + bmi2_error_codes_print_result(rslt); + + printf("MAGNETOMETER, ACCEL AND GYRO DATA IN MANUAL MODE\n"); + + if (bmm150_dev.chip_id == BMM150_CHIP_ID) + { + while (limit--) + { + /* Delay has been added to get aux, accel and gyro data at the interval of every 0.05 second. */ + bmi2_dev.delay_us(50000, bmi2_dev.intf_ptr); + + rslt = bmi270_wh_get_sensor_data(sensor_data, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nAcc_x = %d\t", sensor_data[ACCEL].sens_data.acc.x); + printf("Acc_y = %d\t", sensor_data[ACCEL].sens_data.acc.y); + printf("Acc_z = %d", sensor_data[ACCEL].sens_data.acc.z); + + /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ + x = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.x, 2, bmi2_dev.resolution); + y = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.y, 2, bmi2_dev.resolution); + z = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.z, 2, bmi2_dev.resolution); + + /* Print the data in m/s2. */ + printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); + + printf("\nGyr_X = %d\t", sensor_data[GYRO].sens_data.gyr.x); + printf("Gyr_Y = %d\t", sensor_data[GYRO].sens_data.gyr.y); + printf("Gyr_Z = %d", sensor_data[GYRO].sens_data.gyr.z); + + /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ + x = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.x, 2000, bmi2_dev.resolution); + y = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.y, 2000, bmi2_dev.resolution); + z = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.z, 2000, bmi2_dev.resolution); + + /* Print the data in dps. */ + printf("\nGyro_DPS_X = %4.2f, Gyro_DPS_Y = %4.2f, Gyro_DPS_Z = %4.2f\n", x, y, z); + } + + /* Read aux data from the bmm150 data registers. */ + rslt = bmi2_read_aux_man_mode(BMM150_REG_DATA_X_LSB, aux_data, 8, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + if (rslt == BMI2_OK) + { + /* Compensating the raw auxiliary data available from the BMM150 API. */ + rslt = bmm150_aux_mag_data(aux_data, &mag_data, &bmm150_dev); + bmi2_error_codes_print_result(rslt); + printf("Mag_x_axis = %d \t Mag_y_axis = %d \t Mag_z_axis = %d \t\n", mag_data.x, mag_data.y, + mag_data.z); + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This function reads the data from auxiliary sensor in manual mode. + */ +int8_t user_aux_read(uint8_t reg_addr, uint8_t *aux_data, uint32_t len, void *intf_ptr) +{ + int8_t rslt; + + /* Discarding the parameter id as it is redundant */ + rslt = bmi2_read_aux_man_mode(reg_addr, aux_data, len, &bmi2_dev); + + return rslt; +} + +/*! + * @brief This function writes the data to auxiliary sensor in manual mode. + */ +int8_t user_aux_write(uint8_t reg_addr, const uint8_t *aux_data, uint32_t len, void *intf_ptr) +{ + int8_t rslt; + + /* Discarding the parameter id as it is redundant */ + rslt = bmi2_write_aux_man_mode(reg_addr, aux_data, len, &bmi2_dev); + + return rslt; +} + +/*! + * Delay function map to COINES platform + */ +static void user_delay_us(uint32_t period, void *intf_ptr) +{ + coines_delay_usec(period); +} + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (GRAVITY_EARTH * val * g_range) / half_scale; +} + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_activity/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_activity/Makefile new file mode 100644 index 000000000..a4fcf100c --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_activity/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= step_activity.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_wh.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_activity/step_activity.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_activity/step_activity.c new file mode 100644 index 000000000..0c3e9c755 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_activity/step_activity.c @@ -0,0 +1,110 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_wh.h" +#include "common.h" + +/******************************************************************************/ +/*! Functions */ +/* This function starts the execution of program. */ +int main(void) +{ + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Structure to define the type of sensor and their respective data. */ + struct bmi2_sensor_data sensor_data; + + /* Structure to define the type of sensor and its configurations. */ + struct bmi2_sens_config config; + + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Accel sensor and step activity feature are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_STEP_ACTIVITY }; + + /* Type of sensor to get step activity data. */ + sensor_data.type = BMI2_STEP_ACTIVITY; + + /* Variable to get step activity interrupt status. */ + uint16_t int_status = 0; + + uint16_t loop = 10; + + /* Select features and their pins to be mapped to. */ + struct bmi2_sens_int_config sens_int = { .type = BMI2_STEP_ACTIVITY, .hw_int_pin = BMI2_INT2 }; + + /* The step activities are listed in array. */ + const char *activity_output[4] = { "BMI2_STILL", "BMI2_WALK", "BMI2_RUN", "BMI2_UNKNOWN" }; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_wh_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the selected sensor. */ + rslt = bmi270_wh_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configure the type of sensor. */ + config.type = BMI2_STEP_ACTIVITY; + + if (rslt == BMI2_OK) + { + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_wh_get_sensor_config(&config, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Map the feature interrupt for step activity. */ + rslt = bmi270_wh_map_feat_int(&sens_int, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("\nMove the board in steps to perform step activity:\n"); + + /* Loop to get step activity. */ + while (loop) + { + /* To get the interrupt status of step detector. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the interrupt status of step detector. */ + if (int_status & BMI270_WH_STEP_ACT_STATUS_MASK) + { + printf("Step detected\n"); + + /* Get step activity output. */ + rslt = bmi270_wh_get_sensor_data(&sensor_data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Print the step activity output. */ + printf("Step activity = %s\n", activity_output[sensor_data.sens_data.activity_output]); + + loop--; + } + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_counter/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_counter/Makefile new file mode 100644 index 000000000..1c16cc62a --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_counter/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= step_counter.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_wh.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_counter/step_counter.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_counter/step_counter.c new file mode 100644 index 000000000..8d5f968fb --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_counter/step_counter.c @@ -0,0 +1,146 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_wh.h" +#include "common.h" + +/******************************************************************************/ +/*! Static function declaration */ + +/*! + * @brief This internal API is used to set configurations for step counter. + * + * @param[in] bmi2_dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_feature_config(struct bmi2_dev *bmi2_dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Structure to define type of sensor and their respective data. */ + struct bmi2_sensor_data sensor_data; + + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Accel sensor and step counter feature are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_STEP_COUNTER }; + + /* Variable to get step counter interrupt status. */ + uint16_t int_status = 0; + + /* Select features and their pins to be mapped to. */ + struct bmi2_sens_int_config sens_int = { .type = BMI2_STEP_COUNTER, .hw_int_pin = BMI2_INT2 }; + + /* Type of sensor to get step counter data. */ + sensor_data.type = BMI2_STEP_COUNTER; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_wh. */ + rslt = bmi270_wh_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the selected sensor. */ + rslt = bmi270_wh_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Set the feature configuration for step counter. */ + rslt = set_feature_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("Step counter watermark level set to 1 (20 steps)\n"); + + if (rslt == BMI2_OK) + { + /* Map the step counter feature interrupt. */ + rslt = bmi270_wh_map_feat_int(&sens_int, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Move the board in steps for 20 times to get step counter interrupt. */ + printf("Move the board in steps\n"); + + /* Loop to get number of steps counted. */ + do + { + /* To get the interrupt status of the step counter. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the interrupt status of the step counter. */ + if (int_status & BMI270_WH_STEP_CNT_STATUS_MASK) + { + printf("Step counter interrupt occurred when watermark level (20 steps) is reached\n"); + + /* Get step counter output. */ + rslt = bmi270_wh_get_sensor_data(&sensor_data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Print the step counter output. */ + printf("No of steps counted = %ld", sensor_data.sens_data.step_counter_output); + break; + } + } while (rslt == BMI2_OK); + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for step counter. + */ +static int8_t set_feature_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define the type of sensor and its configurations. */ + struct bmi2_sens_config config; + + /* Configure the type of sensor. */ + config.type = BMI2_STEP_COUNTER; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_wh_get_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Setting water-mark level to 1 for step counter to get interrupt after 20 step counts. Every 20 steps once + * output triggers. */ + config.cfg.step_counter.watermark_level = 1; + + /* Set new configuration. */ + rslt = bmi270_wh_set_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/wrist_wear_wakeup/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/wrist_wear_wakeup/Makefile new file mode 100644 index 000000000..65d3561d8 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/wrist_wear_wakeup/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= wrist_wear_wakeup.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_wh.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/wrist_wear_wakeup/wrist_wear_wakeup.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/wrist_wear_wakeup/wrist_wear_wakeup.c new file mode 100644 index 000000000..bee1191d9 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/wrist_wear_wakeup/wrist_wear_wakeup.c @@ -0,0 +1,131 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_wh.h" +#include "common.h" + +/******************************************************************************/ +/*! Static function declaration */ + +/*! + * @brief This internal API is used to set the sensor configuration. + * + * @param[in] bmi2_dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t bmi2_set_config(struct bmi2_dev *bmi2_dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program */ +int main(void) +{ + /* Variable to define result */ + int8_t rslt; + + /* Initialize status of wrist wear wakeup interrupt */ + uint16_t int_status = 0; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Select features and their pins to be mapped to */ + struct bmi2_sens_int_config sens_int = { .type = BMI2_WRIST_WEAR_WAKE_UP_WH, .hw_int_pin = BMI2_INT1 }; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_wh. */ + rslt = bmi270_wh_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Set the sensor configuration */ + rslt = bmi2_set_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Map the feature interrupt */ + rslt = bmi270_wh_map_feat_int(&sens_int, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("Tilt the board in landscape position to trigger wrist wear wakeup\n"); + + /* Loop to print the wrist wear wakeup data when interrupt occurs */ + while (1) + { + int_status = 0; + + /* To get the interrupt status of the wrist wear wakeup */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the interrupt status of the wrist gesture */ + if ((rslt == BMI2_OK) && (int_status & BMI270_WH_WRIST_WEAR_WAKEUP_WH_STATUS_MASK)) + { + printf("Wrist wear wakeup detected\n"); + break; + } + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API sets the sensor configuration + */ +static int8_t bmi2_set_config(struct bmi2_dev *bmi2_dev) +{ + /* Variable to define result */ + int8_t rslt; + + /* List the sensors which are required to enable */ + uint8_t sens_list[2] = { BMI2_ACCEL, BMI2_WRIST_WEAR_WAKE_UP_WH }; + + /* Structure to define the type of the sensor and its configurations */ + struct bmi2_sens_config config; + + /* Configure type of feature */ + config.type = BMI2_WRIST_WEAR_WAKE_UP_WH; + + /* Enable the selected sensors */ + rslt = bmi270_wh_sensor_enable(sens_list, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Get default configurations for the type of feature selected */ + rslt = bmi270_wh_get_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Set the new configuration */ + rslt = bmi270_wh_set_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/common/common.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/common/common.c new file mode 100644 index 000000000..1a43947a9 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/common/common.c @@ -0,0 +1,153 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +#include +#include +#include + +#include "coines.h" +#include "common.h" + +/******************************************************************************/ +/*! Global declaration */ + +/*! Device address for primary and secondary interface */ +static uint8_t ois_dev_addr; + +/******************************************************************************/ +/*! Functions */ + +/*! + * SPI read function map to COINES platform + */ +int8_t bmi2_ois_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_read_spi(dev_addr, reg_addr, reg_data, (uint16_t)len); +} + +/*! + * SPI write function map to COINES platform + */ +int8_t bmi2_ois_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_write_spi(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len); +} + +/*! + * Delay function map to COINES platform + */ +void bmi2_ois_delay_us(uint32_t period, void *intf_ptr) +{ + coines_delay_usec(period); +} + +/******************************************************************************/ +int8_t bmi2_ois_init(struct bmi2_ois_dev *ois_dev) +{ + int8_t rslt; + + if (ois_dev != NULL) + { + int16_t result = coines_open_comm_intf(COINES_COMM_INTF_USB); + if (result < COINES_SUCCESS) + { + printf( + "\n Unable to connect with Application Board ! \n" " 1. Check if the board is connected and powered on. \n" " 2. Check if Application Board USB driver is installed. \n" + " 3. Check if board is in use by another application. (Insufficient permissions to access USB) \n"); + exit(result); + } + + coines_set_shuttleboard_vdd_vddio_config(0, 0); + coines_delay_msec(100); + + printf("SPI Interface \n"); + + /* To initialize the user SPI function */ + ois_dev_addr = COINES_SHUTTLE_PIN_8; /*COINES_SHUTTLE_PIN_7 */ + ois_dev->ois_read = bmi2_ois_spi_read; + ois_dev->ois_write = bmi2_ois_spi_write; + + /* Assign device address to interface pointer */ + ois_dev->intf_ptr = &ois_dev_addr; + + /* Configure delay in microseconds */ + ois_dev->ois_delay_us = bmi2_ois_delay_us; + + /* SDO pin is made low for selecting I2C address 0x76*/ + coines_set_pin_config(COINES_SHUTTLE_PIN_8, COINES_PIN_DIRECTION_OUT, COINES_PIN_VALUE_LOW); + + /* coines_config_spi_bus(COINES_SPI_BUS_0, COINES_SPI_SPEED_5_MHZ, COINES_SPI_MODE3); */ + coines_config_spi_bus(COINES_SPI_BUS_0, COINES_SPI_SPEED_7_5_MHZ, COINES_SPI_MODE0); + coines_delay_msec(10); + + /* PS pin is made high for selecting I2C protocol (gyroscope)*/ + coines_set_pin_config(COINES_SHUTTLE_PIN_9, COINES_PIN_DIRECTION_OUT, COINES_PIN_VALUE_HIGH); + + coines_delay_usec(10000); + + coines_set_shuttleboard_vdd_vddio_config(3300, 3300); + + coines_delay_usec(10000); + + } + else + { + rslt = BMI2_OIS_E_NULL_PTR; + } + + return rslt; +} + +/******************************************************************************/ + +/*! + * @brief This internal API prints the execution status + */ +void bmi2_ois_error_codes_print_result(const char api_name[], int8_t rslt) +{ + if (rslt != BMI2_OIS_OK) + { + printf("%s\t", api_name); + + if (rslt == BMI2_OIS_E_NULL_PTR) + { + printf("Error [%d] : Null pointer error.\r\n", rslt); + printf( + "It occurs when the user tries to assign value (not address) to a pointer, which has been initialized to NULL.\r\n"); + } + else if (rslt == BMI2_OIS_E_COM_FAIL) + { + printf("Error [%d] : Communication failure error.\r\n", rslt); + printf( + "It occurs due to read/write operation failure and also due to power failure during communication\r\n"); + } + else if (rslt == BMI2_OIS_E_INVALID_SENSOR) + { + printf( + "Error [%d] : Invalid sensor error. It occurs when there is a mismatch in the requested feature with the available one\r\n", + rslt); + } + else + { + printf("Error [%d] : Unknown error code\r\n", rslt); + } + } +} + +/*! + * @brief Deinitializes coines platform + * + * @return void. + */ +void bmi2_ois_coines_deinit(void) +{ + coines_close_comm_intf(COINES_COMM_INTF_USB); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/common/common.h b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/common/common.h new file mode 100644 index 000000000..82a843bbf --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/common/common.h @@ -0,0 +1,92 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +#ifndef _COMMON_H +#define _COMMON_H + +/*! CPP guard */ +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include "bmi2_ois.h" + +/*! + * @brief Function for reading the sensor's registers through SPI bus. + * + * @param[in] reg_addr : Register address. + * @param[out] reg_data : Pointer to the data buffer to store the read data. + * @param[in] length : No of bytes to read. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +int8_t bmi2_ois_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief Function for writing the sensor's registers through SPI bus. + * + * @param[in] reg_addr : Register address. + * @param[in] reg_data : Pointer to the data buffer whose data has to be written. + * @param[in] length : No of bytes to write. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +int8_t bmi2_ois_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief This function provides the delay for required time (Microsecond) as per the input provided in some of the + * APIs. + * + * @param[in] period_us : The required wait time in microsecond. + * @param[in] intf_ptr : Interface pointer + * + * @return void. + * + */ +void bmi2_ois_delay_us(uint32_t period, void *intf_ptr); + +/*! + * @brief Function to initialize device structure and coines platform + * + * @param[in] bma : Structure instance of bmi2_dev + * + * @return Status of execution + * @retval 0 -> Success + * @retval < 0 -> Failure Info + */ +int8_t bmi2_ois_init(struct bmi2_ois_dev *ois_dev); + +/*! + * @brief Prints the execution status of the APIs. + * + * @param[in] api_name : API name with return status + * @param[in] rslt : Error code returned by the API whose execution status has to be printed. + * + * @return void. + */ +void bmi2_ois_error_codes_print_result(const char api_name[], int8_t rslt); + +/*! + * @brief Deinitializes coines platform + * + * @return void. + */ +void bmi2_ois_coines_deinit(void); + +#ifdef __cplusplus +} +#endif /* End of CPP guard */ + +#endif /* End _COMMON_H */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/ois_accel_gyro/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/ois_accel_gyro/Makefile new file mode 100644 index 000000000..65cd94644 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/ois_accel_gyro/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= ois_accel_gyro.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi2_ois.c \ +$(COMMON_LOCATION)/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/ois_accel_gyro/ois_accel_gyro.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/ois_accel_gyro/ois_accel_gyro.c new file mode 100644 index 000000000..bdb9d8f93 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/ois_accel_gyro/ois_accel_gyro.c @@ -0,0 +1,118 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +#include + +#include "bmi2_ois.h" +#include "bmi2_ois_common.h" + +/******************************************************************************/ +/*! Macro definition */ + +/*! Macro that holds the total number of accel x,y and z axes sample counts to be printed */ +#define ACCEL_GYRO_SAMPLE_COUNT UINT8_C(50) + +/******************************************************************************/ +/*! Functions */ + +int main(void) +{ + int8_t rslt; + + struct bmi2_ois_dev ois_dev; + + int8_t idx; + + /* To store the gyroscope cross sensitivity value */ + int16_t ois_gyr_cross_sens_zx = 0; + + /* Array to enable sensor through OIS interface */ + uint8_t sens_sel[2] = { BMI2_OIS_ACCEL, BMI2_OIS_GYRO }; + + /* Variable that holds the accel and gyro sample count */ + uint8_t n_data = ACCEL_GYRO_SAMPLE_COUNT; + + /* Initialize the device structure */ + rslt = bmi2_ois_init(&ois_dev); + bmi2_ois_error_codes_print_result("bmi2_ois_init", rslt); + + /* Get configurations for OIS */ + rslt = bmi2_ois_get_config(&ois_dev); + bmi2_ois_error_codes_print_result("bmi2_ois_get_config", rslt); + + /* OIS configurations */ + ois_dev.acc_en = BMI2_OIS_ENABLE; + ois_dev.gyr_en = BMI2_OIS_ENABLE; + ois_dev.lp_filter_en = BMI2_OIS_ENABLE; + + /* Set configurations for OIS */ + rslt = bmi2_ois_set_config(&ois_dev); + bmi2_ois_error_codes_print_result("bmi2_ois_set_config", rslt); + + /* Get configurations for OIS */ + rslt = bmi2_ois_get_config(&ois_dev); + bmi2_ois_error_codes_print_result("bmi2_ois_get_config", rslt); + + if (rslt == BMI2_OIS_OK) + { + for (idx = 0; idx < n_data; idx++) + { + ois_dev.ois_delay_us(156, ois_dev.intf_ptr); + + /* Accel ODR is 1600hz and gyro ODR is 6400hz.Delay required for + * accel 156us and 625us for gyro. + * taken modules from accel and gyro ODR which results every 4th sample accel and gyro + * read happens, rest of three samples gyro data alone will be read */ + if (idx % 4 == 0) + { + /* Get OIS accelerometer and gyro data through OIS interface + * @note for sensor which support gyro cross axes sensitivity pass the + * gyr_cross_sens_zx from the bmi2_dev structure */ + rslt = bmi2_ois_read_data(sens_sel, 2, &ois_dev, ois_gyr_cross_sens_zx); + bmi2_ois_error_codes_print_result("bmi2_ois_read_data", rslt); + + if (rslt == BMI2_OIS_OK) + { + printf("\n"); + + /* Print accelerometer data */ + printf("OIS Accel x-axis = %d ", ois_dev.acc_data.x); + printf("OIS Accel y-axis = %d ", ois_dev.acc_data.y); + printf("OIS Accel z-axis = %d \n", ois_dev.acc_data.z); + + /* Print gyro data */ + printf("OIS Gyro x-axis = %d ", ois_dev.gyr_data.x); + printf("OIS Gyro y-axis = %d ", ois_dev.gyr_data.y); + printf("OIS Gyro z-axis = %d \n", ois_dev.gyr_data.z); + + printf("\n"); + } + } + else + { + /* Get OIS gyro data through OIS interface + * @note for sensor which support gyro cross axes sensitivity pass the + * gyr_cross_sens_zx from the bmi2_dev structure */ + rslt = bmi2_ois_read_data(&sens_sel[1], 1, &ois_dev, ois_gyr_cross_sens_zx); + bmi2_ois_error_codes_print_result("bmi2_ois_read_data", rslt); + if (rslt == BMI2_OIS_OK) + { + /* Print gyro data */ + printf("OIS Gyro x-axis = %d ", ois_dev.gyr_data.x); + printf("OIS Gyro y-axis = %d ", ois_dev.gyr_data.y); + printf("OIS Gyro z-axis = %d ", ois_dev.gyr_data.z); + + printf("\n"); + } + } + } + } + + bmi2_ois_coines_deinit(); + + return rslt; +} diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi270.c b/src/main/drivers/accgyro/accgyro_spi_bmi270.c index 9267f3e38..bd379a185 100644 --- a/src/main/drivers/accgyro/accgyro_spi_bmi270.c +++ b/src/main/drivers/accgyro/accgyro_spi_bmi270.c @@ -40,10 +40,10 @@ #define BMI270_FIFO_FRAME_SIZE 6 -#define BMI270_CONFIG_SIZE 8192 +#define BMI270_CONFIG_SIZE 328 // Declaration for the device config (microcode) that must be uploaded to the sensor -extern const uint8_t bmi270_config_file[BMI270_CONFIG_SIZE]; +extern const uint8_t bmi270_maximum_fifo_config_file[BMI270_CONFIG_SIZE]; #define BMI270_CHIP_ID 0x24 @@ -174,7 +174,7 @@ static void bmi270UploadConfig(const busDevice_t *bus) // Transfer the config file IOLo(bus->busdev_u.spi.csnPin); spiTransferByte(bus->busdev_u.spi.instance, BMI270_REG_INIT_DATA); - spiTransfer(bus->busdev_u.spi.instance, bmi270_config_file, NULL, sizeof(bmi270_config_file)); + spiTransfer(bus->busdev_u.spi.instance, bmi270_maximum_fifo_config_file, NULL, sizeof(bmi270_maximum_fifo_config_file)); IOHi(bus->busdev_u.spi.csnPin); delay(10); diff --git a/src/main/target/IFLIGHT_H743_AIO/target.mk b/src/main/target/IFLIGHT_H743_AIO/target.mk index 3aa286078..bc160d7eb 100644 --- a/src/main/target/IFLIGHT_H743_AIO/target.mk +++ b/src/main/target/IFLIGHT_H743_AIO/target.mk @@ -11,7 +11,7 @@ TARGET_SRC += \ drivers/accgyro/accgyro_spi_mpu6000.c \ drivers/accgyro/accgyro_spi_lsm6dso_init.c \ drivers/accgyro/accgyro_spi_lsm6dso.c \ - $(ROOT)/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.c \ + $(ROOT)/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_maximum_fifo.c \ drivers/accgyro/accgyro_spi_bmi270.c \ drivers/barometer/barometer_dps310.c \ drivers/barometer/barometer_bmp280.c \ @@ -21,4 +21,4 @@ TARGET_SRC += \ drivers/compass/compass_lis3mdl.c \ drivers/light_ws2811strip.c \ drivers/light_ws2811strip_hal.c \ - drivers/max7456.c \ No newline at end of file + drivers/max7456.c diff --git a/src/main/target/IFLIGHT_H743_AIO_V2/target.mk b/src/main/target/IFLIGHT_H743_AIO_V2/target.mk index 3aa286078..bc160d7eb 100644 --- a/src/main/target/IFLIGHT_H743_AIO_V2/target.mk +++ b/src/main/target/IFLIGHT_H743_AIO_V2/target.mk @@ -11,7 +11,7 @@ TARGET_SRC += \ drivers/accgyro/accgyro_spi_mpu6000.c \ drivers/accgyro/accgyro_spi_lsm6dso_init.c \ drivers/accgyro/accgyro_spi_lsm6dso.c \ - $(ROOT)/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.c \ + $(ROOT)/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_maximum_fifo.c \ drivers/accgyro/accgyro_spi_bmi270.c \ drivers/barometer/barometer_dps310.c \ drivers/barometer/barometer_bmp280.c \ @@ -21,4 +21,4 @@ TARGET_SRC += \ drivers/compass/compass_lis3mdl.c \ drivers/light_ws2811strip.c \ drivers/light_ws2811strip_hal.c \ - drivers/max7456.c \ No newline at end of file + drivers/max7456.c diff --git a/src/main/target/STM32_UNIFIED/target.mk b/src/main/target/STM32_UNIFIED/target.mk index 35882ae88..7032e91ae 100644 --- a/src/main/target/STM32_UNIFIED/target.mk +++ b/src/main/target/STM32_UNIFIED/target.mk @@ -35,7 +35,7 @@ endif TARGET_SRC = \ $(addprefix drivers/accgyro/,$(notdir $(wildcard $(SRC_DIR)/drivers/accgyro/*.c))) \ - $(ROOT)/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.c \ + $(ROOT)/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_maximum_fifo.c \ $(addprefix drivers/barometer/,$(notdir $(wildcard $(SRC_DIR)/drivers/barometer/*.c))) \ $(addprefix drivers/compass/,$(notdir $(wildcard $(SRC_DIR)/drivers/compass/*.c))) \ drivers/max7456.c \