diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/LICENSE b/lib/main/BoschSensortec/BMI270-Sensor-API/LICENSE new file mode 100644 index 000000000..df241f252 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/LICENSE @@ -0,0 +1,30 @@ +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. \ 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 new file mode 100644 index 000000000..2bf3b9eb6 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/README.md @@ -0,0 +1,19 @@ +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) + +--- + +#### Copyright (C) 2019 Bosch Sensortec GmbH diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2.c b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2.c new file mode 100644 index 000000000..f04e04d1d --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2.c @@ -0,0 +1,19615 @@ +/** +* 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.c +* @date 2020-01-10 +* @version v2.46.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 +{ + /*! X data */ + int32_t x; + + /*! Y data */ + int32_t y; + + /*! Z data */ + 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 +{ + /*! X data */ + int32_t x; + + /*! Y data */ + int32_t y; + + /*! Z data */ + int32_t z; +}; + +/*! @name Structure to store accelerometer data deviation from ideal value */ +struct offset_delta +{ + /*! X axis */ + int16_t x; + + /*! Y axis */ + int16_t y; + + /*! Z axis */ + int16_t z; +}; + +/*! @name Structure to store accelerometer offset values */ +struct accel_offset +{ + /*! offset X data */ + uint8_t x; + + /*! offset Y data */ + uint8_t y; + + /*! offset Z data */ + uint8_t z; +}; + +/******************************************************************************/ + +/*! Local Function Prototypes + ******************************************************************************/ + +/*! + * @brief This internal API writes the configuration file. + * + * @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 write_config_file(struct bmi2_dev *dev); + +/*! + * @brief This internal API enables/disables the loading of the configuration + * file. + * + * @param[in] enable : To enable/disable configuration load. + * @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_config_load(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API loads the configuration file. + * + * @param[in] config_data : Pointer to the configuration file. + * @param[in] index : Variable to define array index. + * @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 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. + * + * @param[in,out] config : Structure instance of bmi2_accel_config. + * @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 + * + */ +static int8_t set_accel_config(struct bmi2_accel_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API validates bandwidth and performance mode of the + * accelerometer set by the user. + * + * @param[in, out] bandwidth : Pointer to bandwidth value set by the user. + * @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 + */ +static int8_t validate_bw_perf_mode(uint8_t *bandwidth, uint8_t *perf_mode, struct bmi2_dev *dev); + +/*! + * @brief This internal API validates ODR and range of the accelerometer set by + * the user. + * + * @param[in, out] odr : Pointer to ODR value set by the user. + * @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 + */ +static int8_t validate_odr_range(uint8_t *odr, uint8_t *range, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets gyroscope configurations like ODR, bandwidth, + * low power/high performance mode, performance mode and range. + * + * @param[in,out] config : Structure instance of bmi2_gyro_config. + * @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 + */ +static int8_t set_gyro_config(struct bmi2_gyro_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API validates bandwidth, performance mode, low power/ + * high performance mode, ODR, and range set by the user. + * + * @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 + */ +static int8_t validate_gyro_config(struct bmi2_gyro_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API shows the error status when illegal sensor + * configuration is set. + * + * @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 + */ +static int8_t cfg_error_status(const struct bmi2_dev *dev); + +/*! + * @brief This internal API: + * 1) Enables/Disables auxiliary interface. + * 2) Sets auxiliary interface configurations like I2C address, manual/auto + * mode enable, manual burst read length, AUX burst read length and AUX read + * address. + * 3)It maps/un-maps data interrupts to that of hardware interrupt line. + * + * @param[in] config : Structure instance of bmi2_aux_config. + * @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 + */ +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. + * + * @param[in] config : Structure instance of bmi2_gyro_user_gain_config. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @verbatim + *---------------------------------------------------------------------------- + * bmi2_gyro_user_gain_config| + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * ratio_x | Gain update value for x-axis + * -------------------------|--------------------------------------------------- + * 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 + */ +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. + * + * @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 + * + * @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_config(const struct bmi2_tilt_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets uphold to wake configurations like output + * configuration. + * + * @param[in] config : Structure instance of bmi2_uphold_to_wake_config. + * @param[in, out] 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 + * + * @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_config(const struct bmi2_up_hold_to_wake_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets glance detector configurations like output + * configuration. + * + * @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 + * + * @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_detect_config(const struct bmi2_glance_det_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets wake-up configurations like sensitivity, + * single/double tap enable and output-configuration. + * + * @param[in] 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 + * + * @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_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); + +/*! + * @brief This internal API gets accelerometer configurations like ODR, + * bandwidth, performance mode and g-range. + * + * @param[out] config : Structure instance of bmi2_accel_config. + * @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 + */ +static int8_t get_accel_config(struct bmi2_accel_config *config, const struct bmi2_dev *dev); + +/*! + * @brief This internal API gets gyroscope configurations like ODR, bandwidth, + * low power/ high performance mode, performance mode and range. + * + * @param[out] config : Structure instance of bmi2_gyro_config. + * @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 + */ +static int8_t get_gyro_config(struct bmi2_gyro_config *config, const struct bmi2_dev *dev); + +/*! + * @brief This internal API: + * 1) Gets the status of auxiliary interface enable. + * 2) Gets auxiliary interface configurations like I2C address, manual/auto + * mode enable, manual burst read length, AUX burst read length and AUX read + * address. + * 3) Gets ODR and offset. + * + * @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_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); + +/*! + * @brief This internal API gets gyroscope user-gain configurations like gain + * update value for x, y and z-axis. + * + * @param[out] config : Structure instance of bmi2_gyro_user_gain_config. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @verbatim + *---------------------------------------------------------------------------- + * bmi2_gyro_user_gain_config| + * Structure parameters | Description + *-------------------------|-------------------------------------------------- + * ratio_x | Gain update value for x-axis + * ------------------------|--------------------------------------------------- + * 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 + */ +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. + * + * @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 + * + * @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_tilt_config(struct bmi2_tilt_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets uphold to wake configurations like output + * configuration. + * + * @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 + * + * @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_up_hold_to_wake_config(struct bmi2_up_hold_to_wake_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets glance detector configurations like output + * configuration. + * + * @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 + * + * @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_glance_detect_config(struct bmi2_glance_det_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets wake-up configurations like sensitivity, + * single/double tap enable and output-configuration. + * + * @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 + * + * @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_wake_up_config(struct bmi2_wake_up_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. + * -------------------------|--------------------------------------------------- + * | 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_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_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_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); + +/*! + * @brief This internal API gets the accelerometer data from the register. + * + * @param[out] data : Structure instance of sensor_data. + * @param[in] reg_addr : Register address where data is stored. + * @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 get_accel_sensor_data(struct bmi2_sens_axes_data *data, uint8_t reg_addr, const struct bmi2_dev *dev); + +/*! + * @brief This internal API gets the gyroscope data from the register. + * + * @param[out] data : Structure instance of sensor_data. + * @param[in] reg_addr : Register address where data is stored. + * @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 get_gyro_sensor_data(struct bmi2_sens_axes_data *data, uint8_t reg_addr, const struct bmi2_dev *dev); + +/*! + * @brief This internal API gets the accelerometer/gyroscope data. + * + * @param[out] data : Structure instance of sensor_data. + * @param[in] reg_data : Data stored in the register. + * + * @return None + * + * @retval None + */ +static void get_acc_gyr_data(struct bmi2_sens_axes_data *data, const uint8_t *reg_data); + +/*! + * @brief This internal API gets the re-mapped accelerometer/gyroscope data. + * + * @param[out] data : Structure instance of sensor_data. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return None + * + * @retval None + */ +static void get_remapped_data(struct bmi2_sens_axes_data *data, const struct bmi2_dev *dev); + +/*! + * @brief This internal API reads the user-defined bytes of data from the given + * register address of auxiliary sensor in data mode. + * + * @param[out] aux_data : Pointer to the stored auxiliary 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_AUX_INVALID_CFG - Error: Invalid auxiliary configuration + */ +static int8_t read_aux_data_mode(uint8_t *aux_data, const struct bmi2_dev *dev); + +/*! + * @brief This internal API reads the user-defined bytes of data from the given + * register address of auxiliary sensor in manual mode. + * + * @param[in] reg_addr : AUX address from where data is read. + * @param[out] aux_data : Pointer to the stored auxiliary data. + * @param[in] len : Total bytes to be read. + * @param[in] burst_len : Bytes of data to be read in bursts. + * @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 + */ +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); + +/*! + * @brief This internal API checks the busy status of auxiliary sensor and sets + * the auxiliary register addresses when not busy. + * + * @param[in] reg_addr : Address in which AUX register address is + * set. + * @param[in] reg_data : Auxiliary register address to be set when AUX is + * not busy. + * @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 + */ +static int8_t set_if_aux_not_busy(uint8_t reg_addr, uint8_t reg_data, struct bmi2_dev *dev); + +/*! + * @brief his internal API maps the actual burst read length with that of the + * register value set by user. + * + * @param[out] len : Actual burst length. + * @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 + */ +static int8_t map_read_len(uint8_t *len, const struct bmi2_dev *dev); + +/*! + * @brief This internal API writes AUX write address and the user-defined bytes + * of data to the AUX sensor in manual mode. + * + * @note Change of BMI2_AUX_WR_ADDR is only allowed if AUX is not busy. + * + * @param[in] reg_addr : AUX address in which data is to be written. + * @param[in] reg_data : Data to be written + * @param[in] dev : Structure instance of bmi2_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 + */ +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] 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 + */ +static int8_t map_feat_int(uint8_t *reg_data_array, enum bmi2_hw_int_pin int_pin, uint8_t int_mask); + +/*! + * @brief This internal API computes the number of bytes of accelerometer FIFO + * data which is to be parsed in header-less mode. + * + * @param[out] start_idx : The start index for parsing data. + * @param[out] len : Number of bytes to be parsed. + * @param[in] acc_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 + */ +static int8_t parse_fifo_accel_len(uint16_t *start_idx, + uint16_t *len, + const uint16_t *acc_count, + const struct bmi2_fifo_frame *fifo); + +/*! + * @brief This internal API is used to parse accelerometer data from the FIFO + * data in header mode. + * + * @param[out] acc : Structure instance of bmi2_sens_axes_data where + * the parsed accelerometer data bytes are stored. + * @param[in] accel_length : Number of accelerometer frames (x,y,z data). + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * @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 + */ +static int8_t extract_accel_header_mode(struct bmi2_sens_axes_data *acc, + uint16_t *accel_length, + struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to parse the accelerometer data from the + * FIFO data in both header and header-less mode. It updates the current data + * byte to be parsed. + * + * @param[in,out] acc : Structure instance of bmi2_sens_axes_data where + * where the parsed data bytes are stored. + * @param[in,out] idx : Index value of number of bytes parsed. + * @param[in,out] acc_idx : Index value of accelerometer data (x,y,z axes) + * frame to be parsed. + * @param[in] frame : Either data is enabled by user in header-less + * mode or header frame value in header mode. + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * @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 + */ +static int8_t unpack_accel_frame(struct bmi2_sens_axes_data *acc, + uint16_t *idx, + uint16_t *acc_idx, + uint8_t frame, + const struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to parse accelerometer data from the FIFO + * data. + * + * @param[out] acc : Structure instance of bmi2_sens_axes_data + * where the parsed data bytes are stored. + * @param[in] data_start_index : Index value of the accelerometer data bytes + * which is to be parsed from the FIFO data. + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return None + * @retval None + */ +static void unpack_accel_data(struct bmi2_sens_axes_data *acc, + uint16_t data_start_index, + const struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev); + +/*! + * @brief This internal API computes the number of bytes of gyroscope FIFO data + * which is to be parsed in header-less mode. + * + * @param[out] start_idx : The start index for parsing data. + * @param[out] len : Number of bytes to be parsed. + * @param[in] gyr_count : Number of gyroscope frames to be read. + * @param[in] frame : Either data enabled by user in header-less + * mode or header frame value in header mode. + * @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 + */ +static int8_t parse_fifo_gyro_len(uint16_t *start_idx, + uint16_t(*len), + const uint16_t *gyr_count, + const struct bmi2_fifo_frame *fifo); + +/*! + * @brief This internal API is used to parse the gyroscope data from the FIFO + * data in both header and header-less mode It updates the current data byte to + * be parsed. + * + * @param[in,out] gyr : Structure instance of bmi2_sens_axes_data. + * @param[in,out] idx : Index value of number of bytes parsed + * @param[in,out] gyr_idx : Index value of gyroscope data (x,y,z axes) + * frame to be parsed. + * @param[in] frame : Either data is enabled by user in header-less + * mode or header frame value in header mode. + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * @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 + */ +static int8_t unpack_gyro_frame(struct bmi2_sens_axes_data *gyr, + uint16_t *idx, + uint16_t *gyr_idx, + uint8_t frame, + const struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to parse gyroscope data from the FIFO data. + * + * @param[out] gyr : Structure instance of bmi2_sens_axes_data where + * the parsed gyroscope data bytes are stored. + * @param[in] data_start_index : Index value of the gyroscope data bytes + * which is to be parsed from the FIFO data. + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return None + * @retval None + */ +static void unpack_gyro_data(struct bmi2_sens_axes_data *gyr, + uint16_t data_start_index, + const struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to parse the gyroscope data from the + * FIFO data in header mode. + * + * @param[out] gyr : Structure instance of bmi2_sens_axes_data where + * the parsed gyroscope data bytes are stored. + * @param[in] gyro_length : Number of gyroscope frames (x,y,z data). + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * @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 + */ +static int8_t extract_gyro_header_mode(struct bmi2_sens_axes_data *gyr, + uint16_t *gyro_length, + struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev); + +/*! + * @brief This API computes the number of bytes of auxiliary FIFO data + * which is to be parsed in header-less mode. + * + * @param[out] start_idx : The start index for parsing data. + * @param[out] len : Number of bytes to be parsed. + * @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 + */ +static int8_t parse_fifo_aux_len(uint16_t *start_idx, + uint16_t(*len), + const uint16_t *aux_count, + const struct bmi2_fifo_frame *fifo); + +/*! + * @brief This API is used to parse auxiliary data from the FIFO data. + * + * @param[out] aux : Pointer to buffer where the parsed auxiliary data + * bytes are stored. + * @param[in] aux_length : Number of auxiliary frames (x,y,z data). + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * @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 + */ +static int8_t extract_aux_header_mode(struct bmi2_aux_fifo_data *aux, + uint16_t *aux_length, + struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev); + +/*! + * @brief This API is used to parse the auxiliary data from the FIFO data in + * both header and header-less mode. It updates the current data byte to be + * parsed. + * + * @param[out] aux : Pointer to structure where the parsed auxiliary data + * bytes are stored. + * @param[in,out] idx : Index value of number of bytes parsed + * @param[in,out] aux_idx : Index value of auxiliary data (x,y,z axes) + * frame to be parsed + * @param[in] frame : Either data is enabled by user in header-less + * mode or header frame value in header mode. + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * @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 + */ +static int8_t unpack_aux_frame(struct bmi2_aux_fifo_data *aux, + uint16_t *idx, + uint16_t *aux_idx, + uint8_t frame, + const struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev); + +/*! + * @brief This API is used to parse auxiliary data from the FIFO data. + * + * @param[out] aux : Pointer to structure where the parsed + * auxiliary data bytes are stored. + * @param[in] data_start_index : Index value of the auxiliary data bytes which + * is to be parsed from the FIFO data. + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * + * @return None + * @retval None + */ +static void unpack_aux_data(struct bmi2_aux_fifo_data *aux, + uint16_t data_start_index, + const struct bmi2_fifo_frame *fifo); + +/*! + * @brief This internal API is used to reset the FIFO related configurations + * in the FIFO frame structure for the next FIFO read. + * + * @param[in, out] fifo : Structure instance of bmi2_fifo_frame. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return None + * @retval None + */ +static void reset_fifo_frame_structure(struct bmi2_fifo_frame *fifo, const struct bmi2_dev *dev); + +/*! + * @brief This internal API checks whether the FIFO data read is an empty frame. + * If empty frame, index is moved to the last byte. + * + * @param[in,out] data_index : The index of the current data to be parsed from + * FIFO data. + * @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 check_empty_fifo(uint16_t *data_index, const struct bmi2_fifo_frame *fifo); + +/*! + * @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 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_next_frame(uint16_t *data_index, uint8_t current_frame_length, const struct bmi2_fifo_frame *fifo); + +/*! + * @brief This internal API is used to parse and store the sensor time from the + * FIFO data. + * + * @param[in,out] data_index : Index of the FIFO data which has the sensor time. + * @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 unpack_sensortime_frame(uint16_t *data_index, struct bmi2_fifo_frame *fifo); + +/*! + * @brief This internal API is used to parse and store the skipped frame count + * from the FIFO data. + * + * @param[in,out] data_index : Index of the FIFO data which contains skipped + * frame count. + * @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 unpack_skipped_frame(uint16_t *data_index, struct bmi2_fifo_frame *fifo); + +/*! + * @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. + * + * @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 + */ +static int8_t pre_self_test_config(struct bmi2_dev *dev); + +/*! + * @brief This internal API performs the steps needed for self-test operation + * before reading the accelerometer self-test data. + * + * @param[in] sign : Selects sign of self-test excitation + * @param[in] dev : Structure instance of bmi2_dev. + * + * sign | Description + * -------------|--------------- + * BMI2_ENABLE | positive excitation + * BMI2_DISABLE | negative excitation + * + * @return Result of API execution status + * + * @retval BMI2_OK - Success. + * @retval BMI2_E_COM_FAIL - Error: Communication fail + */ +static int8_t self_test_config(uint8_t sign, struct bmi2_dev *dev); + +/*! + * @brief This internal API enables or disables the accelerometer self-test + * feature in the sensor. + * + * @param[in] enable : Enables/ Disables self-test. + * @param[in] dev : Structure instance of bmi2_dev. + * + * sign | Description + * -------------|--------------- + * BMI2_ENABLE | Enables self-test + * BMI2_DISABLE | Disables self-test + * + * @return Result of API execution status + * + * @retval BMI2_OK - Success. + * @retval BMI2_E_COM_FAIL - Error: Communication fail + */ +static int8_t set_accel_self_test_enable(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API selects the sign for accelerometer self-test + * excitation. + * + * @param[in] sign : Selects sign of self-test excitation + * @param[in] dev : Structure instance of bmi2_dev. + * + * sign | Description + * -------------|--------------- + * BMI2_ENABLE | positive excitation + * BMI2_DISABLE | negative excitation + * + * @return Result of API execution status + * + * @retval BMI2_OK - Success. + * @retval BMI2_E_COM_FAIL - Error: Communication fail + */ +static int8_t set_acc_self_test_sign(uint8_t sign, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets the amplitude of the accelerometer self-test + * deflection in the sensor. + * + * @param[in] amp : Select amplitude of the self-test deflection. + * @param[in] dev : Structure instance of bmi2_dev. + * + * amp | Description + * -------------|--------------- + * BMI2_ENABLE | self-test amplitude is high + * 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 + */ +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. + * + * @param[out] accel : Buffer to store the acceleration 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 + */ +static int8_t read_accel_xyz(struct bmi2_sens_axes_data *accel, const struct bmi2_dev *dev); + +/*! + * @brief This internal API converts LSB value of accelerometer axes to form + * 'g' to 'mg' for self-test. + * + * @param[in] acc_data_diff : Stores the acceleration value difference in g. + * @param[out]acc_data_diff_mg : Stores the acceleration value difference in mg. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @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, + const struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to calculate the power of a value. + * + * @param[in] base : base for power calculation. + * @param[in] resolution : exponent for power calculation. + * + * @return the calculated power + * @retval the power value + */ +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. + * + * @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 + */ +static int8_t validate_self_test(const struct selftest_delta_limit *accel_data_diff); + +/*! + * @brief This internal API gets the re-mapped x, y and z axes from the sensor. + * + * @param[out] remap : Structure that stores local copy of re-mapped 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_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. + * + * @param[in] remap : Structure that stores local copy of re-mapped 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 set_remap_axes(const struct axes_remap *remap, struct bmi2_dev *dev); + +/*! + * @brief Interface to get max burst length + * + * @param[in] max_burst_len : Pointer to store max burst length + * @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_maxburst_len(uint8_t *max_burst_len, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets the max burst length. + * + * @param[in] write_len_byte : read & write length + * @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_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. + * + * @param[in, out] frame_header : FIFO frame header. + * @param[in, out] data_index : Index value of the FIFO data bytes + * from which sensor frame header is to be parsed + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * + * @return None + * @retval None + */ +static void parse_if_virtual_header(uint8_t *frame_header, uint16_t *data_index, const struct bmi2_fifo_frame *fifo); + +/*! + * @brief This internal API gets sensor time from the accelerometer and + * gyroscope virtual frames and updates in the data structure. + * + * @param[out] sens : Sensor data structure + * @param[in, out] idx : Index of FIFO from where the data is to retrieved. + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * + * @return None + * @retval None + */ +static void unpack_virt_sensor_time(struct bmi2_sens_axes_data *sens, uint16_t *idx, + const struct bmi2_fifo_frame *fifo); + +/*! + * @brief This internal API gets sensor time from the auxiliary virtual + * frames and updates in the data structure. + * + * @param[out] aux : Auxiliary sensor data structure + * @param[in, out] idx : Index of FIFO from where the data is to retrieved. + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * + * @return None + * @retval None + */ +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. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[out] gyr_data : Structure instance of gyroscope data + * + * @return Result of API execution status + * + * @return None + * @retval None + */ +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. + * + * @param[out] acc_cfg : Accelerometer configuration value + * @param[out] aps : Advance power mode value + * @param[out] acc_en : Accelerometer enable 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 + */ +static int8_t save_accel_foc_config(struct bmi2_accel_config *acc_cfg, + uint8_t *aps, + uint8_t *acc_en, + struct bmi2_dev *dev); + +/*! + * @brief This internal API performs Fast Offset Compensation for accelerometer. + * + * @param[in] accel_g_value : This parameter selects the accel foc + * axis to be performed + * + * input format is {x, y, z, sign}. '1' to enable. '0' to disable + * + * eg to choose x axis {1, 0, 0, 0} + * eg to choose -x axis {1, 0, 0, 1} + * + * @param[in] acc_cfg : Accelerometer 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 + */ +static int8_t perform_accel_foc(const struct accel_foc_g_value *accel_g_value, + const struct bmi2_accel_config *acc_cfg, + struct bmi2_dev *dev); + +/*! + * @brief This internal sets configurations for performing accelerometer FOC. + * + * @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 + */ +static int8_t set_accel_foc_config(struct bmi2_dev *dev); + +/*! + * @brief This internal API enables/disables the offset compensation for + * filtered and un-filtered accelerometer data. + * + * @param[in] offset_en : enables/disables offset compensation. + * @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_accel_offset_comp(uint8_t offset_en, struct bmi2_dev *dev); + +/*! + * @brief This internal API converts the range value into accelerometer + * corresponding integer value. + * + * @param[in] range_in : Input range value. + * @param[out] range_out : Stores the integer value of range. + * + * @return None + * @retval None + */ +static void map_accel_range(uint8_t range_in, uint8_t *range_out); + +/*! + * @brief This internal API compensate the accelerometer data against gravity. + * + * @param[in] lsb_per_g : LSB value pre 1g. + * @param[in] g_val : G reference value of all axis. + * @param[in] data : Accelerometer data + * @param[out] comp_data : Stores the data that is compensated by taking the + * difference in accelerometer data and lsb_per_g + * value. + * + * @return None + * @retval None + */ +static void comp_for_gravity(uint16_t lsb_per_g, + const struct accel_foc_g_value *g_val, + const struct bmi2_sens_axes_data *data, + struct offset_delta *comp_data); + +/*! + * @brief This internal API scales the compensated accelerometer data according + * to the offset register resolution. + * + * @param[in] range : G-range of the accelerometer. + * @param[out] comp_data : Data that is compensated by taking the + * difference in accelerometer data and lsb_per_g + * value. + * @param[out] data : Stores offset data + * + * @return None + * @retval None + */ +static void scale_accel_offset(uint8_t range, const struct offset_delta *comp_data, struct accel_offset *data); + +/*! + * @brief This internal API finds the bit position of 3.9mg according to given + * range and resolution. + * + * @param[in] range : G-range of the accelerometer. + * + * @return Result of API execution status + * @retval Bit position of 3.9mg + */ +static int8_t get_bit_pos_3_9mg(uint8_t range); + +/*! + * @brief This internal API inverts the accelerometer offset data. + * + * @param[out] offset_data : Stores the inverted offset data + * + * @return None + * @retval None + */ +static void invert_accel_offset(struct accel_offset *offset_data); + +/*! + * @brief This internal API writes the offset data in the offset compensation + * register. + * + * @param[in] 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 + */ +static int8_t write_accel_offset(const struct accel_offset *offset, struct bmi2_dev *dev); + +/*! + * @brief This internal API restores the configurations saved before performing + * accelerometer FOC. + * + * @param[in] acc_cfg : Accelerometer configuration value + * @param[in] acc_en : Accelerometer enable value + * @param[in] aps : Advance power mode 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 BMI2_E_SET_APS_FAIL - Error: Set Advance Power Save Fail + */ +static int8_t restore_accel_foc_config(struct bmi2_accel_config *acc_cfg, + uint8_t aps, + uint8_t acc_en, + struct bmi2_dev *dev); + +/*! + * @brief This internal API saves the configurations before performing gyroscope + * FOC. + * + * @param[out] gyr_cfg : Gyroscope configuration value + * @param[out] gyr_en : Gyroscope enable value + * @param[out] aps : Advance power mode 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 + */ +static int8_t save_gyro_config(struct bmi2_gyro_config *gyr_cfg, uint8_t *aps, uint8_t *gyr_en, struct bmi2_dev *dev); + +/*! + * @brief This internal sets configurations for performing gyroscope FOC. + * + * @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 + */ +static int8_t set_gyro_foc_config(struct bmi2_dev *dev); + +/*! + * @brief This internal API inverts the gyroscope offset data. + * + * @param[out] offset_data : Stores the inverted offset data + * + * @return None + * @retval None + */ +static void invert_gyro_offset(struct bmi2_sens_axes_data *offset_data); + +/*! + * @brief This internal API restores the gyroscope configurations saved + * before performing FOC. + * + * @param[in] gyr_cfg : Gyroscope configuration value + * @param[in] gyr_en : Gyroscope enable value + * @param[in] aps : Advance power mode 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 + */ +static int8_t restore_gyro_config(struct bmi2_gyro_config *gyr_cfg, uint8_t aps, uint8_t gyr_en, struct bmi2_dev *dev); + +/*! + * @brief This internal API saturates the gyroscope data value before writing to + * to 10 bit offset register. + * + * @param[in, out] gyr_off : Gyroscope data to be stored in offset register + * + * @return None + * @retval None + */ +static void saturate_gyro_data(struct bmi2_sens_axes_data *gyr_off); + +/*! + * @brief This internal API reads the gyroscope data for x, y and z axis from + * the sensor. + * + * @param[out] gyro : Buffer to store the gyroscope 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 + */ +static int8_t read_gyro_xyz(struct bmi2_sens_axes_data *gyro, const struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to check the boundary conditions. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in,out] val : Pointer to the value to be validated. + * @param[in] min : minimum value. + * @param[in] max : maximum value. + * + * @return Result of API execution status + * + * @retval BMI2_OK - Success. + * @retval BMI2_E_NULL_PTR - Error: Null pointer error + * + */ +static int8_t check_boundary_val(uint8_t *val, uint8_t min, uint8_t max, struct bmi2_dev *dev); + +/*! + * @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 BMI2_OK - Success. + * @retval BMI2_E_NULL_PTR - Error: Null pointer error + */ +static int8_t null_ptr_check(const struct bmi2_dev *dev); + +/*! + * @brief This updates the result for 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_CRT_ERROR - Error: CRT Fail + * + */ +static int8_t crt_gyro_st_update_result(struct bmi2_dev *dev); + +/*! + * @brief This function is to get the st_status status. + * + * @param[in] *st_status: gets the crt running 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 + * + */ +static int8_t get_st_running(uint8_t *st_status, const struct bmi2_dev *dev); + +/*! + * @brief This function is to set crt bit to running. + * + * @param[in] enable + * @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 + * + */ +static int8_t set_st_running(uint8_t st_status, struct bmi2_dev *dev); + +/*! + * @brief This function is to make the initial changes for CRT. + * Disable the gyro, OIS, aps + * Note: For the purpose of preparing CRT Gyro, OIS and APS are disabled + * + * @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 + * + */ +static int8_t crt_prepare_setup(struct bmi2_dev *dev); + +static int8_t do_gtrigger_test(uint8_t gyro_st_crt, struct bmi2_dev *dev); + +/*! + * @brief This function is to get the rdy for dl bit status + * this will toggle from 0 to 1 and visevers according to the + * dowload status + * + * @param[in] *rdy_for_dl: gets the rdy_for_dl 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 + * + */ +static int8_t get_rdy_for_dl(uint8_t *rdy_for_dl, const struct bmi2_dev *dev); + +/*! + * @brief This function is to write the config file in the given location for crt. + * which inter checks the status of the rdy_for_dl bit and also the crt running, and + * wirtes the given size. + * + * @param[in] write_len: length of the words to be written + * @param[in] config_file_size: length of the words to be written + * @param[in] start_index: provide the start index from where config file has to written + * @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 + * + */ +static int8_t write_crt_config_file(uint16_t write_len, + uint16_t config_file_size, + uint16_t start_index, + struct bmi2_dev *dev); + +/*! + * @brief This function is to check for rdy_for_dl bit to toggle for CRT process + * + * @param[in] retry_complete: wait for given time to toggle + * @param[in] download_ready: get the status for rdy_for_dl + * @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 + * + */ +static int8_t wait_rdy_for_dl_toggle(uint8_t retry_complete, uint8_t download_ready, const struct bmi2_dev *dev); + +/*! + * @brief This function is to wait till the CRT or gyro self-test process is completed + * + * @param[in] retry_complete: wait for given time to complete the crt process + * @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 + * + */ +static int8_t wait_st_running(uint8_t retry_complete, const struct bmi2_dev *dev); + +/*! + * @brief This function is to complete the crt process if max burst length is not zero + * this checks for the crt status and rdy_for_dl bit to toggle + * + * @param[in] last_byte_flag: to provide the last toggled state + * @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 + * + */ +static int8_t process_crt_download(uint8_t last_byte_flag, struct bmi2_dev *dev); + +/*! + * @brief This api is used to 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 + */ +static int8_t select_self_test(uint8_t gyro_st_crt, struct bmi2_dev *dev); + +/*! + * @brief This api is used to enable/disable abort. + * + * @param[in] abort_enable : variable to enable the abort feature. + * @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 + */ +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. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * + * @retval BMI2_OK - 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); + +/*! + * @brief This api is used to trigger the preparation for system for NVM programming. + * + * @param[out] nvm_prep : pointer to variable to store the status of nvm_prep_prog. + * @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 + */ +static int8_t set_nvm_prep_prog(uint8_t nvm_prep, struct bmi2_dev *dev); + +/*! + * @brief This api validates accel foc position as per the range + * + * @param[in] sens_list : Sensor type + * @param[in] accel_g_axis : accel axis to foc. NA for gyro foc + * @param[in] avg_foc_data : average value of sensor sample datas + * @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 + */ +static int8_t validate_foc_position(uint8_t sens_list, + const struct accel_foc_g_value *accel_g_axis, + struct bmi2_sens_axes_data avg_foc_data, + struct bmi2_dev *dev); + +/*! + * @brief This api validates accel foc axis given as input + * + * @param[in] avg_foc_data : average value of sensor sample datas + * @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 + */ +static int8_t validate_foc_accel_axis(int16_t avg_foc_data, struct bmi2_dev *dev); + +/*! + * @brief This api is used to verify the right position of the sensor before doing accel foc + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] sens_list: Sensor type + * @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 + */ +static int8_t verify_foc_position(uint8_t sens_list, const struct accel_foc_g_value *accel_g_axis, + struct bmi2_dev *dev); + +/*! + * @brief This API reads and provides average for 128 samples of sensor data for foc operation + * gyro. + * + * @param[in] sens_list : Sensor type. + * @param[in] bmi2_dev: Structure instance of bmi2_dev. + * @param[in] temp_foc_data: to store data samples + * + * @return Result of API execution status + * + * @retval BMI2_OK + */ +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); + +/*! + * @brief This internal api gets major and minor version for config file + * + * @param[out] config_major : Pointer to store the major version + * @param[out] config_minor : Pointer to store the minor version + * @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 + */ +static int8_t extract_config_file(uint16_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. + * + * @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 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(uint8_t enable, struct bmi2_dev *dev); + +/******************************************************************************/ +/*! @name User Interface Definitions */ +/******************************************************************************/ + +/*! + * @brief This API is the entry point for bmi2 sensor. It selects between + * I2C/SPI interface, based on user selection. It reads and validates the + * chip-id of the sensor. + */ +int8_t bmi2_sec_init(struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to assign chip id */ + uint8_t chip_id = 0; + + /* 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 + }; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + /* Perform soft-reset to bring all register values to their + * 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 */ + if (chip_id == dev->chip_id) + { + /* Assign resolution to the structure */ + dev->resolution = 16; + + /* Set manual enable flag */ + dev->aux_man_en = 1; + + /* Set the default values for axis + * re-mapping in the device structure + */ + dev->remap = axes_remap; + } + else + { + /* Storing the chip-id value read from + * the register to identify the sensor + */ + dev->chip_id = chip_id; + rslt = BMI2_E_DEV_NOT_FOUND; + } + } + } + } + + return rslt; +} + +/*! + * @brief This API reads the data from the given register address of bmi2 + * sensor. + * + * @note For most of the registers auto address increment applies, with the + * 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) +{ + /* 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; + + /* 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) + { + reg_addr = (reg_addr | BMI2_SPI_RD_MASK); + } + if (dev->aps_status == BMI2_ENABLE) + { + rslt = dev->read(dev->dev_id, reg_addr, temp_buf, temp_len); + dev->delay_us(450); + } + else + { + rslt = dev->read(dev->dev_id, reg_addr, temp_buf, temp_len); + dev->delay_us(20); + } + + if (rslt == BMI2_OK) + { + /* Read the data from the position next to dummy byte */ + while (index < len) + { + data[index] = temp_buf[index + dev->dummy_byte]; + index++; + } + } + else + { + rslt = BMI2_E_COM_FAIL; + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API writes data to the given register address of bmi2 sensor. + */ +int8_t bmi2_set_regs(uint8_t reg_addr, const uint8_t *data, uint16_t len, struct bmi2_dev *dev) +{ + /* 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) + { + reg_addr = (reg_addr & BMI2_SPI_WR_MASK); + } + + 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; + } + } + } + else + { + /* Burst write if APS is disabled */ + rslt = dev->write(dev->dev_id, reg_addr, data, len); + dev->delay_us(20); + } + + /* updating the advance power saver flag */ + if (reg_addr == BMI2_PWR_CONF_ADDR) + { + if (*data & BMI2_ADV_POW_EN_MASK) + { + dev->aps_status = BMI2_ENABLE; + } + else + { + dev->aps_status = BMI2_DISABLE; + } + } + if (rslt != BMI2_OK) + { + rslt = BMI2_E_COM_FAIL; + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API resets bmi2 sensor. All registers are overwritten with + * their default values. + */ +int8_t bmi2_soft_reset(struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define soft reset value */ + uint8_t data = BMI2_SOFT_RESET_CMD; + + /* Variable to read the dummy byte */ + uint8_t dummy_read = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + /* Reset bmi2 device */ + rslt = bmi2_set_regs(BMI2_CMD_REG_ADDR, &data, 1, dev); + dev->delay_us(2000); + + /* set APS flag as after soft reset the sensor is on advance power save mode */ + dev->aps_status = BMI2_ENABLE; + + /* 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)) + { + rslt = bmi2_get_regs(BMI2_CHIP_ID_ADDR, &dummy_read, 1, dev); + } + + if (rslt == BMI2_OK) + { + /* Write the configuration file */ + rslt = bmi2_write_config_file(dev); + } + + /* Reset the sensor status flag in the device structure */ + if (rslt == BMI2_OK) + { + dev->sens_en_stat = 0; + } + } + + return rslt; +} + +/*! + * @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) +{ + /* Variable to define error */ + int8_t rslt; + + /* NULL pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (config_major != NULL) && (config_minor != NULL)) + { + /* Extract the config file identification from the dmr page and get the major and minor version */ + rslt = extract_config_file(config_major, config_minor, dev); + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + 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. + */ +int8_t bmi2_set_adv_power_save(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store data */ + uint8_t reg_data = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + rslt = bmi2_get_regs(BMI2_PWR_CONF_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + 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); + } + } + } + + return rslt; +} + +/*! + * @brief This API gets the status of advance power save mode in the sensor. + */ +int8_t bmi2_get_adv_power_save(uint8_t *aps_status, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store data */ + uint8_t reg_data = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (aps_status != NULL)) + { + rslt = bmi2_get_regs(BMI2_PWR_CONF_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + *aps_status = BMI2_GET_BIT_POS0(reg_data, BMI2_ADV_POW_EN); + dev->aps_status = *aps_status; + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API loads the configuration file into the bmi2 sensor. + */ +int8_t bmi2_write_config_file(struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to know the load status */ + uint8_t load_status = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (dev->config_size != 0)) + { + /* Bytes written are multiples of 2 */ + if ((dev->read_write_len % 2) != 0) + { + dev->read_write_len = dev->read_write_len - 1; + } + if (dev->read_write_len < 2) + { + dev->read_write_len = 2; + } + + /* Write the configuration file */ + rslt = write_config_file(dev); + if (rslt == BMI2_OK) + { + /* Check the configuration load status */ + rslt = bmi2_get_internal_status(&load_status, dev); + + /* Return error if loading not successful */ + if ((rslt == BMI2_OK) && (!(load_status & BMI2_CONFIG_LOAD_SUCCESS))) + { + rslt = BMI2_E_CONFIG_LOAD; + } + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief 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. + */ +int8_t bmi2_set_int_pin_config(const struct bmi2_int_pin_config *int_cfg, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define data array */ + uint8_t data_array[3] = { 0 }; + + /* Variable to store register data */ + uint8_t reg_data = 0; + + /* Variable to define type of interrupt pin */ + uint8_t int_pin = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (int_cfg != NULL)) + { + /* Copy the pin type to a local variable */ + int_pin = int_cfg->pin_type; + if ((int_pin > BMI2_INT_NONE) && (int_pin < BMI2_INT_PIN_MAX)) + { + /* Get the previous configuration data */ + rslt = bmi2_get_regs(BMI2_INT1_IO_CTRL_ADDR, data_array, 3, dev); + if (rslt == BMI2_OK) + { + /* Set interrupt pin 1 configuration */ + if ((int_pin == BMI2_INT1) || (int_pin == BMI2_INT_BOTH)) + { + /* Configure active low or high */ + reg_data = BMI2_SET_BITS(data_array[0], BMI2_INT_LEVEL, int_cfg->pin_cfg[0].lvl); + + /* Configure push-pull or open drain */ + reg_data = BMI2_SET_BITS(reg_data, BMI2_INT_OPEN_DRAIN, int_cfg->pin_cfg[0].od); + + /* Configure output enable */ + reg_data = BMI2_SET_BITS(reg_data, BMI2_INT_OUTPUT_EN, int_cfg->pin_cfg[0].output_en); + + /* Configure input enable */ + reg_data = BMI2_SET_BITS(reg_data, BMI2_INT_INPUT_EN, int_cfg->pin_cfg[0].input_en); + + /* Copy the data to be written in the respective array */ + data_array[0] = reg_data; + } + + /* Set interrupt pin 2 configuration */ + if ((int_pin == BMI2_INT2) || (int_pin == BMI2_INT_BOTH)) + { + /* Configure active low or high */ + reg_data = BMI2_SET_BITS(data_array[1], BMI2_INT_LEVEL, int_cfg->pin_cfg[1].lvl); + + /* Configure push-pull or open drain */ + reg_data = BMI2_SET_BITS(reg_data, BMI2_INT_OPEN_DRAIN, int_cfg->pin_cfg[1].od); + + /* Configure output enable */ + reg_data = BMI2_SET_BITS(reg_data, BMI2_INT_OUTPUT_EN, int_cfg->pin_cfg[1].output_en); + + /* Configure input enable */ + reg_data = BMI2_SET_BITS(reg_data, BMI2_INT_INPUT_EN, int_cfg->pin_cfg[1].input_en); + + /* Copy the data to be written in the respective array */ + data_array[1] = reg_data; + } + + /* Configure the interrupt mode */ + data_array[2] = BMI2_SET_BIT_POS0(data_array[2], BMI2_INT_LATCH, int_cfg->int_latch); + + /* Set the configurations simultaneously as + * INT1_IO_CTRL, INT2_IO_CTRL, and INT_LATCH lie + * in consecutive addresses + */ + rslt = bmi2_set_regs(BMI2_INT1_IO_CTRL_ADDR, data_array, 3, dev); + } + } + else + { + rslt = BMI2_E_INVALID_INT_PIN; + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief 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. + */ +int8_t bmi2_get_int_pin_config(struct bmi2_int_pin_config *int_cfg, const struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define data array */ + uint8_t data_array[3] = { 0 }; + + /* Variable to define type of interrupt pin */ + uint8_t int_pin = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (int_cfg != NULL)) + { + /* Copy the pin type to a local variable */ + int_pin = int_cfg->pin_type; + + /* Get the previous configuration data */ + rslt = bmi2_get_regs(BMI2_INT1_IO_CTRL_ADDR, data_array, 3, dev); + if (rslt == BMI2_OK) + { + /* Get interrupt pin 1 configuration */ + if ((int_pin == BMI2_INT1) || (int_pin == BMI2_INT_BOTH)) + { + /* Get active low or high */ + int_cfg->pin_cfg[0].lvl = BMI2_GET_BITS(data_array[0], BMI2_INT_LEVEL); + + /* Get push-pull or open drain */ + int_cfg->pin_cfg[0].od = BMI2_GET_BITS(data_array[0], BMI2_INT_OPEN_DRAIN); + + /* Get output enable */ + int_cfg->pin_cfg[0].output_en = BMI2_GET_BITS(data_array[0], BMI2_INT_OUTPUT_EN); + + /* Get input enable */ + int_cfg->pin_cfg[0].input_en = BMI2_GET_BITS(data_array[0], BMI2_INT_INPUT_EN); + } + + /* Get interrupt pin 2 configuration */ + if ((int_pin == BMI2_INT2) || (int_pin == BMI2_INT_BOTH)) + { + /* Get active low or high */ + int_cfg->pin_cfg[1].lvl = BMI2_GET_BITS(data_array[1], BMI2_INT_LEVEL); + + /* Get push-pull or open drain */ + int_cfg->pin_cfg[1].od = BMI2_GET_BITS(data_array[1], BMI2_INT_OPEN_DRAIN); + + /* Get output enable */ + int_cfg->pin_cfg[1].output_en = BMI2_GET_BITS(data_array[1], BMI2_INT_OUTPUT_EN); + + /* Get input enable */ + int_cfg->pin_cfg[1].input_en = BMI2_GET_BITS(data_array[1], BMI2_INT_INPUT_EN); + } + + /* Get interrupt mode */ + int_cfg->int_latch = BMI2_GET_BIT_POS0(data_array[2], BMI2_INT_LATCH); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @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) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to store data */ + uint8_t data_array[2] = { 0 }; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (int_status != NULL)) + { + /* Get the interrupt status */ + rslt = bmi2_get_regs(BMI2_INT_STATUS_0_ADDR, data_array, 2, dev); + if (rslt == BMI2_OK) + { + *int_status = (uint16_t) data_array[0] | ((uint16_t) data_array[1] << 8); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API sets the FIFO configuration in the sensor. + */ +int8_t bmi2_set_fifo_config(uint16_t config, uint8_t enable, struct bmi2_dev *dev) +{ + int8_t rslt; + uint8_t data[2] = { 0 }; + uint8_t max_burst_len = 0; + + /* Variable to store data of FIFO configuration register 0 */ + uint8_t fifo_config_0 = (uint8_t)(config & BMI2_FIFO_CONFIG_0_MASK); + + /* Variable to store data of FIFO configuration register 1 */ + uint8_t fifo_config_1 = (uint8_t)((config & BMI2_FIFO_CONFIG_1_MASK) >> 8); + + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + rslt = bmi2_get_regs(BMI2_FIFO_CONFIG_0_ADDR, data, BMI2_FIFO_CONFIG_LENGTH, dev); + if (rslt == BMI2_OK) + { + /* Get data to set FIFO configuration register 0 */ + if (fifo_config_0 > 0) + { + if (enable == BMI2_ENABLE) + { + data[0] = data[0] | fifo_config_0; + } + else + { + data[0] = data[0] & (~fifo_config_0); + } + } + + /* Get data to set FIFO configuration register 1 */ + if (enable == BMI2_ENABLE) + { + data[1] = data[1] | fifo_config_1; + if (dev->variant_feature & BMI2_CRT_RTOSK_ENABLE) + { + + /* Burst length is needed for CRT + * FIFO enable will reset the default values + * So configure the max burst length again. + */ + 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); + } + } + } + else + { + data[1] = data[1] & (~fifo_config_1); + } + + /* Set the FIFO configurations */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_regs(BMI2_FIFO_CONFIG_0_ADDR, data, 2, dev); + } + } + } + + return rslt; +} + +/*! + * @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) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to store data */ + uint8_t data[2] = { 0 }; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (fifo_config != NULL)) + { + /* Get the FIFO configuration value */ + rslt = bmi2_get_regs(BMI2_FIFO_CONFIG_0_ADDR, data, BMI2_FIFO_CONFIG_LENGTH, dev); + if (rslt == BMI2_OK) + { + (*fifo_config) = (uint16_t)((uint16_t) data[0] & BMI2_FIFO_CONFIG_0_MASK); + (*fifo_config) |= (uint16_t)(((uint16_t) data[1] << 8) & BMI2_FIFO_CONFIG_1_MASK); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API reads the FIFO data. + */ +int8_t bmi2_read_fifo_data(struct bmi2_fifo_frame *fifo, const struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to store FIFO configuration data */ + uint8_t config_data[2] = { 0 }; + + /* Variable to define FIFO address */ + uint8_t addr = BMI2_FIFO_DATA_ADDR; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (fifo != NULL)) + { + /* Clear the FIFO data structure */ + reset_fifo_frame_structure(fifo, dev); + + /* Read FIFO data */ + rslt = bmi2_get_regs(addr, fifo->data, fifo->length, dev); + if (rslt == BMI2_OK) + { + + /* Get the set FIFO frame configurations */ + rslt = bmi2_get_regs(BMI2_FIFO_CONFIG_0_ADDR, config_data, 2, dev); + if (rslt == BMI2_OK) + { + /* Get FIFO header status */ + fifo->header_enable = (uint8_t)((config_data[1]) & (BMI2_FIFO_HEADER_EN >> 8)); + + /* Get sensor enable status, of which the data is to be read */ + fifo->data_enable = + (uint16_t)(((config_data[0]) | ((uint16_t) config_data[1] << 8)) & BMI2_FIFO_ALL_EN); + } + } + else + { + rslt = BMI2_E_COM_FAIL; + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief 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. + */ +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) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to index the bytes */ + uint16_t data_index = 0; + + /* Variable to index accelerometer frames */ + uint16_t accel_index = 0; + + /* Variable to store the number of bytes to be read */ + uint16_t data_read_length = 0; + + /* Variable to define the data enable byte */ + uint8_t data_enable = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (accel_data != NULL) && (accel_length != NULL) && (fifo != NULL)) + { + /* Parsing the FIFO data in header-less mode */ + if (fifo->header_enable == 0) + { + + /* Get the number of accelerometer bytes to be read */ + rslt = parse_fifo_accel_len(&data_index, &data_read_length, accel_length, fifo); + + /* Convert word to byte since all sensor enables are in a byte */ + data_enable = (uint8_t)(fifo->data_enable >> 8); + for (; (data_index < data_read_length) && (rslt != BMI2_W_FIFO_EMPTY);) + { + /* 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 */ + rslt = check_empty_fifo(&data_index, fifo); + } + } + + /* Update number of accelerometer frames to be read */ + (*accel_length) = accel_index; + + /* Update the accelerometer byte index */ + fifo->acc_byte_start_idx = data_index; + } + else + { + /* Parsing the FIFO data in header mode */ + rslt = extract_accel_header_mode(accel_data, accel_length, fifo, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief 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. + */ +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) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to index the bytes */ + uint16_t data_index = 0; + + /* Variable to index gyroscope frames */ + uint16_t gyro_index = 0; + + /* Variable to store the number of bytes to be read */ + uint16_t data_read_length = 0; + + /* Variable to define the data enable byte */ + uint8_t data_enable = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (gyro_data != NULL) && (gyro_length != NULL) && (fifo != NULL)) + { + /* Parsing the FIFO data in header-less mode */ + if (fifo->header_enable == 0) + { + /* Get the number of gyro bytes to be read */ + rslt = parse_fifo_gyro_len(&data_index, &data_read_length, gyro_length, fifo); + + /* Convert word to byte since all sensor enables are in a byte */ + data_enable = (uint8_t)(fifo->data_enable >> 8); + for (; (data_index < data_read_length) && (rslt != BMI2_W_FIFO_EMPTY);) + { + /* Unpack frame to get gyroscope data */ + rslt = unpack_gyro_frame(gyro_data, &data_index, &gyro_index, data_enable, fifo, dev); + if (rslt != BMI2_W_FIFO_EMPTY) + { + /* Check for the availability of next two bytes of FIFO data */ + rslt = check_empty_fifo(&data_index, fifo); + } + } + + /* Update number of gyroscope frames to be read */ + (*gyro_length) = gyro_index; + + /* Update the gyroscope byte index */ + fifo->acc_byte_start_idx = data_index; + } + else + { + /* Parsing the FIFO data in header mode */ + rslt = extract_gyro_header_mode(gyro_data, gyro_length, fifo, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief 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. + */ +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) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to index the bytes */ + uint16_t data_index = 0; + + /* Variable to index auxiliary frames */ + uint16_t aux_index = 0; + + /* Variable to store the number of bytes to be read */ + uint16_t data_read_length = 0; + + /* Variable to define the data enable byte */ + uint8_t data_enable = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (aux != NULL) && (aux_length != NULL) && (fifo != NULL)) + { + /* Parsing the FIFO data in header-less mode */ + if (fifo->header_enable == 0) + { + rslt = parse_fifo_aux_len(&data_index, &data_read_length, aux_length, fifo); + + /* Convert word to byte since all sensor enables are in + * a byte + */ + data_enable = (uint8_t)(fifo->data_enable >> 8); + for (; (data_index < data_read_length) && (rslt != BMI2_W_FIFO_EMPTY);) + { + /* Unpack frame to get auxiliary data */ + rslt = unpack_aux_frame(aux, &data_index, &aux_index, data_enable, fifo, dev); + if (rslt != BMI2_W_FIFO_EMPTY) + { + /* Check for the availability of next + * two bytes of FIFO data + */ + rslt = check_empty_fifo(&data_index, fifo); + } + } + + /* Update number of auxiliary frames to be read */ + *aux_length = aux_index; + + /* Update the auxiliary byte index */ + fifo->aux_byte_start_idx = data_index; + } + else + { + /* Parsing the FIFO data in header mode */ + rslt = extract_aux_header_mode(aux, aux_length, fifo, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API writes the available sensor specific commands to the sensor. + */ +int8_t bmi2_set_command_register(uint8_t command, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + /* Set the command in the command register */ + rslt = bmi2_set_regs(BMI2_CMD_REG_ADDR, &command, 1, dev); + } + + return rslt; +} + +/* + * @brief This API sets the FIFO self wake up functionality in the sensor. + */ +int8_t bmi2_set_fifo_self_wake_up(uint8_t fifo_self_wake_up, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store data */ + uint8_t data = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + /* Set FIFO self wake-up */ + rslt = bmi2_get_regs(BMI2_PWR_CONF_ADDR, &data, 1, dev); + if (rslt == BMI2_OK) + { + data = BMI2_SET_BITS(data, BMI2_FIFO_SELF_WAKE_UP, fifo_self_wake_up); + rslt = bmi2_set_regs(BMI2_PWR_CONF_ADDR, &data, 1, dev); + } + } + + return rslt; +} + +/*! + * @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) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store data */ + uint8_t data = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (fifo_self_wake_up != NULL)) + { + /* Get the status of FIFO self wake-up */ + rslt = bmi2_get_regs(BMI2_PWR_CONF_ADDR, &data, 1, dev); + if (rslt == BMI2_OK) + { + (*fifo_self_wake_up) = BMI2_GET_BITS(data, BMI2_FIFO_SELF_WAKE_UP); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API sets the FIFO water-mark level in the sensor. + */ +int8_t bmi2_set_fifo_wm(uint16_t fifo_wm, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to store data */ + uint8_t data[2] = { 0 }; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + /* Get LSB value of FIFO water-mark */ + data[0] = BMI2_GET_LSB(fifo_wm); + + /* Get MSB value of FIFO water-mark */ + data[1] = BMI2_GET_MSB(fifo_wm); + + /* Set the FIFO water-mark level */ + rslt = bmi2_set_regs(BMI2_FIFO_WTM_0_ADDR, data, 2, dev); + } + + return rslt; +} + +/*! + * @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) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to to store data */ + uint8_t data[2] = { 0 }; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (fifo_wm != NULL)) + { + /* Read the FIFO water mark level */ + rslt = bmi2_get_regs(BMI2_FIFO_WTM_0_ADDR, data, BMI2_FIFO_WM_LENGTH, dev); + if (rslt == BMI2_OK) + { + (*fifo_wm) = (uint16_t)((uint16_t) data[1] << 8) | (data[0]); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API sets either filtered or un-filtered FIFO accelerometer or + * gyroscope data. + */ +int8_t bmi2_set_fifo_filter_data(uint8_t sens_sel, uint8_t fifo_filter_data, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store data */ + uint8_t data = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + switch (sens_sel) + { + case BMI2_ACCEL: + + /* Validate filter mode */ + if (fifo_filter_data <= BMI2_MAX_VALUE_FIFO_FILTER) + { + /* Set the accelerometer FIFO filter data */ + rslt = bmi2_get_regs(BMI2_FIFO_DOWNS_ADDR, &data, 1, dev); + if (rslt == BMI2_OK) + { + data = BMI2_SET_BITS(data, BMI2_ACC_FIFO_FILT_DATA, fifo_filter_data); + rslt = bmi2_set_regs(BMI2_FIFO_DOWNS_ADDR, &data, 1, dev); + } + } + else + { + rslt = BMI2_E_OUT_OF_RANGE; + } + break; + case BMI2_GYRO: + + /* Validate filter mode */ + if (fifo_filter_data <= BMI2_MAX_VALUE_FIFO_FILTER) + { + /* Set the gyroscope FIFO filter data */ + rslt = bmi2_get_regs(BMI2_FIFO_DOWNS_ADDR, &data, 1, dev); + if (rslt == BMI2_OK) + { + data = BMI2_SET_BITS(data, BMI2_GYR_FIFO_FILT_DATA, fifo_filter_data); + rslt = bmi2_set_regs(BMI2_FIFO_DOWNS_ADDR, &data, 1, dev); + } + } + else + { + rslt = BMI2_E_OUT_OF_RANGE; + } + break; + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + } + + return rslt; +} + +/*! + * @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) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store FIFO filter mode */ + uint8_t data = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (fifo_filter_data != NULL)) + { + switch (sens_sel) + { + case BMI2_ACCEL: + + /* Read the accelerometer FIFO filter data */ + rslt = bmi2_get_regs(BMI2_FIFO_DOWNS_ADDR, &data, 1, dev); + if (rslt == BMI2_OK) + { + (*fifo_filter_data) = BMI2_GET_BITS(data, BMI2_ACC_FIFO_FILT_DATA); + } + break; + case BMI2_GYRO: + + /* Read the gyroscope FIFO filter data */ + rslt = bmi2_get_regs(BMI2_FIFO_DOWNS_ADDR, &data, 1, dev); + if (rslt == BMI2_OK) + { + (*fifo_filter_data) = BMI2_GET_BITS(data, BMI2_GYR_FIFO_FILT_DATA); + } + break; + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API sets the down-sampling rates for accelerometer or gyroscope + * FIFO data. + */ +int8_t bmi2_set_fifo_down_sample(uint8_t sens_sel, uint8_t fifo_down_samp, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store sampling rate */ + uint8_t data = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + switch (sens_sel) + { + case BMI2_ACCEL: + + /* Set the accelerometer FIFO down sampling rate */ + rslt = bmi2_get_regs(BMI2_FIFO_DOWNS_ADDR, &data, 1, dev); + if (rslt == BMI2_OK) + { + 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: + + /* Set the gyroscope FIFO down sampling rate */ + rslt = bmi2_get_regs(BMI2_FIFO_DOWNS_ADDR, &data, 1, dev); + if (rslt == BMI2_OK) + { + 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; + break; + } + } + + return rslt; +} + +/*! + * @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) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store sampling rate */ + uint8_t data = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (fifo_down_samp != NULL)) + { + switch (sens_sel) + { + case BMI2_ACCEL: + + /* Read the accelerometer FIFO down data sampling rate */ + rslt = bmi2_get_regs(BMI2_FIFO_DOWNS_ADDR, &data, 1, dev); + if (rslt == BMI2_OK) + { + (*fifo_down_samp) = BMI2_GET_BITS(data, BMI2_ACC_FIFO_DOWNS); + } + break; + case BMI2_GYRO: + + /* Read the gyroscope FIFO down data sampling rate */ + rslt = bmi2_get_regs(BMI2_FIFO_DOWNS_ADDR, &data, 1, dev); + if (rslt == BMI2_OK) + { + (*fifo_down_samp) = BMI2_GET_BIT_POS0(data, BMI2_GYR_FIFO_DOWNS); + } + break; + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @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) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define byte index */ + uint8_t index = 0; + + /* Array to store FIFO data length */ + uint8_t data[BMI2_FIFO_DATA_LENGTH] = { 0 }; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (fifo_length != NULL)) + { + /* Read FIFO length */ + rslt = bmi2_get_regs(BMI2_FIFO_LENGTH_0_ADDR, data, BMI2_FIFO_DATA_LENGTH, dev); + if (rslt == BMI2_OK) + { + /* Get the MSB byte index */ + index = BMI2_FIFO_LENGTH_MSB_BYTE; + + /* Get the MSB byte of FIFO length */ + data[index] = BMI2_GET_BIT_POS0(data[index], BMI2_FIFO_BYTE_COUNTER_MSB); + + /* Get total FIFO length */ + (*fifo_length) = ((data[index] << 8) | data[index - 1]); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API reads the user-defined bytes of data from the given register + * address of auxiliary sensor in manual mode. + * + * @note Change of BMI2_AUX_RD_ADDR is only allowed if AUX is not busy. + */ +int8_t bmi2_read_aux_man_mode(uint8_t reg_addr, uint8_t *aux_data, uint16_t len, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store burst length */ + uint8_t burst_len = 0; + + /* Variable to define APS status */ + uint8_t aps_stat = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (aux_data != NULL)) + { + /* Validate if manual mode */ + if (dev->aux_man_en) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + /* 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 + * length + */ + rslt = map_read_len(&burst_len, dev); + if (rslt == BMI2_OK) + { + /* Read auxiliary data */ + rslt = read_aux_data(reg_addr, aux_data, len, burst_len, dev); + } + } + + /* Enable Advance power save if disabled for reading + * data and not when already disabled + */ + if ((rslt == BMI2_OK) && (aps_stat == BMI2_ENABLE)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + else + { + rslt = BMI2_E_AUX_INVALID_CFG; + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API writes the user-defined bytes of data and the address of + * auxiliary sensor where data is to be written in manual mode. + * + * @note Change of BMI2_AUX_WR_ADDR is only allowed if AUX is not busy. + */ +int8_t bmi2_write_aux_man_mode(uint8_t reg_addr, const uint8_t *aux_data, uint16_t len, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop = 0; + + /* Variable to define APS status */ + uint8_t aps_stat = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (aux_data != NULL)) + { + /* Validate if manual mode */ + if (dev->aux_man_en) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + /* Disable APS if enabled */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + /* Byte write data in the corresponding address */ + if (rslt == BMI2_OK) + { + for (; ((loop < len) && (rslt == BMI2_OK)); loop++) + { + rslt = write_aux_data((reg_addr + loop), aux_data[loop], dev); + dev->delay_us(1000); + } + } + + /* Enable Advance power save if disabled for writing + * data and not when already disabled + */ + if ((rslt == BMI2_OK) && (aps_stat == BMI2_ENABLE)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + else + { + rslt = BMI2_E_AUX_INVALID_CFG; + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief 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. + * + * @note Change of BMI2_AUX_WR_ADDR is only allowed if AUX is not busy. + */ +int8_t bmi2_write_aux_interleaved(uint8_t reg_addr, const uint8_t *aux_data, uint16_t len, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop = 1; + + /* Variable to define APS status */ + uint8_t aps_stat = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (aux_data != NULL)) + { + /* Validate if manual mode */ + if (dev->aux_man_en) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + /* Disable APS if enabled */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + if (rslt == BMI2_OK) + { + /* Write the start register address extracted + * from the interleaved data + */ + rslt = write_aux_data(reg_addr, aux_data[0], dev); + + /* Extract the remaining address and data from + * the interleaved data and write it in the + * corresponding addresses byte by byte + */ + for (; ((loop < len) && (rslt == BMI2_OK)); loop += 2) + { + rslt = write_aux_data(aux_data[loop], aux_data[loop + 1], dev); + dev->delay_us(1000); + } + + /* Enable Advance power save if disabled for + * writing data and not when already disabled + */ + if ((rslt == BMI2_OK) && (aps_stat == BMI2_ENABLE)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + } + else + { + rslt = BMI2_E_AUX_INVALID_CFG; + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @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) +{ + /* Variable to define error */ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (status != NULL)) + { + rslt = bmi2_get_regs(BMI2_STATUS_ADDR, status, 1, dev); + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API enables/disables OIS interface. + */ +int8_t bmi2_set_ois_interface(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store data */ + uint8_t reg_data = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + rslt = bmi2_get_regs(BMI2_IF_CONF_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + /* Enable/Disable OIS interface */ + reg_data = BMI2_SET_BITS(reg_data, BMI2_OIS_IF_EN, enable); + if (enable) + { + /* Disable auxiliary interface if OIS is enabled */ + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_AUX_IF_EN); + } + + /* Set the OIS interface configurations */ + rslt = bmi2_set_regs(BMI2_IF_CONF_ADDR, ®_data, 1, dev); + } + } + + return rslt; +} + +/*! + * @brief This API can be used to write sync commands like ODR, sync period, + * frequency and phase, resolution ratio, sync time and delay time. + */ +int8_t bmi2_write_sync_commands(const uint8_t *command, uint8_t n_comm, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (command != NULL)) + { + rslt = bmi2_set_regs(BMI2_SYNC_COMMAND_ADDR, command, n_comm, dev); + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API performs self-test to check the proper functionality of the + * accelerometer sensor. + */ +int8_t bmi2_perform_accel_self_test(struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store self-test result */ + int8_t st_rslt = 0; + + /* Structure to define positive accelerometer axes */ + struct bmi2_sens_axes_data positive = { 0, 0, 0, 0 }; + + /* Structure to define negative accelerometer axes */ + 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 }; + + /* Structure for difference of accelerometer values in mg */ + struct selftest_delta_limit accel_data_diff_mg = { 0, 0, 0 }; + + /* Initialize the polarity of self-test as positive */ + int8_t sign = BMI2_ENABLE; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + /* Sets the configuration required before enabling self-test */ + rslt = pre_self_test_config(dev); + + /* Wait for greater than 2 milliseconds */ + dev->delay_us(3000); + if (rslt == BMI2_OK) + { + do + { + /* Select positive first, then negative polarity + * after enabling self-test + */ + rslt = self_test_config((uint8_t) sign, dev); + if (rslt == BMI2_OK) + { + /* Wait for greater than 50 milli-sec */ + dev->delay_us(51000); + + /* If polarity is positive */ + if (sign == BMI2_ENABLE) + { + /* Read and store positive acceleration value */ + rslt = read_accel_xyz(&positive, dev); + } + /* If polarity is negative */ + else if (sign == BMI2_DISABLE) + { + /* Read and store negative acceleration value */ + rslt = read_accel_xyz(&negative, dev); + } + } + else + { + /* Break if error */ + break; + } + + /* Break if error */ + if (rslt != BMI2_OK) + { + break; + } + + /* Turn the polarity of self-test negative */ + sign--; + } while (sign >= 0); + if (rslt == BMI2_OK) + { + /* Subtract -ve acceleration values from that of +ve values */ + accel_data_diff.x = (positive.x) - (negative.x); + accel_data_diff.y = (positive.y) - (negative.y); + accel_data_diff.z = (positive.z) - (negative.z); + + /* Convert differences of acceleration values + * from 'g' to 'mg' + */ + convert_lsb_g(&accel_data_diff, &accel_data_diff_mg, dev); + + /* Validate self-test for acceleration values + * in mg and get the self-test result + */ + st_rslt = validate_self_test(&accel_data_diff_mg); + + /* Trigger a soft reset after performing self-test */ + rslt = bmi2_soft_reset(dev); + + /* Return the self-test result */ + if (rslt == BMI2_OK) + { + rslt = st_rslt; + } + } + } + } + + return rslt; +} + +/*! + * @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) +{ + /* 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 }; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_int != NULL)) + { + /* 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 */ + feat_int = dev->int_map.sig_mot_out_conf; + break; + case BMI2_ANY_MOTION: + + /* 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 to INT1 and INT2 map register */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_regs(BMI2_INT1_MAP_FEAT_ADDR, data_array, 2, dev); + } + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API maps/un-maps data interrupts to that of interrupt pins. + */ +int8_t bmi2_map_data_int(uint8_t data_int, enum bmi2_hw_int_pin int_pin, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to mask interrupt pin 1 - lower nibble */ + uint8_t int1_mask = data_int; + + /* Variable to mask interrupt pin 2 - higher nibble */ + uint8_t int2_mask = (uint8_t)(data_int << 4); + + /* Variable to store register data */ + uint8_t reg_data = 0; + + /* Read interrupt map1 and map2 and register */ + rslt = bmi2_get_regs(BMI2_INT_MAP_DATA_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + if (int_pin < BMI2_INT_PIN_MAX) + { + switch (int_pin) + { + case BMI2_INT_NONE: + + /* Un-Map the corresponding data + * interrupt to both interrupt pin 1 and 2 + */ + reg_data &= ~(int1_mask | int2_mask); + break; + case BMI2_INT1: + + /* Map the corresponding data interrupt to + * interrupt pin 1 + */ + reg_data |= int1_mask; + break; + case BMI2_INT2: + + /* Map the corresponding data interrupt to + * interrupt pin 2 + */ + reg_data |= int2_mask; + break; + case BMI2_INT_BOTH: + + /* Map the corresponding data + * interrupt to both interrupt pin 1 and 2 + */ + reg_data |= (int1_mask | int2_mask); + break; + default: + break; + } + + /* Set the interrupts in the map register */ + rslt = bmi2_set_regs(BMI2_INT_MAP_DATA_ADDR, ®_data, 1, dev); + } + else + { + /* Return error if invalid pin selection */ + rslt = BMI2_E_INVALID_INT_PIN; + } + } + + return rslt; +} + +/*! + * @brief This API gets the re-mapped x, y and z axes from the sensor and + * updates the values in the device structure. + */ +int8_t bmi2_get_remap_axes(struct bmi2_remap *remapped_axis, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Initialize the local structure for axis re-mapping */ + struct axes_remap remap = { 0, 0, 0, 0, 0, 0 }; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (remapped_axis != NULL)) + { + /* Get the re-mapped axes from the sensor */ + rslt = get_remap_axes(&remap, dev); + if (rslt == BMI2_OK) + { + /* Store the re-mapped x-axis value in device structure + * and its user-value in the interface structure + */ + switch (remap.x_axis) + { + case MAP_X_AXIS: + + /* If mapped to x-axis */ + dev->remap.x_axis = MAP_X_AXIS; + remapped_axis->x = BMI2_X; + break; + case MAP_Y_AXIS: + + /* If mapped to y-axis */ + dev->remap.x_axis = MAP_Y_AXIS; + remapped_axis->x = BMI2_Y; + break; + case MAP_Z_AXIS: + + /* If mapped to z-axis */ + dev->remap.x_axis = MAP_Z_AXIS; + remapped_axis->x = BMI2_Z; + break; + default: + break; + } + + /* Store the re-mapped x-axis sign in device structure + * and its user-value in the interface structure + */ + if (remap.x_axis_sign) + { + /* If x-axis is mapped to -ve sign */ + dev->remap.x_axis_sign = NEG_SIGN; + remapped_axis->x |= BMI2_AXIS_SIGN; + } + else + { + dev->remap.x_axis_sign = POS_SIGN; + } + + /* Store the re-mapped y-axis value in device structure + * and its user-value in the interface structure + */ + switch (remap.y_axis) + { + case MAP_X_AXIS: + + /* If mapped to x-axis */ + dev->remap.y_axis = MAP_X_AXIS; + remapped_axis->y = BMI2_X; + break; + case MAP_Y_AXIS: + + /* If mapped to y-axis */ + dev->remap.y_axis = MAP_Y_AXIS; + remapped_axis->y = BMI2_Y; + break; + case MAP_Z_AXIS: + + /* If mapped to z-axis */ + dev->remap.y_axis = MAP_Z_AXIS; + remapped_axis->y = BMI2_Z; + break; + default: + break; + } + + /* Store the re-mapped y-axis sign in device structure + * and its user-value in the interface structure + */ + if (remap.y_axis_sign) + { + /* If y-axis is mapped to -ve sign */ + dev->remap.y_axis_sign = NEG_SIGN; + remapped_axis->y |= BMI2_AXIS_SIGN; + } + else + { + dev->remap.y_axis_sign = POS_SIGN; + } + + /* Store the re-mapped z-axis value in device structure + * and its user-value in the interface structure + */ + switch (remap.z_axis) + { + case MAP_X_AXIS: + + /* If mapped to x-axis */ + dev->remap.z_axis = MAP_X_AXIS; + remapped_axis->z = BMI2_X; + break; + case MAP_Y_AXIS: + + /* If mapped to y-axis */ + dev->remap.z_axis = MAP_Y_AXIS; + remapped_axis->z = BMI2_Y; + break; + case MAP_Z_AXIS: + + /* If mapped to z-axis */ + dev->remap.z_axis = MAP_Z_AXIS; + remapped_axis->z = BMI2_Z; + break; + default: + break; + } + + /* Store the re-mapped z-axis sign in device structure + * and its user-value in the interface structure + */ + if (remap.z_axis_sign) + { + /* If z-axis is mapped to -ve sign */ + dev->remap.z_axis_sign = NEG_SIGN; + remapped_axis->z |= BMI2_AXIS_SIGN; + } + else + { + dev->remap.z_axis_sign = POS_SIGN; + } + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API sets the re-mapped x, y and z axes to the sensor and + * updates the them in the device structure. + */ +int8_t bmi2_set_remap_axes(const struct bmi2_remap *remapped_axis, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store all the re-mapped axes */ + uint8_t remap_axes = 0; + + /* Variable to store the re-mapped x-axes */ + uint8_t remap_x = 0; + + /* Variable to store the re-mapped y-axes */ + uint8_t remap_y = 0; + + /* Variable to store the re-mapped z-axes */ + uint8_t remap_z = 0; + + /* Initialize the local structure for axis re-mapping */ + struct axes_remap remap = { 0, 0, 0, 0, 0, 0 }; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (remapped_axis != NULL)) + { + /* Check whether all the axes are re-mapped */ + remap_axes = remapped_axis->x | remapped_axis->y | remapped_axis->z; + + /* If all the axes are re-mapped */ + if ((remap_axes & BMI2_AXIS_MASK) == BMI2_AXIS_MASK) + { + /* Get the re-mapped value of x, y and z axis */ + remap_x = remapped_axis->x & BMI2_AXIS_MASK; + remap_y = remapped_axis->y & BMI2_AXIS_MASK; + remap_z = remapped_axis->z & BMI2_AXIS_MASK; + + /* Store the value of re-mapped x-axis in both + * device structure and the local structure + */ + switch (remap_x) + { + case BMI2_X: + + /* If mapped to x-axis */ + dev->remap.x_axis = MAP_X_AXIS; + remap.x_axis = 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; + break; + case BMI2_Z: + + /* If mapped to z-axis */ + dev->remap.x_axis = MAP_Z_AXIS; + remap.x_axis = MAP_Z_AXIS; + break; + default: + break; + } + + /* Store the re-mapped x-axis sign in the device + * structure and its value in local structure + */ + 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; + } + else + { + dev->remap.x_axis_sign = POS_SIGN; + remap.x_axis_sign = MAP_POSITIVE; + } + + /* Store the value of re-mapped y-axis in both + * device structure and the local structure + */ + switch (remap_y) + { + case BMI2_X: + + /* If mapped to x-axis */ + dev->remap.y_axis = MAP_X_AXIS; + remap.y_axis = 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; + break; + case BMI2_Z: + + /* If mapped to z-axis */ + dev->remap.y_axis = MAP_Z_AXIS; + remap.y_axis = MAP_Z_AXIS; + break; + default: + break; + } + + /* Store the re-mapped y-axis sign in the device + * structure and its value in local structure + */ + 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; + } + else + { + dev->remap.y_axis_sign = POS_SIGN; + remap.y_axis_sign = MAP_POSITIVE; + } + + /* Store the value of re-mapped z-axis in both + * device structure and the local structure + */ + switch (remap_z) + { + case BMI2_X: + + /* If mapped to x-axis */ + dev->remap.z_axis = MAP_X_AXIS; + remap.z_axis = 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; + break; + case BMI2_Z: + + /* If mapped to z-axis */ + dev->remap.z_axis = MAP_Z_AXIS; + remap.z_axis = MAP_Z_AXIS; + break; + default: + break; + } + + /* Store the re-mapped z-axis sign in the device + * structure and its value in local structure + */ + 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; + } + else + { + dev->remap.z_axis_sign = POS_SIGN; + remap.z_axis_sign = MAP_POSITIVE; + } + + /* Set the re-mapped axes in the sensor */ + rslt = set_remap_axes(&remap, dev); + } + else + { + rslt = BMI2_E_REMAP_ERROR; + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + 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. + */ +int8_t bmi2_set_gyro_offset_comp(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define register data */ + uint8_t reg_data = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + /* Get the status of gyroscope offset enable */ + 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_OFF_COMP_EN, enable); + + /* Enable/Disable gyroscope offset compensation */ + rslt = bmi2_set_regs(BMI2_GYR_OFF_COMP_6_ADDR, ®_data, 1, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @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) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define register data */ + uint8_t reg_data[4] = { 0 }; + + /* Variable to store LSB value of offset compensation for x-axis */ + uint8_t gyr_off_lsb_x; + + /* Variable to store LSB value of offset compensation for y-axis */ + uint8_t gyr_off_lsb_y; + + /* Variable to store LSB value of offset compensation for z-axis */ + uint8_t gyr_off_lsb_z; + + /* Variable to store MSB value of offset compensation for x-axis */ + uint8_t gyr_off_msb_x; + + /* Variable to store MSB value of offset compensation for y-axis */ + uint8_t gyr_off_msb_y; + + /* Variable to store MSB value of offset compensation for z-axis */ + uint8_t gyr_off_msb_z; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (gyr_off_comp_axes != NULL)) + { + /* Get the gyroscope compensated offset values */ + rslt = bmi2_get_regs(BMI2_GYR_OFF_COMP_3_ADDR, reg_data, 4, dev); + if (rslt == BMI2_OK) + { + /* Get LSB and MSB values of offset compensation for + * x, y and z axis + */ + 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; + + /* 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); + + /* Gyroscope offset compensation value for y-axis */ + gyr_off_comp_axes->y = (int16_t)(((uint16_t) gyr_off_msb_y << 6) | gyr_off_lsb_y); + + /* Gyroscope offset compensation value for z-axis */ + gyr_off_comp_axes->z = (int16_t)(((uint16_t) gyr_off_msb_z << 4) | gyr_off_lsb_z); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API writes the gyroscope bias values for each axis which is used + * for gyroscope offset compensation. + */ +int8_t bmi2_write_gyro_offset_comp_axes(const struct bmi2_sens_axes_data *gyr_off_comp_axes, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define register data */ + uint8_t reg_data[4] = { 0 }; + + /* Variable to store MSB value of offset compensation for x-axis */ + uint8_t gyr_off_msb_x; + + /* Variable to store MSB value of offset compensation for y-axis */ + uint8_t gyr_off_msb_y; + + /* Variable to store MSB value of offset compensation for z-axis */ + uint8_t gyr_off_msb_z; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (gyr_off_comp_axes != NULL)) + { + /* Get the MSB values of gyroscope compensated offset values */ + rslt = bmi2_get_regs(BMI2_GYR_OFF_COMP_6_ADDR, ®_data[3], 1, dev); + 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); + + /* 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); + + /* 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); + + /* Get LSB value of x-axis from user-input */ + reg_data[0] = (uint8_t)(gyr_off_comp_axes->x & 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); + + /* Get LSB value of z-axis from user-input */ + reg_data[2] = (uint8_t)(gyr_off_comp_axes->z & 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); + + /* 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); + + /* 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); + + /* Set the offset compensation values of axes */ + rslt = bmi2_set_regs(BMI2_GYR_OFF_COMP_3_ADDR, reg_data, 4, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + 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. + */ +int8_t bmi2_get_gyro_cross_sense(struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + struct bmi2_sensor_data data; + + /* Check if the feature is supported by this variant */ + if (dev->variant_feature & BMI2_GYRO_CROSS_SENS_ENABLE) + { + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + /* Select the feature whose data is to be acquired */ + data.type = BMI2_GYRO_CROSS_SENSE; + + /* Get the respective data */ + rslt = bmi2_get_sensor_data(&data, 1, dev); + if (rslt == BMI2_OK) + { + /* Update the gyroscope cross sense value of z axis + * in the device structure + */ + dev->gyr_cross_sens_zx = data.sens_data.correction_factor_zx; + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + } + + return rslt; +} + +/*! + * @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) +{ + /* Variable to define error */ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (int_stat != NULL)) + { + /* Delay to read the internal status */ + dev->delay_us(20000); + + /* Get the error bits and message */ + rslt = bmi2_get_regs(BMI2_INTERNAL_STATUS_ADDR, int_stat, 1, dev); + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @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) +{ + int8_t rslt; + + struct bmi2_sens_axes_data avg_foc_data = { 0 }; + struct foc_temp_value temp_foc_data = { 0 }; + + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + /* Enable sensor */ + rslt = bmi2_sensor_enable(&sens_list, 1, dev); + } + if (rslt == BMI2_OK) + { + + rslt = get_average_of_sensor_data(sens_list, &temp_foc_data, dev); + if (rslt == BMI2_OK) + { + if (sens_list == BMI2_ACCEL) + { + + /* Taking modulus to make negative values as positive */ + if ((accel_g_axis->x == 1) && (accel_g_axis->sign == 1)) + { + temp_foc_data.x = temp_foc_data.x * -1; + } + else if ((accel_g_axis->y == 1) && (accel_g_axis->sign == 1)) + { + temp_foc_data.y = temp_foc_data.y * -1; + } + else if ((accel_g_axis->z == 1) && (accel_g_axis->sign == 1)) + { + temp_foc_data.z = temp_foc_data.z * -1; + } + } + + /* Typecasting into 16bit */ + avg_foc_data.x = (int16_t)(temp_foc_data.x); + avg_foc_data.y = (int16_t)(temp_foc_data.y); + avg_foc_data.z = (int16_t)(temp_foc_data.z); + + rslt = validate_foc_position(sens_list, accel_g_axis, avg_foc_data, dev); + } + } + + return rslt; +} + +/*! + * @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) +{ + /* Variable to define error */ + int8_t rslt; + + /* Structure to define the accelerometer configurations */ + struct bmi2_accel_config acc_cfg = { 0, 0, 0, 0 }; + + /* Variable to store status of advance power save */ + uint8_t aps = 0; + + /* Variable to store status of accelerometer enable */ + uint8_t acc_en = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (accel_g_value != NULL)) + { + /* Check for input validity */ + if ((((BMI2_ABS(accel_g_value->x)) + (BMI2_ABS(accel_g_value->y)) + (BMI2_ABS(accel_g_value->z))) == 1) && + ((accel_g_value->sign == 1) || (accel_g_value->sign == 0))) + { + rslt = verify_foc_position(BMI2_ACCEL, accel_g_value, dev); + if (rslt == BMI2_OK) + { + + /* Save accelerometer configurations, accelerometer + * enable status and advance power save status + */ + rslt = save_accel_foc_config(&acc_cfg, &aps, &acc_en, dev); + } + + /* Set configurations for FOC */ + if (rslt == BMI2_OK) + { + rslt = set_accel_foc_config(dev); + } + + /* Perform accelerometer FOC */ + if (rslt == BMI2_OK) + { + rslt = perform_accel_foc(accel_g_value, &acc_cfg, dev); + } + + /* Restore the saved configurations */ + if (rslt == BMI2_OK) + { + rslt = restore_accel_foc_config(&acc_cfg, aps, acc_en, dev); + } + } + else + { + rslt = BMI2_E_INVALID_INPUT; + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API performs Fast Offset Compensation for gyroscope. + */ +int8_t bmi2_perform_gyro_foc(struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Structure to define the gyroscope configurations */ + struct bmi2_gyro_config gyr_cfg = { 0, 0, 0, 0, 0, 0 }; + + /* Variable to store status of advance power save */ + uint8_t aps = 0; + + /* Variable to store status of gyroscope enable */ + uint8_t gyr_en = 0; + + /* Array of structure to store gyroscope data */ + struct bmi2_sens_axes_data gyr_value[128]; + + /* Structure to store gyroscope data temporarily */ + struct foc_temp_value temp = { 0, 0, 0 }; + + /* Variable to store status read from the status register */ + uint8_t reg_status = 0; + + /* Variable to define count */ + uint8_t loop = 0; + + /* Structure to store the offset values to be stored in the register */ + struct bmi2_sens_axes_data gyro_offset = { 0, 0, 0, 0 }; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + /* Argument2 is not applicable for gyro */ + rslt = verify_foc_position(BMI2_GYRO, 0, dev); + if (rslt == BMI2_OK) + { + /* Save gyroscope configurations, gyroscope enable + * status and advance power save status + */ + rslt = save_gyro_config(&gyr_cfg, &aps, &gyr_en, dev); + + /* Set configurations for gyroscope FOC */ + if (rslt == BMI2_OK) + { + rslt = set_gyro_foc_config(dev); + } + + /* Perform FOC */ + if (rslt == BMI2_OK) + { + for (loop = 0; loop < 128; loop++) + { + /* Giving a delay of more than 40ms since ODR is configured as 25Hz */ + dev->delay_us(50000); + + /* Get gyroscope data ready interrupt status */ + rslt = bmi2_get_status(®_status, dev); + + /* Read 128 samples of gyroscope data on data ready interrupt */ + if ((rslt == BMI2_OK) && (reg_status & BMI2_DRDY_GYR)) + { + rslt = read_gyro_xyz(&gyr_value[loop], dev); + if (rslt == BMI2_OK) + { + /* Store the data in a temporary structure */ + temp.x = temp.x + (int32_t)gyr_value[loop].x; + temp.y = temp.y + (int32_t)gyr_value[loop].y; + temp.z = temp.z + (int32_t)gyr_value[loop].z; + } + } + if (rslt != BMI2_OK) + { + break; + } + else if ((reg_status & BMI2_DRDY_GYR) != BMI2_DRDY_GYR) + { + rslt = BMI2_E_INVALID_STATUS; + break; + } + } + if (rslt == BMI2_OK) + { + /* Take average of x, y and z data for lesser + * noise. It is same as offset data since lsb/dps + * is same for both data and offset register + */ + gyro_offset.x = (int16_t)(temp.x / 128); + gyro_offset.y = (int16_t)(temp.y / 128); + gyro_offset.z = (int16_t)(temp.z / 128); + + /* Saturate gyroscope data since the offset + * registers are of 10 bit value where as the + * gyroscope data is of 16 bit value + */ + saturate_gyro_data(&gyro_offset); + + /* Invert the gyroscope offset data */ + invert_gyro_offset(&gyro_offset); + + /* Write offset data in the gyroscope offset + * compensation register + */ + rslt = bmi2_write_gyro_offset_comp_axes(&gyro_offset, dev); + } + + /* Enable gyroscope offset compensation */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_gyro_offset_comp(BMI2_ENABLE, dev); + } + + /* Restore the saved gyroscope configurations */ + if (rslt == BMI2_OK) + { + rslt = restore_gyro_config(&gyr_cfg, aps, gyr_en, dev); + } + } + } + } + + return rslt; +} + +/*! + * @brief This api is used for retrieving the activity recognition settings currently set. + */ +int8_t bmi2_get_act_recg_sett(struct act_recg_sett *sett, 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 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 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) + { + 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); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This api is used for setting the activity recognition settings. + */ +int8_t bmi2_set_act_recg_sett(const struct act_recg_sett *sett, 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 set flag */ + uint8_t feat_found; + + /* 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) + { + 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; + 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; +} + +/***************************************************************************/ + +/*! Local Function Definitions + ****************************************************************************/ + +/*! + * @brief This internal API writes the configuration file. + */ +static int8_t write_config_file(struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to update the configuration file index */ + uint16_t index = 0; + + /* config file size */ + uint16_t config_size = dev->config_size; + + /* Variable to get the remainder */ + uint8_t remain = (uint8_t)(config_size % dev->read_write_len); + + /* Variable to get the balance bytes */ + uint16_t bal_byte = 0; + + /* Variable to define temporary read/write length */ + uint16_t read_write_len = 0; + + /* Disable advanced power save mode */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + /* Disable loading of the configuration */ + rslt = set_config_load(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + if (!remain) + { + /* Write the configuration file */ + for (index = 0; (index < config_size) && (rslt == BMI2_OK); index += dev->read_write_len) + { + rslt = upload_file((dev->config_file_ptr + index), index, dev->read_write_len, dev); + } + } + else + { + /* Get the balance bytes */ + bal_byte = (uint16_t) config_size - (uint16_t) remain; + + /* Write the configuration file for the balancem bytes */ + for (index = 0; (index < bal_byte) && (rslt == BMI2_OK); index += dev->read_write_len) + { + rslt = upload_file((dev->config_file_ptr + index), index, dev->read_write_len, dev); + } + if (rslt == BMI2_OK) + { + /* Update length in a temporary variable */ + read_write_len = dev->read_write_len; + + /* Write the remaining bytes in 2 bytes length */ + dev->read_write_len = 2; + + /* Write the configuration file for the remaining bytes */ + for (index = bal_byte; + (index < config_size) && (rslt == BMI2_OK); + index += dev->read_write_len) + { + rslt = upload_file((dev->config_file_ptr + index), index, dev->read_write_len, dev); + } + + /* Restore the user set length back from the temporary variable */ + 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); + if (rslt == BMI2_OK) + { + /* Enable advanced power save mode */ + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + dev->delay_us(1000); + } + } + } + } + + return rslt; +} + +/*! + * @brief This internal API enables/disables the loading of the configuration + * file. + */ +static int8_t set_config_load(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store data */ + uint8_t reg_data = 0; + + rslt = bmi2_get_regs(BMI2_INIT_CTRL_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_CONF_LOAD_EN, enable); + rslt = bmi2_set_regs(BMI2_INIT_CTRL_ADDR, ®_data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This internal API loads the configuration file. + */ +static int8_t upload_file(const uint8_t *config_data, uint16_t index, uint16_t write_len, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to store address */ + uint8_t addr_array[2] = { 0 }; + + if (config_data != NULL) + { + /* Store 0 to 3 bits of address in first byte */ + addr_array[0] = (uint8_t)((index / 2) & 0x0F); + + /* Store 4 to 11 bits of address in the second byte */ + addr_array[1] = (uint8_t)((index / 2) >> 4); + + /* Write the 2 bytes of address in consecutive locations */ + rslt = bmi2_set_regs(BMI2_INIT_ADDR_0, addr_array, 2, dev); + if (rslt == BMI2_OK) + { + /* Burst write configuration file data corresponding to user set length */ + rslt = bmi2_set_regs(BMI2_INIT_DATA_ADDR, (uint8_t *)config_data, write_len, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + 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. + */ +static int8_t validate_bw_perf_mode(uint8_t *bandwidth, uint8_t *perf_mode, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Validate and auto-correct performance mode */ + rslt = check_boundary_val(perf_mode, BMI2_POWER_OPT_MODE, BMI2_PERF_OPT_MODE, dev); + if (rslt == BMI2_OK) + { + /* Validate and auto-correct bandwidth parameter */ + if (*perf_mode == BMI2_PERF_OPT_MODE) + { + /* Validate for continuous filter mode */ + rslt = check_boundary_val(bandwidth, BMI2_ACC_OSR4_AVG1, BMI2_ACC_CIC_AVG8, dev); + } + else + { + /* Validate for CIC averaging mode */ + rslt = check_boundary_val(bandwidth, BMI2_ACC_OSR4_AVG1, BMI2_ACC_RES_AVG128, dev); + } + } + + return rslt; +} + +/*! + * @brief This internal API validates ODR and range of the accelerometer set by + * the user. + */ +static int8_t validate_odr_range(uint8_t *odr, uint8_t *range, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Validate and auto correct ODR */ + rslt = check_boundary_val(odr, BMI2_ACC_ODR_0_78HZ, BMI2_ACC_ODR_1600HZ, dev); + if (rslt == BMI2_OK) + { + /* Validate and auto correct Range */ + rslt = check_boundary_val(range, BMI2_ACC_RANGE_2G, BMI2_ACC_RANGE_16G, 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. + */ +static int8_t validate_gyro_config(struct bmi2_gyro_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Validate and auto-correct performance mode */ + rslt = check_boundary_val(&config->filter_perf, BMI2_POWER_OPT_MODE, BMI2_PERF_OPT_MODE, dev); + if (rslt == BMI2_OK) + { + /* Validate and auto-correct bandwidth parameter */ + rslt = check_boundary_val(&config->bwp, BMI2_GYR_OSR4_MODE, BMI2_GYR_CIC_MODE, dev); + if (rslt == BMI2_OK) + { + /* Validate and auto-correct low power/high-performance parameter */ + rslt = check_boundary_val(&config->noise_perf, BMI2_POWER_OPT_MODE, BMI2_PERF_OPT_MODE, dev); + if (rslt == BMI2_OK) + { + /* Validate and auto-correct ODR parameter */ + rslt = check_boundary_val(&config->odr, BMI2_GYR_ODR_25HZ, BMI2_GYR_ODR_3200HZ, dev); + if (rslt == BMI2_OK) + { + /* Validate and auto-correct OIS range */ + rslt = check_boundary_val(&config->ois_range, BMI2_GYR_OIS_250, BMI2_GYR_OIS_2000, dev); + if (rslt == BMI2_OK) + { + /* Validate and auto-correct range parameter */ + rslt = check_boundary_val(&config->range, BMI2_GYR_RANGE_2000, BMI2_GYR_RANGE_125, dev); + } + } + } + } + } + + return rslt; +} + +/*! + * @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) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store data */ + uint8_t reg_data; + + /* Get error status of the set sensor configuration */ + rslt = bmi2_get_regs(BMI2_EVENT_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + reg_data = BMI2_GET_BITS(reg_data, BMI2_EVENT_FLAG); + switch (reg_data) + { + case BMI2_NO_ERROR: + rslt = BMI2_OK; + break; + case BMI2_ACC_ERROR: + rslt = BMI2_E_ACC_INVALID_CFG; + break; + case BMI2_GYR_ERROR: + rslt = BMI2_E_GYRO_INVALID_CFG; + break; + case BMI2_ACC_GYR_ERROR: + rslt = BMI2_E_ACC_GYR_INVALID_CFG; + break; + default: + break; + } + } + + return rslt; +} + +/*! + * @brief This internal API: + * 1) Enables/Disables auxiliary interface. + * 2) Sets auxiliary interface configurations like I2C address, manual/auto + * mode enable, manual burst read length, AUX burst read length and AUX read + * address. + * 3)It maps/un-maps data interrupts to that of hardware interrupt line. + */ +static int8_t set_aux_config(struct bmi2_aux_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Validate auxiliary configurations */ + rslt = validate_aux_config(config, dev); + if (rslt == BMI2_OK) + { + /* Enable/Disable auxiliary interface */ + rslt = set_aux_interface(config, dev); + if (rslt == BMI2_OK) + { + /* Set the auxiliary interface configurations */ + rslt = config_aux_interface(config, dev); + if (rslt == BMI2_OK) + { + /* Set read out offset and ODR */ + rslt = config_aux(config, dev); + } + } + } + + return rslt; +} + +/*! + * @brief This internal API enables/disables auxiliary interface. + */ +static int8_t set_aux_interface(const struct bmi2_aux_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store data */ + uint8_t reg_data; + + rslt = bmi2_get_regs(BMI2_IF_CONF_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_AUX_IF_EN, config->aux_en); + + /* Enable/Disable auxiliary interface */ + rslt = bmi2_set_regs(BMI2_IF_CONF_ADDR, ®_data, 1, dev); + } + + return rslt; +} + +/*! + * @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. + * + * @note Auxiliary sensor should not be busy when configuring aux_i2c_addr, + * man_rd_burst_len, aux_rd_burst_len and aux_rd_addr. + */ +static int8_t config_aux_interface(const struct bmi2_aux_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store data */ + uint8_t reg_data[2] = { 0 }; + + /* Variable to store status */ + uint8_t status = 0; + + /* Variable to define count */ + uint8_t count = 0; + + rslt = bmi2_get_regs(BMI2_AUX_DEV_ID_ADDR, reg_data, 2, dev); + if (rslt == BMI2_OK) + { + /* Set I2C address for AUX sensor */ + reg_data[0] = BMI2_SET_BITS(reg_data[0], BMI2_AUX_SET_I2C_ADDR, config->i2c_device_addr); + + /* Set the AUX IF to either manual or auto mode */ + reg_data[1] = BMI2_SET_BITS(reg_data[1], BMI2_AUX_MAN_MODE_EN, config->manual_en); + + /* Enables FCU write command on AUX IF for auxiliary sensors that need a trigger */ + reg_data[1] = BMI2_SET_BITS(reg_data[1], BMI2_AUX_FCU_WR_EN, config->fcu_write_en); + + /* Set the burst read length for manual mode */ + reg_data[1] = BMI2_SET_BITS(reg_data[1], BMI2_AUX_MAN_READ_BURST, config->man_rd_burst); + + /* Set the burst read length for data mode */ + reg_data[1] = BMI2_SET_BIT_POS0(reg_data[1], BMI2_AUX_READ_BURST, config->aux_rd_burst); + for (;;) + { + /* Check if auxiliary sensor is busy */ + rslt = bmi2_get_status(&status, dev); + if ((rslt == BMI2_OK) && (!(status & BMI2_AUX_BUSY))) + { + /* 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); + if (rslt == BMI2_OK) + { + /* If data mode */ + if (!config->manual_en) + { + /* Disable manual enable flag in device structure */ + dev->aux_man_en = 0; + + /* 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); + } + else + { + /* Enable manual enable flag in device structure */ + dev->aux_man_en = 1; + + /* Update manual read burst length in device structure */ + dev->aux_man_rd_burst_len = config->man_rd_burst; + } + } + + /* Break after setting the register */ + break; + } + + /* Increment count after every 10 seconds */ + dev->delay_us(10000); + count++; + + /* Break after 2 seconds if AUX still busy - since slowest ODR is 0.78Hz*/ + if (count > 20) + { + rslt = BMI2_E_AUX_BUSY; + break; + } + } + } + + return rslt; +} + +/*! + * @brief This internal API triggers read out offset and sets ODR of the + * auxiliary sensor. + */ +static int8_t config_aux(const struct bmi2_aux_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store data */ + uint8_t reg_data; + + rslt = bmi2_get_regs(BMI2_AUX_CONF_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + /* Trigger read out offset */ + reg_data = BMI2_SET_BITS(reg_data, BMI2_AUX_OFFSET_READ_OUT, config->offset); + + /* Set ODR */ + reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_AUX_ODR_EN, config->odr); + + /* Set auxiliary configuration register */ + rslt = bmi2_set_regs(BMI2_AUX_CONF_ADDR, ®_data, 1, dev); + dev->delay_us(1000); + } + + return rslt; +} + +/*! + * @brief This internal API checks the busy status of auxiliary sensor and sets + * the auxiliary register addresses when not busy. + */ +static int8_t set_if_aux_not_busy(uint8_t reg_addr, uint8_t reg_data, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to get status of AUX_BUSY */ + uint8_t status = 0; + + /* Variable to define count for time-out */ + uint8_t count = 0; + + for (;;) + { + /* Check if AUX is busy */ + rslt = bmi2_get_status(&status, dev); + + /* Set the registers if not busy */ + if ((rslt == BMI2_OK) && (!(status & BMI2_AUX_BUSY))) + { + rslt = bmi2_set_regs(reg_addr, ®_data, 1, dev); + dev->delay_us(1000); + + /* Break after setting the register */ + break; + } + + /* Increment count after every 10 seconds */ + dev->delay_us(10000); + count++; + + /* Break after 2 seconds if AUX still busy - since slowest ODR is 0.78Hz*/ + if (count > 20) + { + rslt = BMI2_E_AUX_BUSY; + break; + } + } + + return rslt; +} + +/*! + * @brief This internal API validates auxiliary configuration set by the user. + */ +static int8_t validate_aux_config(struct bmi2_aux_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Validate ODR for auxiliary sensor */ + rslt = check_boundary_val(&config->odr, BMI2_AUX_ODR_0_78HZ, BMI2_AUX_ODR_800HZ, dev); + + 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) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to store data */ + uint8_t data_array[2] = { 0 }; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (config != NULL)) + { + /* Read the sensor configuration details */ + rslt = bmi2_get_regs(BMI2_ACC_CONF_ADDR, data_array, 2, dev); + if (rslt == BMI2_OK) + { + /* Get accelerometer performance mode */ + config->filter_perf = BMI2_GET_BITS(data_array[0], BMI2_ACC_FILTER_PERF_MODE); + + /* Get accelerometer bandwidth */ + config->bwp = BMI2_GET_BITS(data_array[0], BMI2_ACC_BW_PARAM); + + /* Get accelerometer ODR */ + config->odr = BMI2_GET_BIT_POS0(data_array[0], BMI2_ACC_ODR); + + /* Get accelerometer range */ + config->range = BMI2_GET_BIT_POS0(data_array[1], BMI2_ACC_RANGE); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @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) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to store data */ + uint8_t data_array[2] = { 0 }; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (config != NULL)) + { + /* Read the sensor configuration details */ + rslt = bmi2_get_regs(BMI2_GYR_CONF_ADDR, data_array, 2, dev); + if (rslt == BMI2_OK) + { + /* Get gyroscope performance mode */ + config->filter_perf = BMI2_GET_BITS(data_array[0], BMI2_GYR_FILTER_PERF_MODE); + + /* Get gyroscope noise performance mode */ + config->noise_perf = BMI2_GET_BITS(data_array[0], BMI2_GYR_NOISE_PERF_MODE); + + /* Get gyroscope bandwidth */ + config->bwp = BMI2_GET_BITS(data_array[0], BMI2_GYR_BW_PARAM); + + /* Get gyroscope ODR */ + config->odr = BMI2_GET_BIT_POS0(data_array[0], BMI2_GYR_ODR); + + /* Get gyroscope OIS range */ + config->ois_range = BMI2_GET_BITS(data_array[1], BMI2_GYR_OIS_RANGE); + + /* Get gyroscope range */ + config->range = BMI2_GET_BIT_POS0(data_array[1], BMI2_GYR_RANGE); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This internal API: + * 1) Gets the status of auxiliary interface enable. + * 2) Gets auxiliary interface configurations like I2C address, manual/auto + * mode enable, manual burst read length, AUX burst read length and AUX read + * address. + * 3) Gets ODR and offset. + */ +static int8_t get_aux_config(struct bmi2_aux_config *config, const struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (config != NULL)) + { + /* Get enable status of auxiliary interface */ + rslt = get_aux_interface(config, dev); + if (rslt == BMI2_OK) + { + /* Get the auxiliary interface configurations */ + rslt = get_aux_interface_config(config, dev); + if (rslt == BMI2_OK) + { + /* Get read out offset and ODR */ + rslt = get_aux_cfg(config, dev); + } + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + 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) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store data */ + uint8_t reg_data; + + /* Get the enable status of auxiliary interface */ + rslt = bmi2_get_regs(BMI2_IF_CONF_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + config->aux_en = BMI2_GET_BITS(reg_data, BMI2_AUX_IF_EN); + } + + return rslt; +} + +/*! + * @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) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store data */ + uint8_t reg_data[2] = { 0 }; + + rslt = bmi2_get_regs(BMI2_AUX_DEV_ID_ADDR, reg_data, 2, dev); + if (rslt == BMI2_OK) + { + /* Get I2C address for auxiliary sensor */ + config->i2c_device_addr = BMI2_GET_BITS(reg_data[0], BMI2_AUX_SET_I2C_ADDR); + + /* Get the AUX IF to either manual or auto mode */ + config->manual_en = BMI2_GET_BITS(reg_data[1], BMI2_AUX_MAN_MODE_EN); + + /* Enables FCU write command on AUX IF for auxiliary sensors that need a trigger */ + config->fcu_write_en = BMI2_GET_BITS(reg_data[1], BMI2_AUX_FCU_WR_EN); + + /* Get the burst read length for manual mode */ + config->man_rd_burst = BMI2_GET_BITS(reg_data[1], BMI2_AUX_MAN_READ_BURST); + + /* Get the burst read length for data mode */ + config->aux_rd_burst = BMI2_GET_BIT_POS0(reg_data[1], BMI2_AUX_READ_BURST); + + /* If data mode, get the read address of the auxiliary sensor from where data is to be read */ + if (!config->manual_en) + { + rslt = bmi2_get_regs(BMI2_AUX_RD_ADDR, &config->read_addr, 1, dev); + } + } + + return rslt; +} + +/*! + * @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) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store data */ + uint8_t reg_data; + + rslt = bmi2_get_regs(BMI2_AUX_CONF_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + /* Get read out offset */ + config->offset = BMI2_GET_BITS(reg_data, BMI2_AUX_OFFSET_READ_OUT); + + /* Get ODR */ + config->odr = BMI2_GET_BIT_POS0(reg_data, BMI2_AUX_ODR_EN); + } + + 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. + */ +static int8_t map_feat_int(uint8_t *reg_data_array, enum bmi2_hw_int_pin int_pin, uint8_t int_mask) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Check for NULL error */ + if (reg_data_array != NULL) + { + /* Check validity on interrupt pin selection */ + if (int_pin < BMI2_INT_PIN_MAX) + { + switch (int_pin) + { + case BMI2_INT_NONE: + + /* Un-Map the corresponding feature interrupt to interrupt pin 1 and 2 */ + reg_data_array[0] &= ~(int_mask); + reg_data_array[1] &= ~(int_mask); + break; + case BMI2_INT1: + + /* Map the corresponding feature interrupt to interrupt pin 1 */ + reg_data_array[0] |= int_mask; + + /* Un-map the corresponding feature interrupt to interrupt pin 2 */ + reg_data_array[1] &= ~(int_mask); + break; + case BMI2_INT2: + + /* Map the corresponding feature interrupt to interrupt pin 2 */ + reg_data_array[1] |= int_mask; + + /* Un-map the corresponding feature interrupt to interrupt pin 1 */ + reg_data_array[0] &= ~(int_mask); + break; + case BMI2_INT_BOTH: + + /* Map the corresponding feature interrupt to interrupt pin 1 and 2 */ + reg_data_array[0] |= int_mask; + reg_data_array[1] |= int_mask; + break; + default: + break; + } + } + else + { + /* Return error if invalid pin selection */ + rslt = BMI2_E_INVALID_INT_PIN; + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @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) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define data stored in register */ + uint8_t reg_data[BMI2_ACC_GYR_NUM_BYTES] = { 0 }; + + /* Read the sensor data */ + rslt = bmi2_get_regs(reg_addr, reg_data, BMI2_ACC_GYR_NUM_BYTES, dev); + if (rslt == BMI2_OK) + { + /* Get accelerometer data from the register */ + get_acc_gyr_data(data, reg_data); + + /* Get the re-mapped accelerometer data */ + get_remapped_data(data, dev); + } + + return rslt; +} + +/*! + * @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) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define data stored in register */ + uint8_t reg_data[BMI2_ACC_GYR_NUM_BYTES] = { 0 }; + + /* Read the sensor data */ + rslt = bmi2_get_regs(reg_addr, reg_data, BMI2_ACC_GYR_NUM_BYTES, dev); + if (rslt == BMI2_OK) + { + /* Get gyroscope data from the register */ + get_acc_gyr_data(data, reg_data); + + /* Get the compensated gyroscope data */ + comp_gyro_cross_axis_sensitivity(data, dev); + + /* Get the re-mapped gyroscope data */ + get_remapped_data(data, dev); + + } + + return rslt; +} + +/*! + * @brief This internal API gets the accelerometer/gyroscope data. + */ +static void get_acc_gyr_data(struct bmi2_sens_axes_data *data, const uint8_t *reg_data) +{ + /* Variables to store msb value */ + uint8_t msb; + + /* Variables to store lsb value */ + uint8_t lsb; + + /* Variables to store both msb and lsb value */ + uint16_t msb_lsb; + + /* Variables to define index */ + uint8_t index = 0; + + /* Read x-axis data */ + lsb = reg_data[index++]; + msb = reg_data[index++]; + msb_lsb = ((uint16_t) msb << 8) | (uint16_t) lsb; + data->x = (int16_t) msb_lsb; + + /* Read y-axis data */ + lsb = reg_data[index++]; + msb = reg_data[index++]; + msb_lsb = ((uint16_t) msb << 8) | (uint16_t) lsb; + data->y = (int16_t) msb_lsb; + + /* Read z-axis data */ + lsb = reg_data[index++]; + msb = reg_data[index++]; + msb_lsb = ((uint16_t) msb << 8) | (uint16_t) lsb; + data->z = (int16_t) msb_lsb; +} + +/*! + * @brief This internal API gets the re-mapped accelerometer/gyroscope data. + */ +static void get_remapped_data(struct bmi2_sens_axes_data *data, const struct bmi2_dev *dev) +{ + /* Array to defined the re-mapped sensor data */ + int16_t remap_data[3] = { 0 }; + + /* 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); +} + +/*! + * @brief This internal API reads the user-defined bytes of data from the given + * register address of auxiliary sensor in manual mode. + */ +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) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Array to store the register data */ + uint8_t reg_data[15] = { 0 }; + + /* Variable to define number of bytes to read */ + uint16_t read_length = 0; + + /* Variable to define loop */ + uint8_t loop = 0; + + /* Variable to define counts to get the correct array index */ + uint8_t count = 0; + + /* Variable to define index for the array */ + uint8_t idx = 0; + + while (len > 0) + { + /* Set the read address if AUX is not busy */ + rslt = set_if_aux_not_busy(BMI2_AUX_RD_ADDR, reg_addr, dev); + if (rslt == BMI2_OK) + { + /* 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); + if (rslt == BMI2_OK) + { + /* Get number of bytes to be read */ + if (len < burst_len) + { + read_length = (uint8_t) len; + } + else + { + read_length = burst_len; + } + + /* Update array index and store the data */ + for (loop = 0; loop < read_length; loop++) + { + idx = loop + count; + aux_data[idx] = reg_data[loop]; + } + } + } + + /* Update address for the next read */ + reg_addr += burst_len; + + /* Update count for the array index */ + count += burst_len; + + /* Update no of bytes left to read */ + len -= read_length; + } + + return rslt; +} + +/*! + * @brief This internal API writes AUX write address and the user-defined bytes + * of data to the AUX sensor in manual mode. + * + * @note Change of BMI2_AUX_WR_ADDR is only allowed if AUX is not busy. + */ +static int8_t write_aux_data(uint8_t reg_addr, uint8_t reg_data, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Write data to be written to the AUX sensor in bmi2 register */ + rslt = bmi2_set_regs(BMI2_AUX_WR_DATA_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + /* Write the AUX address where data is to be stored when AUX is not busy */ + rslt = set_if_aux_not_busy(BMI2_AUX_WR_ADDR, reg_addr, dev); + } + + return rslt; +} + +/*! + * @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) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variables to define loop */ + uint8_t count = 0; + + /* Variables to define index */ + uint8_t index = 0; + + /* Array to define data stored in register */ + uint8_t reg_data[BMI2_AUX_NUM_BYTES] = { 0 }; + + /* Check if data mode */ + if (!dev->aux_man_en) + { + /* Read the auxiliary sensor data */ + rslt = bmi2_get_regs(BMI2_AUX_X_LSB_ADDR, reg_data, BMI2_AUX_NUM_BYTES, dev); + if (rslt == BMI2_OK) + { + /* Get the 8 bytes of auxiliary data */ + do + { + *(aux_data + count++) = *(reg_data + index++); + } while (count < BMI2_AUX_NUM_BYTES); + } + } + else + { + rslt = BMI2_E_AUX_INVALID_CFG; + } + + return rslt; +} + +/*! + * @brief This internal API maps the actual burst read length with that of the + * register value set by user. + */ +static int8_t map_read_len(uint8_t *len, const struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Get the burst read length against the values set by the user */ + switch (dev->aux_man_rd_burst_len) + { + case BMI2_AUX_READ_LEN_0: + *len = 1; + break; + case BMI2_AUX_READ_LEN_1: + *len = 2; + break; + case BMI2_AUX_READ_LEN_2: + *len = 6; + break; + case BMI2_AUX_READ_LEN_3: + *len = 8; + break; + default: + rslt = BMI2_E_AUX_INVALID_CFG; + break; + } + + 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. + */ +static int8_t parse_fifo_accel_len(uint16_t *start_idx, + uint16_t *len, + const uint16_t *acc_count, + const struct bmi2_fifo_frame *fifo) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Data start index */ + (*start_idx) = fifo->acc_byte_start_idx; + + /* If only accelerometer is enabled */ + if (fifo->data_enable == BMI2_FIFO_ACC_EN) + { + /* Number of bytes to be read */ + (*len) = (uint16_t)((*acc_count) * BMI2_FIFO_ACC_LENGTH); + } + /* If only accelerometer and auxiliary are enabled */ + else if (fifo->data_enable == (BMI2_FIFO_ACC_EN | BMI2_FIFO_AUX_EN)) + { + /* Number of bytes to be read */ + (*len) = (uint16_t)((*acc_count) * BMI2_FIFO_ACC_AUX_LENGTH); + } + /* If only accelerometer and gyroscope are enabled */ + else if (fifo->data_enable == (BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN)) + { + /* Number of bytes to be read */ + (*len) = (uint16_t)((*acc_count) * BMI2_FIFO_ACC_GYR_LENGTH); + } + /* If only accelerometer, gyroscope and auxiliary are enabled */ + else if (fifo->data_enable == (BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN | BMI2_FIFO_AUX_EN)) + { + /* Number of bytes to be read */ + (*len) = (uint16_t)((*acc_count) * BMI2_FIFO_ALL_LENGTH); + } + else + { + /* Move the data index to the last byte to mark completion when + * no sensors or sensors apart from accelerometer are enabled + */ + (*start_idx) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + } + + /* If more data is requested than available */ + if ((*len) > fifo->length) + { + (*len) = fifo->length; + } + + return rslt; +} + +/*! + * @brief This internal API is used to parse the accelerometer data from the + * FIFO in header mode. + */ +static int8_t extract_accel_header_mode(struct bmi2_sens_axes_data *acc, + uint16_t *accel_length, + struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Variable to define header frame */ + uint8_t frame_header = 0; + + /* Variable to index the data bytes */ + uint16_t data_index; + + /* Variable to index accelerometer frames */ + uint16_t accel_index = 0; + + /* Variable to indicate accelerometer frames read */ + uint16_t frame_to_read = *accel_length; + + for (data_index = fifo->acc_byte_start_idx; data_index < fifo->length;) + { + /* Get frame header byte */ + frame_header = fifo->data[data_index] & BMI2_FIFO_TAG_INTR_MASK; + + /* Parse virtual header if S4S is enabled */ + parse_if_virtual_header(&frame_header, &data_index, fifo); + + /* Index shifted to next byte where data starts */ + data_index++; + switch (frame_header) + { + /* If header defines accelerometer frame */ + case BMI2_FIFO_HEADER_ACC_FRM: + case BMI2_FIFO_HEADER_AUX_ACC_FRM: + case BMI2_FIFO_HEADER_GYR_ACC_FRM: + case BMI2_FIFO_HEADER_ALL_FRM: + + /* Unpack from normal frames */ + rslt = unpack_accel_frame(acc, &data_index, &accel_index, frame_header, fifo, dev); + 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 only 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 = unpack_sensortime_frame(&data_index, fifo); + break; + + /* If header defines skip frame */ + case BMI2_FIFO_HEADER_SKIP_FRM: + rslt = unpack_skipped_frame(&data_index, 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; + case BMI2_FIFO_VIRT_ACT_RECOG_FRM: + rslt = move_next_frame(&data_index, BMI2_FIFO_VIRT_ACT_DATA_LENGTH, fifo); + 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; + } + + /* Break if Number of frames to be read is complete or FIFO is mpty */ + if ((frame_to_read == accel_index) || (rslt == BMI2_W_FIFO_EMPTY)) + { + break; + } + } + + /* Update the accelerometer frame index */ + (*accel_length) = accel_index; + + /* Update the accelerometer byte index */ + fifo->acc_byte_start_idx = data_index; + + return rslt; +} + +/*! + * @brief This internal API is used to parse the accelerometer data from the + * FIFO data in both header and header-less mode. It updates the current data + * byte to be parsed. + */ +static int8_t unpack_accel_frame(struct bmi2_sens_axes_data *acc, + uint16_t *idx, + uint16_t *acc_idx, + uint8_t frame, + const struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + switch (frame) + { + /* If frame contains only accelerometer data */ + case BMI2_FIFO_HEADER_ACC_FRM: + case BMI2_FIFO_HEAD_LESS_ACC_FRM: + + /* Partially read, then skip the data */ + if (((*idx) + fifo->acc_frm_len) > fifo->length) + { + /* Update the data index as complete*/ + (*idx) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + } + + /* Get the accelerometer data */ + unpack_accel_data(&acc[(*acc_idx)], *idx, fifo, dev); + + /* Update data index */ + (*idx) = (*idx) + BMI2_FIFO_ACC_LENGTH; + + /* Get virtual sensor time if S4S is enabled */ + if (dev->sens_en_stat & BMI2_EXT_SENS_SEL) + { + unpack_virt_sensor_time(&acc[(*acc_idx)], idx, fifo); + } + + /* Update accelerometer frame index */ + (*acc_idx)++; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + + /* If frame contains accelerometer and gyroscope data */ + case BMI2_FIFO_HEADER_GYR_ACC_FRM: + case BMI2_FIFO_HEAD_LESS_GYR_ACC_FRM: + + /* Partially read, then skip the data */ + if (((*idx) + fifo->acc_gyr_frm_len) > fifo->length) + { + /* Move the data index to the last byte */ + (*idx) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + } + + /* Get the accelerometer data */ + unpack_accel_data(&acc[(*acc_idx)], ((*idx) + BMI2_FIFO_GYR_LENGTH), fifo, dev); + + /* Update data index */ + (*idx) = (*idx) + BMI2_FIFO_ACC_GYR_LENGTH; + + /* Get virtual sensor time if S4S is enabled */ + if (dev->sens_en_stat & BMI2_EXT_SENS_SEL) + { + unpack_virt_sensor_time(&acc[(*acc_idx)], idx, fifo); + } + + /* Update accelerometer frame index */ + (*acc_idx)++; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + + /* If frame contains accelerometer and auxiliary data */ + case BMI2_FIFO_HEADER_AUX_ACC_FRM: + case BMI2_FIFO_HEAD_LESS_AUX_ACC_FRM: + + /* Partially read, then skip the data */ + if (((*idx) + fifo->acc_aux_frm_len) > fifo->length) + { + /* Move the data index to the last byte */ + (*idx) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + } + + /* Get the accelerometer data */ + unpack_accel_data(&acc[(*acc_idx)], ((*idx) + BMI2_FIFO_AUX_LENGTH), fifo, dev); + + /* Update data index */ + (*idx) = (*idx) + BMI2_FIFO_ACC_AUX_LENGTH; + + /* Get virtual sensor time if S4S is enabled */ + if (dev->sens_en_stat & BMI2_EXT_SENS_SEL) + { + unpack_virt_sensor_time(&acc[(*acc_idx)], idx, fifo); + } + + /* Update accelerometer frame index */ + (*acc_idx)++; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + + /* If frame contains accelerometer, gyroscope and auxiliary data */ + case BMI2_FIFO_HEADER_ALL_FRM: + case BMI2_FIFO_HEAD_LESS_ALL_FRM: + + /* Partially read, then skip the data*/ + if ((*idx + fifo->all_frm_len) > fifo->length) + { + /* Move the data index to the last byte */ + (*idx) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + } + + /* Get the accelerometer data */ + unpack_accel_data(&acc[(*acc_idx)], ((*idx) + BMI2_FIFO_GYR_AUX_LENGTH), fifo, dev); + + /* Update data index */ + (*idx) = (*idx) + BMI2_FIFO_ALL_LENGTH; + + /* Get virtual sensor time if S4S is enabled */ + if (dev->sens_en_stat & BMI2_EXT_SENS_SEL) + { + unpack_virt_sensor_time(&acc[(*acc_idx)], idx, fifo); + } + + /* Update accelerometer frame index */ + (*acc_idx)++; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + + /* If frame contains gyroscope and auxiliary data */ + case BMI2_FIFO_HEADER_AUX_GYR_FRM: + case BMI2_FIFO_HEAD_LESS_GYR_AUX_FRM: + + /* Update data index */ + (*idx) = (*idx) + fifo->aux_gyr_frm_len; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + + /* If frame contains only auxiliary data */ + case BMI2_FIFO_HEADER_AUX_FRM: + case BMI2_FIFO_HEAD_LESS_AUX_FRM: + + /* Update data index */ + (*idx) = (*idx) + fifo->aux_frm_len; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + + /* If frame contains only gyroscope data */ + case BMI2_FIFO_HEADER_GYR_FRM: + case BMI2_FIFO_HEAD_LESS_GYR_FRM: + + /* Update data index */ + (*idx) = (*idx) + fifo->gyr_frm_len; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + default: + + /* Move the data index to the last byte in case of invalid values */ + (*idx) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + } + + return rslt; +} + +/*! + * @brief This internal API is used to parse accelerometer data from the + * FIFO data. + */ +static void unpack_accel_data(struct bmi2_sens_axes_data *acc, + uint16_t data_start_index, + const struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev) +{ + /* Variables to store LSB value */ + uint16_t data_lsb; + + /* 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++]; + acc->x = (int16_t)((data_msb << 8) | data_lsb); + + /* Accelerometer raw y data */ + data_lsb = fifo->data[data_start_index++]; + data_msb = fifo->data[data_start_index++]; + acc->y = (int16_t)((data_msb << 8) | data_lsb); + + /* Accelerometer raw z data */ + data_lsb = fifo->data[data_start_index++]; + 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); +} + +/*! + * @brief This internal API computes the number of bytes of gyroscope FIFO data + * which is to be parsed in header-less mode. + */ +static int8_t parse_fifo_gyro_len(uint16_t *start_idx, + uint16_t(*len), + const uint16_t *gyr_count, + const struct bmi2_fifo_frame *fifo) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Data start index */ + (*start_idx) = fifo->gyr_byte_start_idx; + + /* If only gyroscope is enabled */ + if (fifo->data_enable == BMI2_FIFO_GYR_EN) + { + /* Number of bytes to be read */ + (*len) = (uint16_t)((*gyr_count) * BMI2_FIFO_GYR_LENGTH); + } + /* If only gyroscope and auxiliary are enabled */ + else if (fifo->data_enable == (BMI2_FIFO_GYR_EN | BMI2_FIFO_AUX_EN)) + { + /* Number of bytes to be read */ + (*len) = (uint16_t)((*gyr_count) * BMI2_FIFO_GYR_AUX_LENGTH); + } + /* If only accelerometer and gyroscope are enabled */ + else if (fifo->data_enable == (BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN)) + { + /* Number of bytes to be read */ + (*len) = (uint16_t)((*gyr_count) * BMI2_FIFO_ACC_GYR_LENGTH); + } + /* If only accelerometer, gyroscope and auxiliary are enabled */ + else if (fifo->data_enable == (BMI2_FIFO_GYR_EN | BMI2_FIFO_AUX_EN | BMI2_FIFO_ACC_EN)) + { + /* Number of bytes to be read */ + (*len) = (uint16_t)((*gyr_count) * BMI2_FIFO_ALL_LENGTH); + } + else + { + /* Move the data index to the last byte to mark completion when + * no sensors or sensors apart from gyroscope are enabled + */ + (*start_idx) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + } + + /* If more data is requested than available */ + if (((*len)) > fifo->length) + { + (*len) = fifo->length; + } + + return rslt; +} + +/*! + * @brief This internal API is used to parse the gyroscope data from the + * FIFO data in header mode. + */ +static int8_t extract_gyro_header_mode(struct bmi2_sens_axes_data *gyr, + uint16_t *gyro_length, + struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Variable to define header frame */ + uint8_t frame_header = 0; + + /* Variable to index the data bytes */ + uint16_t data_index; + + /* Variable to index gyroscope frames */ + uint16_t gyro_index = 0; + + /* Variable to indicate gyroscope frames read */ + uint16_t frame_to_read = (*gyro_length); + + for (data_index = fifo->gyr_byte_start_idx; data_index < fifo->length;) + { + /* Get frame header byte */ + frame_header = fifo->data[data_index] & BMI2_FIFO_TAG_INTR_MASK; + + /* Parse virtual header if S4S is enabled */ + parse_if_virtual_header(&frame_header, &data_index, fifo); + + /* Index shifted to next byte where data starts */ + data_index++; + switch (frame_header) + { + /* If header defines gyroscope frame */ + case BMI2_FIFO_HEADER_GYR_FRM: + case BMI2_FIFO_HEADER_GYR_ACC_FRM: + case BMI2_FIFO_HEADER_AUX_GYR_FRM: + case BMI2_FIFO_HEADER_ALL_FRM: + + /* Unpack from normal frames */ + rslt = unpack_gyro_frame(gyr, &data_index, &gyro_index, frame_header, fifo, dev); + break; + + /* If header defines only accelerometer frame */ + case BMI2_FIFO_HEADER_ACC_FRM: + rslt = move_next_frame(&data_index, fifo->acc_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 only auxiliary and accelerometer frame */ + case BMI2_FIFO_HEADER_AUX_ACC_FRM: + rslt = move_next_frame(&data_index, fifo->acc_aux_frm_len, fifo); + break; + + /* If header defines sensor time frame */ + case BMI2_FIFO_HEADER_SENS_TIME_FRM: + rslt = unpack_sensortime_frame(&data_index, fifo); + break; + + /* If header defines skip frame */ + case BMI2_FIFO_HEADER_SKIP_FRM: + rslt = unpack_skipped_frame(&data_index, 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 */ + data_index = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + case BMI2_FIFO_VIRT_ACT_RECOG_FRM: + rslt = move_next_frame(&data_index, BMI2_FIFO_VIRT_ACT_DATA_LENGTH, fifo); + 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; + } + + /* Break if number of frames to be read is complete or FIFO is empty */ + if ((frame_to_read == gyro_index) || (rslt == BMI2_W_FIFO_EMPTY)) + { + break; + } + } + + /* Update the gyroscope frame index */ + (*gyro_length) = gyro_index; + + /* Update the gyroscope byte index */ + fifo->gyr_byte_start_idx = data_index; + + return rslt; +} + +/*! + * @brief This internal API is used to parse the gyroscope data from the FIFO + * data in both header and header-less mode. It updates the current data byte to + * be parsed. + */ +static int8_t unpack_gyro_frame(struct bmi2_sens_axes_data *gyr, + uint16_t *idx, + uint16_t *gyr_idx, + uint8_t frame, + const struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + switch (frame) + { + /* If frame contains only gyroscope data */ + case BMI2_FIFO_HEADER_GYR_FRM: + case BMI2_FIFO_HEAD_LESS_GYR_FRM: + + /* Partially read, then skip the data */ + if (((*idx) + fifo->gyr_frm_len) > fifo->length) + { + /* Update the data index as complete*/ + (*idx) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + } + + /* Get the gyroscope data */ + unpack_gyro_data(&gyr[(*gyr_idx)], *idx, fifo, dev); + + /* Update data index */ + (*idx) = (*idx) + BMI2_FIFO_GYR_LENGTH; + + /* Get virtual sensor time if S4S is enabled */ + if (dev->sens_en_stat & BMI2_EXT_SENS_SEL) + { + unpack_virt_sensor_time(&gyr[(*gyr_idx)], idx, fifo); + } + + /* Update gyroscope frame index */ + (*gyr_idx)++; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + + /* If frame contains accelerometer and gyroscope data */ + case BMI2_FIFO_HEADER_GYR_ACC_FRM: + case BMI2_FIFO_HEAD_LESS_GYR_ACC_FRM: + + /* Partially read, then skip the data */ + if (((*idx) + fifo->acc_gyr_frm_len) > fifo->length) + { + /* Move the data index to the last byte */ + (*idx) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + } + + /* Get the gyroscope data */ + unpack_gyro_data(&gyr[(*gyr_idx)], (*idx), fifo, dev); + + /* Update data index */ + (*idx) = (*idx) + BMI2_FIFO_ACC_GYR_LENGTH; + + /* Get virtual sensor time if S4S is enabled */ + if (dev->sens_en_stat & BMI2_EXT_SENS_SEL) + { + unpack_virt_sensor_time(&gyr[(*gyr_idx)], idx, fifo); + } + + /* Update gyroscope frame index */ + (*gyr_idx)++; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + + /* If frame contains gyroscope and auxiliary data */ + case BMI2_FIFO_HEADER_AUX_GYR_FRM: + case BMI2_FIFO_HEAD_LESS_GYR_AUX_FRM: + + /* Partially read, then skip the data */ + if (((*idx) + fifo->aux_gyr_frm_len) > fifo->length) + { + /* Move the data index to the last byte */ + (*idx) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + } + + /* Get the gyroscope data */ + unpack_gyro_data(&gyr[(*gyr_idx)], ((*idx) + BMI2_FIFO_AUX_LENGTH), fifo, dev); + + /* Update data index */ + (*idx) = (*idx) + BMI2_FIFO_GYR_AUX_LENGTH; + + /* Get virtual sensor time if S4S is enabled */ + if (dev->sens_en_stat & BMI2_EXT_SENS_SEL) + { + unpack_virt_sensor_time(&gyr[(*gyr_idx)], idx, fifo); + } + + /* Update gyroscope frame index */ + (*gyr_idx)++; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + + /* If frame contains accelerometer, gyroscope and auxiliary data */ + case BMI2_FIFO_HEADER_ALL_FRM: + case BMI2_FIFO_HEAD_LESS_ALL_FRM: + + /* Partially read, then skip the data*/ + if ((*idx + fifo->all_frm_len) > fifo->length) + { + /* Move the data index to the last byte */ + (*idx) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + } + + /* Get the gyroscope data */ + unpack_gyro_data(&gyr[(*gyr_idx)], ((*idx) + BMI2_FIFO_AUX_LENGTH), fifo, dev); + + /* Update data index */ + (*idx) = (*idx) + BMI2_FIFO_ALL_LENGTH; + + /* Get virtual sensor time if S4S is enabled */ + if (dev->sens_en_stat & BMI2_EXT_SENS_SEL) + { + unpack_virt_sensor_time(&gyr[(*gyr_idx)], idx, fifo); + } + + /* Update gyroscope frame index */ + (*gyr_idx)++; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + + /* If frame contains accelerometer and auxiliary data */ + case BMI2_FIFO_HEADER_AUX_ACC_FRM: + case BMI2_FIFO_HEAD_LESS_AUX_ACC_FRM: + + /* Update data index */ + (*idx) = (*idx) + fifo->acc_aux_frm_len; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + + /* If frame contains only auxiliary data */ + case BMI2_FIFO_HEADER_AUX_FRM: + case BMI2_FIFO_HEAD_LESS_AUX_FRM: + + /* Update data index */ + (*idx) = (*idx) + fifo->aux_frm_len; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + + /* If frame contains only accelerometer data */ + case BMI2_FIFO_HEADER_ACC_FRM: + case BMI2_FIFO_HEAD_LESS_ACC_FRM: + + /* Update data index */ + (*idx) = (*idx) + fifo->acc_frm_len; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + default: + + /* Move the data index to the last byte in case of invalid values */ + (*idx) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + } + + return rslt; +} + +/*! + * @brief This internal API is used to parse gyroscope data from the FIFO data. + */ +static void unpack_gyro_data(struct bmi2_sens_axes_data *gyr, + uint16_t data_start_index, + const struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev) +{ + /* Variables to store LSB value */ + uint16_t data_lsb; + + /* 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++]; + gyr->x = (int16_t)((data_msb << 8) | data_lsb); + + /* Gyroscope raw y data */ + data_lsb = fifo->data[data_start_index++]; + data_msb = fifo->data[data_start_index++]; + gyr->y = (int16_t)((data_msb << 8) | data_lsb); + + /* Gyroscope raw z data */ + data_lsb = fifo->data[data_start_index++]; + data_msb = fifo->data[data_start_index++]; + gyr->z = (int16_t)((data_msb << 8) | data_lsb); + + /* 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); +} + +/*! + * @brief This API computes the number of bytes of auxiliary FIFO data which is + * to be parsed in header-less mode. + */ +static int8_t parse_fifo_aux_len(uint16_t *start_idx, + uint16_t(*len), + const uint16_t *aux_count, + const struct bmi2_fifo_frame *fifo) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Data start index */ + *start_idx = fifo->aux_byte_start_idx; + + /* If only auxiliary is enabled */ + if (fifo->data_enable == BMI2_FIFO_AUX_EN) + { + /* Number of bytes to be read */ + (*len) = (uint16_t)((*aux_count) * BMI2_FIFO_AUX_LENGTH); + } + /* If only accelerometer and auxiliary are enabled */ + else if (fifo->data_enable == (BMI2_FIFO_AUX_EN | BMI2_FIFO_ACC_EN)) + { + /* Number of bytes to be read */ + (*len) = (uint16_t)((*aux_count) * BMI2_FIFO_ACC_AUX_LENGTH); + } + /* If only accelerometer and gyroscope are enabled */ + else if (fifo->data_enable == (BMI2_FIFO_AUX_EN | BMI2_FIFO_GYR_EN)) + { + /* Number of bytes to be read */ + (*len) = (uint16_t)((*aux_count) * BMI2_FIFO_GYR_AUX_LENGTH); + } + /* If only accelerometer, gyroscope and auxiliary are enabled */ + else if (fifo->data_enable == (BMI2_FIFO_AUX_EN | BMI2_FIFO_GYR_EN | BMI2_FIFO_ACC_EN)) + { + /* Number of bytes to be read */ + (*len) = (uint16_t)((*aux_count) * BMI2_FIFO_ALL_LENGTH); + } + else + { + /* Move the data index to the last byte to mark completion when + * no sensors or sensors apart from gyroscope are enabled + */ + (*start_idx) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + } + + /* If more data is requested than available */ + if (((*len)) > fifo->length) + { + (*len) = fifo->length; + } + + return rslt; +} + +/*! + * @brief This API is used to parse the auxiliary data from the FIFO data in + * header mode. + */ +static int8_t extract_aux_header_mode(struct bmi2_aux_fifo_data *aux, + uint16_t *aux_len, + struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Variable to define header frame */ + uint8_t frame_header = 0; + + /* Variable to index the data bytes */ + uint16_t data_index; + + /* Variable to index gyroscope frames */ + uint16_t aux_index = 0; + + /* Variable to indicate auxiliary frames read */ + uint16_t frame_to_read = *aux_len; + + for (data_index = fifo->aux_byte_start_idx; data_index < fifo->length;) + { + /* Get frame header byte */ + frame_header = fifo->data[data_index] & BMI2_FIFO_TAG_INTR_MASK; + + /* Parse virtual header if S4S is enabled */ + parse_if_virtual_header(&frame_header, &data_index, fifo); + + /* Index shifted to next byte where data starts */ + data_index++; + switch (frame_header) + { + /* If header defines auxiliary frame */ + case BMI2_FIFO_HEADER_AUX_FRM: + case BMI2_FIFO_HEADER_AUX_ACC_FRM: + case BMI2_FIFO_HEADER_AUX_GYR_FRM: + case BMI2_FIFO_HEADER_ALL_FRM: + + /* Unpack from normal frames */ + rslt = unpack_aux_frame(aux, &data_index, &aux_index, frame_header, fifo, dev); + break; + + /* If header defines only accelerometer frame */ + case BMI2_FIFO_HEADER_ACC_FRM: + rslt = move_next_frame(&data_index, fifo->acc_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 gyroscope and accelerometer frame */ + case BMI2_FIFO_HEADER_GYR_ACC_FRM: + rslt = move_next_frame(&data_index, fifo->acc_gyr_frm_len, fifo); + break; + + /* If header defines sensor time frame */ + case BMI2_FIFO_HEADER_SENS_TIME_FRM: + rslt = unpack_sensortime_frame(&data_index, fifo); + break; + + /* If header defines skip frame */ + case BMI2_FIFO_HEADER_SKIP_FRM: + rslt = unpack_skipped_frame(&data_index, 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 */ + data_index = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + case BMI2_FIFO_VIRT_ACT_RECOG_FRM: + rslt = move_next_frame(&data_index, BMI2_FIFO_VIRT_ACT_DATA_LENGTH, fifo); + 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; + } + + /* Break if number of frames to be read is complete or FIFO is + * empty + */ + if ((frame_to_read == aux_index) || (rslt == BMI2_W_FIFO_EMPTY)) + { + break; + } + } + + /* Update the gyroscope frame index */ + (*aux_len) = aux_index; + + /* Update the gyroscope byte index */ + fifo->aux_byte_start_idx = data_index; + + return rslt; +} + +/*! + * @brief This API is used to parse the auxiliary frame from the FIFO data in + * both header mode and header-less mode and update the data_index value which + * is used to store the index of the current data byte which is parsed. + */ +static int8_t unpack_aux_frame(struct bmi2_aux_fifo_data *aux, + uint16_t *idx, + uint16_t *aux_idx, + uint8_t frame, + const struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + switch (frame) + { + /* If frame contains only auxiliary data */ + case BMI2_FIFO_HEADER_AUX_FRM: + case BMI2_FIFO_HEAD_LESS_AUX_FRM: + + /* Partially read, then skip the data */ + if (((*idx) + fifo->aux_frm_len) > fifo->length) + { + /* Update the data index as complete*/ + (*idx) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + } + + /* Get the auxiliary data */ + unpack_aux_data(&aux[(*aux_idx)], (*idx), fifo); + + /* Update data index */ + (*idx) = (*idx) + BMI2_FIFO_AUX_LENGTH; + + /* Get virtual sensor time if S4S is enabled */ + if (dev->sens_en_stat & BMI2_EXT_SENS_SEL) + { + unpack_virt_aux_sensor_time(&aux[(*aux_idx)], idx, fifo); + } + + /* Update auxiliary frame index */ + (*aux_idx)++; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + + /* If frame contains accelerometer and auxiliary data */ + case BMI2_FIFO_HEADER_AUX_ACC_FRM: + case BMI2_FIFO_HEAD_LESS_AUX_ACC_FRM: + + /* Partially read, then skip the data */ + if (((*idx) + fifo->acc_aux_frm_len) > fifo->length) + { + /* Move the data index to the last byte */ + (*idx) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + } + + /* Get the auxiliary data */ + unpack_aux_data(&aux[(*aux_idx)], (*idx), fifo); + + /* Update data index */ + (*idx) = (*idx) + BMI2_FIFO_ACC_AUX_LENGTH; + + /* Get virtual sensor time if S4S is enabled */ + if (dev->sens_en_stat & BMI2_EXT_SENS_SEL) + { + unpack_virt_aux_sensor_time(&aux[(*aux_idx)], idx, fifo); + } + + /* Update auxiliary frame index */ + (*aux_idx)++; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + + /* If frame contains gyroscope and auxiliary data */ + case BMI2_FIFO_HEADER_AUX_GYR_FRM: + case BMI2_FIFO_HEAD_LESS_GYR_AUX_FRM: + + /* Partially read, then skip the data */ + if (((*idx) + fifo->aux_gyr_frm_len) > fifo->length) + { + /* Move the data index to the last byte */ + (*idx) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + } + + /* Get the auxiliary data */ + unpack_aux_data(&aux[(*aux_idx)], (*idx), fifo); + + /* Update data index */ + (*idx) = (*idx) + BMI2_FIFO_GYR_AUX_LENGTH; + + /* Get virtual sensor time if S4S is enabled */ + if (dev->sens_en_stat & BMI2_EXT_SENS_SEL) + { + unpack_virt_aux_sensor_time(&aux[(*aux_idx)], idx, fifo); + } + + /* Update auxiliary frame index */ + (*aux_idx)++; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + + /* If frame contains accelerometer, gyroscope and auxiliary data */ + case BMI2_FIFO_HEADER_ALL_FRM: + case BMI2_FIFO_HEAD_LESS_ALL_FRM: + + /* Partially read, then skip the data*/ + if ((*idx + fifo->all_frm_len) > fifo->length) + { + /* Move the data index to the last byte */ + (*idx) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + } + + /* Get the auxiliary data */ + unpack_aux_data(&aux[(*aux_idx)], (*idx), fifo); + + /* Update data index */ + (*idx) = (*idx) + BMI2_FIFO_ALL_LENGTH; + + /* Get virtual sensor time if S4S is enabled */ + if (dev->sens_en_stat & BMI2_EXT_SENS_SEL) + { + unpack_virt_aux_sensor_time(&aux[(*aux_idx)], idx, fifo); + } + + /* Update auxiliary frame index */ + (*aux_idx)++; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + + /* If frame contains only accelerometer data */ + case BMI2_FIFO_HEADER_ACC_FRM: + case BMI2_FIFO_HEAD_LESS_ACC_FRM: + + /* Update data index */ + (*idx) = (*idx) + fifo->acc_frm_len; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + + /* If frame contains only gyroscope data */ + case BMI2_FIFO_HEADER_GYR_FRM: + case BMI2_FIFO_HEAD_LESS_GYR_FRM: + + /* Update data index */ + (*idx) = (*idx) + fifo->gyr_frm_len; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + + /* If frame contains accelerometer and gyroscope data */ + case BMI2_FIFO_HEADER_GYR_ACC_FRM: + case BMI2_FIFO_HEAD_LESS_GYR_ACC_FRM: + + /* Update data index */ + (*idx) = (*idx) + fifo->acc_gyr_frm_len; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + default: + + /* Move the data index to the last byte in case of + * invalid values + */ + (*idx) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + } + + return rslt; +} + +/*! + * @brief This internal API is used to parse auxiliary data from the FIFO data. + */ +static void unpack_aux_data(struct bmi2_aux_fifo_data *aux, + uint16_t data_start_index, + const struct bmi2_fifo_frame *fifo) +{ + /* Variables to store index */ + uint16_t idx = 0; + + /* Get auxiliary data */ + for (; idx < 8; idx++) + { + aux->data[idx] = fifo->data[data_start_index++]; + } +} + +/*! + * @brief This internal API parses virtual frame header from the FIFO data. + */ +static void parse_if_virtual_header(uint8_t *frame_header, uint16_t *data_index, const struct bmi2_fifo_frame *fifo) +{ + /* Variable to extract virtual header byte */ + uint8_t virtual_header_mode; + + /* 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) + { + /* Index shifted to next byte where sensor frame is present */ + (*data_index) = (*data_index) + 1; + + /* Get the sensor frame header */ + *frame_header = fifo->data[*data_index] & BMI2_FIFO_TAG_INTR_MASK; + } + } +} + +/*! + * @brief This internal API gets sensor time from the accelerometer and + * gyroscope virtual frames and updates in the data structure. + */ +static void unpack_virt_sensor_time(struct bmi2_sens_axes_data *sens, uint16_t *idx, const struct bmi2_fifo_frame *fifo) +{ + /* Variables to define 3 bytes of sensor time */ + uint32_t sensor_time_byte3; + uint16_t sensor_time_byte2; + uint8_t sensor_time_byte1; + + /* Get sensor time from the FIFO data */ + sensor_time_byte3 = (uint32_t)(fifo->data[(*idx) + BMI2_SENSOR_TIME_MSB_BYTE] << 16); + sensor_time_byte2 = (uint16_t) fifo->data[(*idx) + BMI2_SENSOR_TIME_XLSB_BYTE] << 8; + sensor_time_byte1 = fifo->data[(*idx)]; + + /* Store sensor time in the sensor data structure */ + sens->virt_sens_time = (uint32_t)(sensor_time_byte3 | sensor_time_byte2 | sensor_time_byte1); + + /* Move the data index by 3 bytes */ + (*idx) = (*idx) + BMI2_SENSOR_TIME_LENGTH; +} + +/*! + * @brief This internal API gets sensor time from the auxiliary virtual + * frames and updates in the data structure. + */ +static void unpack_virt_aux_sensor_time(struct bmi2_aux_fifo_data *aux, + uint16_t *idx, + const struct bmi2_fifo_frame *fifo) +{ + /* Variables to define 3 bytes of sensor time */ + uint32_t sensor_time_byte3; + uint16_t sensor_time_byte2; + uint8_t sensor_time_byte1; + + /* Get sensor time from the FIFO data */ + sensor_time_byte3 = (uint32_t)(fifo->data[(*idx) + BMI2_SENSOR_TIME_MSB_BYTE] << 16); + sensor_time_byte2 = (uint16_t) fifo->data[(*idx) + BMI2_SENSOR_TIME_XLSB_BYTE] << 8; + sensor_time_byte1 = fifo->data[(*idx)]; + + /* Store sensor time in the sensor data structure */ + aux->virt_sens_time = (uint32_t)(sensor_time_byte3 | sensor_time_byte2 | sensor_time_byte1); + + /* Move the data index by 3 bytes */ + (*idx) = (*idx) + BMI2_SENSOR_TIME_LENGTH; +} + +/*! + * @brief This internal API is used to reset the FIFO related configurations in + * the FIFO frame structure for the next FIFO read. + */ +static void reset_fifo_frame_structure(struct bmi2_fifo_frame *fifo, const struct bmi2_dev *dev) +{ + /* Reset FIFO data structure */ + fifo->acc_byte_start_idx = 0; + fifo->aux_byte_start_idx = 0; + fifo->gyr_byte_start_idx = 0; + fifo->sensor_time = 0; + fifo->skipped_frame_count = 0; + fifo->act_recog_byte_start_idx = 0; + + /* If S4S is enabled */ + if ((dev->sens_en_stat & BMI2_EXT_SENS_SEL) == BMI2_EXT_SENS_SEL) + { + fifo->acc_frm_len = BMI2_FIFO_VIRT_ACC_LENGTH; + fifo->gyr_frm_len = BMI2_FIFO_VIRT_GYR_LENGTH; + fifo->aux_frm_len = BMI2_FIFO_VIRT_AUX_LENGTH; + fifo->acc_gyr_frm_len = BMI2_FIFO_VIRT_ACC_GYR_LENGTH; + fifo->acc_aux_frm_len = BMI2_FIFO_VIRT_ACC_AUX_LENGTH; + fifo->aux_gyr_frm_len = BMI2_FIFO_VIRT_GYR_AUX_LENGTH; + fifo->all_frm_len = BMI2_FIFO_VIRT_ALL_LENGTH; + + /* If S4S is not enabled */ + } + else + { + fifo->acc_frm_len = BMI2_FIFO_ACC_LENGTH; + fifo->gyr_frm_len = BMI2_FIFO_GYR_LENGTH; + fifo->aux_frm_len = BMI2_FIFO_AUX_LENGTH; + fifo->acc_gyr_frm_len = BMI2_FIFO_ACC_GYR_LENGTH; + fifo->acc_aux_frm_len = BMI2_FIFO_ACC_AUX_LENGTH; + fifo->aux_gyr_frm_len = BMI2_FIFO_GYR_AUX_LENGTH; + fifo->all_frm_len = BMI2_FIFO_ALL_LENGTH; + } +} + +/*! + * @brief This API internal checks whether the FIFO data read is an empty frame. + * If empty frame, index is moved to the last byte. + */ +static int8_t check_empty_fifo(uint16_t *data_index, const struct bmi2_fifo_frame *fifo) +{ + /* Variables to define error */ + int8_t rslt = BMI2_OK; + + /* Validate data index */ + if (((*data_index) + 2) < 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)) + { + /* Move the data index to the last byte to mark completion */ + (*data_index) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + } + else + { + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + } + } + + return rslt; +} + +/*! + * @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; +} + +/*! + * @brief This internal API is used to parse and store the sensor time from the + * FIFO data. + */ +static int8_t unpack_sensortime_frame(uint16_t *data_index, struct bmi2_fifo_frame *fifo) +{ + /* Variables to define error */ + int8_t rslt = BMI2_OK; + + /* Variables to define 3 bytes of sensor time */ + uint32_t sensor_time_byte3 = 0; + uint16_t sensor_time_byte2 = 0; + uint8_t sensor_time_byte1 = 0; + + /* Validate data index */ + if (((*data_index) + BMI2_SENSOR_TIME_LENGTH) > fifo->length) + { + /* Move the data index to the last byte */ + (*data_index) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + } + else + { + /* Get sensor time from the FIFO data */ + sensor_time_byte3 = fifo->data[(*data_index) + BMI2_SENSOR_TIME_MSB_BYTE] << 16; + sensor_time_byte2 = fifo->data[(*data_index) + BMI2_SENSOR_TIME_XLSB_BYTE] << 8; + sensor_time_byte1 = fifo->data[(*data_index)]; + + /* Update sensor time in the FIFO structure */ + fifo->sensor_time = (uint32_t)(sensor_time_byte3 | sensor_time_byte2 | sensor_time_byte1); + + /* Move the data index by 3 bytes */ + (*data_index) = (*data_index) + BMI2_SENSOR_TIME_LENGTH; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + } + + return rslt; +} + +/*! + * @brief This internal API is used to parse and store the skipped frame count + * from the FIFO data. + */ +static int8_t unpack_skipped_frame(uint16_t *data_index, struct bmi2_fifo_frame *fifo) +{ + /* Variables to define error */ + int8_t rslt = BMI2_OK; + + /* Validate data index */ + if ((*data_index) >= fifo->length) + { + /* Update the data index to the last byte */ + (*data_index) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + } + else + { + /* Update skipped frame count in the FIFO structure */ + fifo->skipped_frame_count = fifo->data[(*data_index)]; + + /* Move the data index by 1 byte */ + (*data_index) = (*data_index) + 1; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + } + + 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. + */ +static int8_t pre_self_test_config(struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Structure to define sensor configurations */ + struct bmi2_sens_config sens_cfg; + + /* List the sensors to be selected */ + uint8_t sens_sel = BMI2_ACCEL; + + /* Enable accelerometer */ + rslt = bmi2_sensor_enable(&sens_sel, 1, dev); + dev->delay_us(1000); + + /* 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 */ + sens_cfg.type = BMI2_ACCEL; + + /* Get the default values */ + rslt = bmi2_get_sensor_config(&sens_cfg, 1, dev); + if (rslt == BMI2_OK) + { + /* Set the configurations required for self-test */ + sens_cfg.cfg.acc.odr = BMI2_ACC_ODR_1600HZ; + sens_cfg.cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + sens_cfg.cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + sens_cfg.cfg.acc.range = BMI2_ACC_RANGE_16G; + + /* Set accelerometer configurations */ + rslt = bmi2_set_sensor_config(&sens_cfg, 1, dev); + } + } + + return rslt; +} + +/*! + * @brief This internal API performs the steps needed for self-test operation + * before reading the accelerometer self-test data. + */ +static int8_t self_test_config(uint8_t sign, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Enable the accelerometer self-test feature */ + rslt = set_accel_self_test_enable(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + /* Selects the sign of accelerometer self-test excitation */ + rslt = set_acc_self_test_sign(sign, dev); + } + + return rslt; +} + +/*! + * @brief This internal API enables or disables the accelerometer self-test + * feature in the sensor. + */ +static int8_t set_accel_self_test_enable(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define data */ + uint8_t data = 0; + + /* Enable/Disable self-test feature */ + rslt = bmi2_get_regs(BMI2_ACC_SELF_TEST_ADDR, &data, 1, dev); + if (rslt == BMI2_OK) + { + data = BMI2_SET_BIT_POS0(data, BMI2_ACC_SELF_TEST_EN, enable); + rslt = bmi2_set_regs(BMI2_ACC_SELF_TEST_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This internal API selects the sign for accelerometer self-test + * excitation. + */ +static int8_t set_acc_self_test_sign(uint8_t sign, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define data */ + uint8_t data = 0; + + /* Select the sign for self-test excitation */ + rslt = bmi2_get_regs(BMI2_ACC_SELF_TEST_ADDR, &data, 1, dev); + if (rslt == BMI2_OK) + { + data = BMI2_SET_BITS(data, BMI2_ACC_SELF_TEST_SIGN, sign); + rslt = bmi2_set_regs(BMI2_ACC_SELF_TEST_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This internal API sets the amplitude of the accelerometer self-test + * deflection in the sensor. + */ +static int8_t set_accel_self_test_amp(uint8_t amp, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define data */ + uint8_t data = 0; + + /* Select amplitude of the self-test deflection */ + rslt = bmi2_get_regs(BMI2_ACC_SELF_TEST_ADDR, &data, 1, dev); + if (rslt == BMI2_OK) + { + data = BMI2_SET_BITS(data, BMI2_ACC_SELF_TEST_AMP, amp); + rslt = bmi2_set_regs(BMI2_ACC_SELF_TEST_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @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) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define LSB */ + uint16_t lsb = 0; + + /* Variable to define MSB */ + uint16_t msb = 0; + + /* Array to define data buffer */ + uint8_t data[BMI2_ACC_GYR_NUM_BYTES] = { 0 }; + + rslt = bmi2_get_regs(BMI2_ACC_X_LSB_ADDR, data, BMI2_ACC_GYR_NUM_BYTES, dev); + if (rslt == BMI2_OK) + { + /* Accelerometer data x axis */ + msb = data[1]; + lsb = data[0]; + accel->x = (int16_t)((msb << 8) | lsb); + + /* Accelerometer data y axis */ + msb = data[3]; + lsb = data[2]; + accel->y = (int16_t)((msb << 8) | lsb); + + /* Accelerometer data z axis */ + msb = data[5]; + lsb = data[4]; + accel->z = (int16_t)((msb << 8) | lsb); + } + + return rslt; +} + +/*! + * @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) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define LSB */ + uint16_t lsb = 0; + + /* Variable to define MSB */ + uint16_t msb = 0; + + /* Array to define data buffer */ + uint8_t data[BMI2_ACC_GYR_NUM_BYTES] = { 0 }; + + rslt = bmi2_get_regs(BMI2_GYR_X_LSB_ADDR, data, BMI2_ACC_GYR_NUM_BYTES, dev); + if (rslt == BMI2_OK) + { + /* Gyroscope data x axis */ + msb = data[1]; + lsb = data[0]; + gyro->x = (int16_t)((msb << 8) | lsb); + + /* Gyroscope data y axis */ + msb = data[3]; + lsb = data[2]; + gyro->y = (int16_t)((msb << 8) | lsb); + + /* Gyroscope data z axis */ + msb = data[5]; + lsb = data[4]; + gyro->z = (int16_t)((msb << 8) | lsb); + } + + 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, + const struct bmi2_dev *dev) +{ + /* Variable to define LSB/g value of axes */ + uint32_t lsb_per_g; + + /* Range considered for self-test is +/-16g */ + uint8_t range = BMI2_ACC_SELF_TEST_RANGE; + + /* lsb_per_g for the respective resolution and 16g range */ + lsb_per_g = (uint32_t)(power(2, dev->resolution) / (2 * range)); + + /* Accelerometer x value in mg */ + acc_data_diff_mg->x = (acc_data_diff->x / (int32_t) lsb_per_g) * 1000; + + /* Accelerometer y value in mg */ + acc_data_diff_mg->y = (acc_data_diff->y / (int32_t) lsb_per_g) * 1000; + + /* Accelerometer z value in mg */ + acc_data_diff_mg->z = (acc_data_diff->z / (int32_t) lsb_per_g) * 1000; +} + +/*! + * @brief This internal API is used to calculate the power of a value. + */ +static int32_t power(int16_t base, uint8_t resolution) +{ + /* Initialize loop */ + uint8_t loop = 1; + + /* Initialize variable to store the power of 2 value */ + int32_t value = 1; + + for (; loop <= resolution; loop++) + { + value = (int32_t)(value * base); + } + + return value; +} + +/*! + * @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) +{ + /* Variable to define error */ + int8_t rslt; + + /* As per the data sheet, The actually measured signal differences should be significantly + * larger than the minimum differences for each axis in order for the self-test to pass. + */ + if ((accel_data_diff->x > BMI2_ST_ACC_X_SIG_MIN_DIFF) && (accel_data_diff->y < BMI2_ST_ACC_Y_SIG_MIN_DIFF) && + (accel_data_diff->z > BMI2_ST_ACC_Z_SIG_MIN_DIFF)) + { + /* Self-test pass */ + rslt = BMI2_OK; + } + else + { + /* Self-test fail*/ + rslt = BMI2_E_SELF_TEST_FAIL; + } + + return rslt; +} + +/*! + * @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) +{ + /* 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; + + /* Initialize feature configuration for axis re-mapping */ + struct bmi2_feature_config remap_config = { 0, 0, 0 }; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat; + + /* 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) + { + /* Search for axis re-mapping and extract its configuration details */ + feat_found = extract_input_feat_config(&remap_config, BMI2_AXIS_MAP, dev); + if (feat_found) + { + rslt = 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); + + /* Get the re-mapped x-axis polarity */ + remap->x_axis_sign = BMI2_GET_BITS(feat_config[idx], X_AXIS_SIGN); + + /* Get the re-mapped y-axis */ + remap->y_axis = BMI2_GET_BITS(feat_config[idx], Y_AXIS); + + /* Get the re-mapped y-axis polarity */ + remap->y_axis_sign = BMI2_GET_BITS(feat_config[idx], Y_AXIS_SIGN); + + /* Get the re-mapped z-axis */ + remap->z_axis = BMI2_GET_BITS(feat_config[idx], 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); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + /* 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 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) +{ + /* 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 define the register address */ + uint8_t reg_addr = 0; + + /* Variable to set the re-mapped x-axes in the sensor */ + uint8_t x_axis = 0; + + /* Variable to set the re-mapped y-axes in the sensor */ + uint8_t y_axis = 0; + + /* Variable to set the re-mapped z-axes in the sensor */ + uint8_t z_axis = 0; + + /* Variable to set the re-mapped x-axes sign in the sensor */ + uint8_t x_axis_sign = 0; + + /* Variable to set the re-mapped y-axes sign in the sensor */ + uint8_t y_axis_sign = 0; + + /* Variable to set the re-mapped z-axes sign in the sensor */ + uint8_t z_axis_sign = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for axis re-mapping */ + struct bmi2_feature_config remap_config = { 0, 0, 0 }; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat; + + /* 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) + { + /* Search for axis-re-mapping and extract its configuration details */ + feat_found = 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); + 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; + + /* Set the value of re-mapped x-axis sign */ + x_axis_sign = ((remap->x_axis_sign << X_AXIS_SIGN_POS) & X_AXIS_SIGN_MASK); + + /* Set the value of re-mapped y-axis */ + y_axis = ((remap->y_axis << Y_AXIS_POS) & 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); + + /* Set the value of re-mapped z-axis */ + z_axis = ((remap->z_axis << Z_AXIS_POS) & Z_AXIS_MASK); + + /* Set the value of re-mapped z-axis sign */ + z_axis_sign = remap->z_axis_sign & 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; + + /* Increment the index */ + idx++; + + /* 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); + + /* Update the register address */ + reg_addr = BMI2_FEATURES_REG_ADDR + remap_config.start_addr; + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(reg_addr, &feat_config[remap_config.start_addr], 2, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + /* 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 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. + */ +static void comp_gyro_cross_axis_sensitivity(struct bmi2_sens_axes_data *gyr_data, const struct bmi2_dev *dev) +{ + /* Get the compensated gyroscope x-axis */ + 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. + */ +static int8_t check_boundary_val(uint8_t *val, uint8_t min, uint8_t max, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + if (val != NULL) + { + /* Check if value is below minimum value */ + if (*val < min) + { + /* Auto correct the invalid value to minimum value */ + *val = min; + dev->info |= BMI2_I_MIN_VALUE; + } + + /* Check if value is above maximum value */ + if (*val > max) + { + /* Auto correct the invalid value to maximum value */ + *val = max; + dev->info |= BMI2_I_MAX_VALUE; + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This internal API saves the configurations before performing FOC. + */ +static int8_t save_accel_foc_config(struct bmi2_accel_config *acc_cfg, + uint8_t *aps, + uint8_t *acc_en, + struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to get the status from PWR_CTRL register */ + uint8_t pwr_ctrl_data = 0; + + /* Get accelerometer configurations to be saved */ + rslt = get_accel_config(acc_cfg, dev); + if (rslt == BMI2_OK) + { + /* Get accelerometer enable status to be saved */ + rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, &pwr_ctrl_data, 1, dev); + *acc_en = BMI2_GET_BITS(pwr_ctrl_data, BMI2_ACC_EN); + + /* Get advance power save mode to be saved */ + if (rslt == BMI2_OK) + { + rslt = bmi2_get_adv_power_save(aps, dev); + } + } + + return rslt; +} + +/*! + * @brief This internal sets configurations for performing accelerometer FOC. + */ +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; + + /* Disabling offset compensation */ + rslt = set_accel_offset_comp(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + /* Set accelerometer configurations to 50Hz, continuous mode, CIC mode */ + rslt = bmi2_set_regs(BMI2_ACC_CONF_ADDR, &acc_conf_data, 1, dev); + if (rslt == BMI2_OK) + { + /* 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 */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + } + } + + return rslt; +} + +/*! + * @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, + const struct bmi2_accel_config *acc_cfg, + struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_E_INVALID_STATUS; + + /* Variable to define count */ + uint8_t loop; + + /* Variable to store status read from the status register */ + uint8_t reg_status = 0; + + /* Array of structure to store accelerometer data */ + struct bmi2_sens_axes_data accel_value[128] = { { 0 } }; + + /* Structure to store accelerometer data temporarily */ + struct 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 }; + + /* Variable to define LSB per g value */ + uint16_t lsb_per_g = 0; + + /* Variable to define range */ + uint8_t range = 0; + + /* Structure to store accelerometer data deviation from ideal value */ + struct offset_delta delta = { 0, 0, 0 }; + + /* Structure to store accelerometer offset values */ + struct accel_offset offset = { 0, 0, 0 }; + + /* Variable tries max 5 times for interrupt then generates timeout */ + uint8_t try_cnt; + + for (loop = 0; loop < 128; loop++) + { + try_cnt = 5; + while (try_cnt && (!(reg_status & BMI2_DRDY_ACC))) + { + /* 20ms delay for 50Hz ODR */ + dev->delay_us(20000); + 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); + } + + if (rslt == BMI2_OK) + { + rslt = read_accel_xyz(&accel_value[loop], dev); + } + if (rslt == BMI2_OK) + { + /* Store the data in a temporary structure */ + temp.x = temp.x + (int32_t)accel_value[loop].x; + temp.y = temp.y + (int32_t)accel_value[loop].y; + temp.z = temp.z + (int32_t)accel_value[loop].z; + } + else + { + break; + } + } + if (rslt == BMI2_OK) + { + /* Take average of x, y and z data for lesser noise */ + accel_avg.x = (int16_t)(temp.x / 128); + accel_avg.y = (int16_t)(temp.y / 128); + accel_avg.z = (int16_t)(temp.z / 128); + + /* Get the exact range value */ + map_accel_range(acc_cfg->range, &range); + + /* Get the smallest possible measurable acceleration level given the range and + * resolution */ + lsb_per_g = (uint16_t)(power(2, dev->resolution) / (2 * range)); + + /* Compensate acceleration data against gravity */ + comp_for_gravity(lsb_per_g, accel_g_value, &accel_avg, &delta); + + /* Scale according to offset register resolution */ + scale_accel_offset(range, &delta, &offset); + + /* Invert the accelerometer offset data */ + invert_accel_offset(&offset); + + /* Write offset data in the offset compensation register */ + rslt = write_accel_offset(&offset, dev); + + /* Enable offset compensation */ + if (rslt == BMI2_OK) + { + rslt = set_accel_offset_comp(BMI2_ENABLE, dev); + } + } + + return rslt; +} + +/*! + * @brief This internal API enables/disables the offset compensation for + * filtered and un-filtered accelerometer data. + */ +static int8_t set_accel_offset_comp(uint8_t offset_en, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store data */ + uint8_t data = 0; + + /* Enable/Disable offset compensation */ + rslt = bmi2_get_regs(BMI2_NV_CONF_ADDR, &data, 1, dev); + if (rslt == BMI2_OK) + { + data = BMI2_SET_BITS(data, BMI2_NV_ACC_OFFSET, offset_en); + rslt = bmi2_set_regs(BMI2_NV_CONF_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This internal API converts the accelerometer range value into + * corresponding integer value. + */ +static void map_accel_range(uint8_t range_in, uint8_t *range_out) +{ + switch (range_in) + { + case BMI2_ACC_RANGE_2G: + *range_out = 2; + break; + case BMI2_ACC_RANGE_4G: + *range_out = 4; + break; + case BMI2_ACC_RANGE_8G: + *range_out = 8; + break; + case BMI2_ACC_RANGE_16G: + *range_out = 16; + break; + default: + + /* By default RANGE 8G is set */ + *range_out = 8; + break; + } +} + +/*! + * @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_sens_axes_data *data, + struct offset_delta *comp_data) +{ + /* Array to store the accelerometer values in LSB */ + int16_t accel_value_lsb[3] = { 0 }; + + /* Convert g-value to LSB */ + accel_value_lsb[BMI2_X_AXIS] = (int16_t)(lsb_per_g * g_val->x); + accel_value_lsb[BMI2_Y_AXIS] = (int16_t)(lsb_per_g * g_val->y); + accel_value_lsb[BMI2_Z_AXIS] = (int16_t)(lsb_per_g * g_val->z); + + /* Get the compensated values for X, Y and Z axis */ + comp_data->x = (data->x - accel_value_lsb[BMI2_X_AXIS]); + comp_data->y = (data->y - accel_value_lsb[BMI2_Y_AXIS]); + comp_data->z = (data->z - accel_value_lsb[BMI2_Z_AXIS]); +} + +/*! + * @brief This internal API scales the compensated accelerometer data according + * to the offset register resolution. + * + * @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) +{ + /* Variable to store the position of bit having 3.9mg resolution */ + int8_t bit_pos_3_9mg; + + /* Variable to store the position previous of bit having 3.9mg resolution */ + int8_t bit_pos_3_9mg_prev_bit; + + /* Variable to store the round-off value */ + uint8_t round_off; + + /* Find the bit position of 3.9mg */ + bit_pos_3_9mg = get_bit_pos_3_9mg(range); + + /* Round off, consider if the next bit is high */ + bit_pos_3_9mg_prev_bit = bit_pos_3_9mg - 1; + round_off = (uint8_t)(power(2, ((uint8_t) bit_pos_3_9mg_prev_bit))); + + /* Scale according to offset register resolution */ + data->x = (uint8_t)((comp_data->x + round_off) / power(2, ((uint8_t) bit_pos_3_9mg))); + data->y = (uint8_t)((comp_data->y + round_off) / power(2, ((uint8_t) bit_pos_3_9mg))); + data->z = (uint8_t)((comp_data->z + round_off) / power(2, ((uint8_t) bit_pos_3_9mg))); +} + +/*! + * @brief This internal API finds the bit position of 3.9mg according to given + * range and resolution. + */ +static int8_t get_bit_pos_3_9mg(uint8_t range) +{ + /* 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 */ + uint32_t divisor = 1; + + /* Scaling factor to get the bit position of 3.9 mg resolution */ + int16_t scale_factor = -1; + + /* Variable to store temporary value */ + uint16_t temp; + + /* Shift left by the times of resolution */ + divisor = divisor << 16; + + /* Get the bit position to be shifted */ + temp = (uint16_t)(divisor / (range * 256)); + + /* Get the scaling factor until bit position is shifted to last bit */ + while (temp != 1) + { + scale_factor++; + temp = temp >> 1; + } + + /* Scaling factor is the bit position of 3.9 mg resolution */ + bit_pos_3_9mg = (int8_t) scale_factor; + + return bit_pos_3_9mg; +} + +/*! + * @brief This internal API inverts the accelerometer offset data. + */ +static void invert_accel_offset(struct accel_offset *offset_data) +{ + /* Get the offset data */ + offset_data->x = (uint8_t)((offset_data->x) * (-1)); + offset_data->y = (uint8_t)((offset_data->y) * (-1)); + offset_data->z = (uint8_t)((offset_data->z) * (-1)); +} + +/*! + * @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) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to store the offset data */ + uint8_t data_array[3] = { 0 }; + + data_array[0] = offset->x; + data_array[1] = offset->y; + data_array[2] = offset->z; + + /* Offset values are written in the offset register */ + rslt = bmi2_set_regs(BMI2_ACC_OFF_COMP_0_ADDR, data_array, 3, dev); + + return rslt; +} + +/*! + * @brief This internal API restores the configurations saved before performing + * accelerometer FOC. + */ +static int8_t restore_accel_foc_config(struct bmi2_accel_config *acc_cfg, + uint8_t aps, + uint8_t acc_en, + struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to get the status from PWR_CTRL register */ + uint8_t pwr_ctrl_data = 0; + + /* Restore the saved accelerometer configurations */ + rslt = set_accel_config(acc_cfg, dev); + if (rslt == BMI2_OK) + { + /* Restore the saved accelerometer enable status */ + rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, &pwr_ctrl_data, 1, dev); + if (rslt == BMI2_OK) + { + pwr_ctrl_data = BMI2_SET_BITS(pwr_ctrl_data, BMI2_ACC_EN, acc_en); + rslt = bmi2_set_regs(BMI2_PWR_CTRL_ADDR, &pwr_ctrl_data, 1, dev); + + /* Restore the saved advance power save */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_adv_power_save(aps, dev); + } + } + } + + return rslt; +} + +/*! + * @brief This internal API saves the configurations before performing gyroscope + * FOC. + */ +static int8_t save_gyro_config(struct bmi2_gyro_config *gyr_cfg, uint8_t *aps, uint8_t *gyr_en, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to get the status from PWR_CTRL register */ + uint8_t pwr_ctrl_data = 0; + + /* Get gyroscope configurations to be saved */ + rslt = get_gyro_config(gyr_cfg, dev); + if (rslt == BMI2_OK) + { + /* Get gyroscope enable status to be saved */ + rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, &pwr_ctrl_data, 1, dev); + *gyr_en = BMI2_GET_BITS(pwr_ctrl_data, BMI2_GYR_EN); + + /* Get advance power save mode to be saved */ + if (rslt == BMI2_OK) + { + rslt = bmi2_get_adv_power_save(aps, dev); + } + } + + return rslt; +} + +/*! + * @brief This internal sets configurations for performing gyroscope FOC. + */ +static int8_t set_gyro_foc_config(struct bmi2_dev *dev) +{ + int8_t rslt; + + /* 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) + { + /* Set gyroscope configurations to 25Hz, continuous mode, + * CIC mode, and 2000 dps range + */ + rslt = bmi2_set_regs(BMI2_GYR_CONF_ADDR, gyr_conf_data, 2, dev); + if (rslt == BMI2_OK) + { + /* 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 */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + } + } + + return rslt; +} + +/*! + * @brief This internal API inverts the gyroscope offset data. + */ +static void invert_gyro_offset(struct bmi2_sens_axes_data *offset_data) +{ + /* Invert the values */ + offset_data->x = (int16_t)((offset_data->x) * (-1)); + offset_data->y = (int16_t)((offset_data->y) * (-1)); + offset_data->z = (int16_t)((offset_data->z) * (-1)); +} + +/*! + * @brief This internal API restores the gyroscope configurations saved + * before performing FOC. + */ +static int8_t restore_gyro_config(struct bmi2_gyro_config *gyr_cfg, uint8_t aps, uint8_t gyr_en, struct bmi2_dev *dev) +{ + int8_t rslt; + uint8_t pwr_ctrl_data = 0; + + /* Restore the saved gyroscope configurations */ + rslt = set_gyro_config(gyr_cfg, dev); + if (rslt == BMI2_OK) + { + /* Restore the saved gyroscope enable status */ + rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, &pwr_ctrl_data, 1, dev); + if (rslt == BMI2_OK) + { + pwr_ctrl_data = BMI2_SET_BITS(pwr_ctrl_data, BMI2_GYR_EN, gyr_en); + rslt = bmi2_set_regs(BMI2_PWR_CTRL_ADDR, &pwr_ctrl_data, 1, dev); + + /* Restore the saved advance power save */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_adv_power_save(aps, dev); + } + } + } + + return rslt; +} + +/*! + * @brief This internal API saturates the gyroscope data value before writing to + * to 10 bit offset register. + */ +static void saturate_gyro_data(struct bmi2_sens_axes_data *gyr_off) +{ + if (gyr_off->x > 511) + { + 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; + } +} + +/*! + * @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) +{ + 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 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) +{ + int8_t rslt; + uint8_t reg_data = 0; + + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + /* Get the status of crt running */ + rslt = bmi2_get_regs(BMI2_GYR_CRT_CONF_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + (*st_status) = BMI2_GET_BITS(reg_data, BMI2_GYR_CRT_RUNNING); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API enables/disables the CRT running. + */ +static int8_t set_st_running(uint8_t st_status, struct bmi2_dev *dev) +{ + int8_t rslt; + uint8_t reg_data = 0; + + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + rslt = bmi2_get_regs(BMI2_GYR_CRT_CONF_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_CRT_RUNNING, st_status); + rslt = bmi2_set_regs(BMI2_GYR_CRT_CONF_ADDR, ®_data, 1, dev); + } + } + + return rslt; +} + +/*! + * @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) +{ + int8_t rslt; + uint8_t reg_data = 0; + + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + /* Get the status of rdy_fo_dl */ + rslt = bmi2_get_regs(BMI2_GYR_CRT_CONF_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + (*rdy_for_dl) = BMI2_GET_BITS(reg_data, BMI2_GYR_RDY_FOR_DL); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API does the crt process if max burst length is not zero. + */ +static int8_t process_crt_download(uint8_t last_byte_flag, struct bmi2_dev *dev) +{ + int8_t rslt; + uint8_t rdy_for_dl = 0; + uint8_t cmd = BMI2_G_TRIGGER_CMD; + + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + rslt = get_rdy_for_dl(&rdy_for_dl, dev); + } + + /* Trigger next CRT command */ + if (rslt == BMI2_OK) + { + 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); + } + + return rslt; +} + +/*! + * @brief This API to write the 2kb size of crt configuration + */ +static int8_t write_crt_config_file(uint16_t write_len, + uint16_t config_file_size, + uint16_t start_index, + struct bmi2_dev *dev) +{ + int8_t rslt = BMI2_OK; + uint16_t index = 0; + uint8_t last_byte_flag = 0; + uint8_t remain = (uint8_t)(config_file_size % write_len); + uint16_t balance_byte = 0; + + if (!remain) + { + + /* Write the configuration file */ + for (index = start_index; + (index < (start_index + config_file_size)) && (rslt == BMI2_OK); + index += write_len) + { + rslt = upload_file((dev->config_file_ptr + index), index, write_len, dev); + if (index >= ((start_index + config_file_size) - (write_len))) + { + last_byte_flag = 1; + } + + if (rslt == BMI2_OK) + { + rslt = process_crt_download(last_byte_flag, dev); + } + } + } + else + { + /* Get the balance bytes */ + balance_byte = (uint16_t)start_index + (uint16_t)config_file_size - (uint16_t)remain; + + /* Write the configuration file for the balance bytes */ + for (index = start_index; (index < balance_byte) && (rslt == BMI2_OK); index += write_len) + { + rslt = upload_file((dev->config_file_ptr + index), index, write_len, dev); + if (rslt == BMI2_OK) + { + rslt = process_crt_download(last_byte_flag, dev); + } + } + if (rslt == BMI2_OK) + { + /* Write the remaining bytes in 2 bytes length */ + write_len = 2; + rslt = set_maxburst_len(write_len, dev); + + /* Write the configuration file for the remaining bytes */ + for (index = balance_byte; + (index < (start_index + config_file_size)) && (rslt == BMI2_OK); + index += write_len) + { + rslt = upload_file((dev->config_file_ptr + index), index, write_len, dev); + if (index < ((start_index + config_file_size) - write_len)) + { + last_byte_flag = 1; + } + if (rslt == BMI2_OK) + { + rslt = process_crt_download(last_byte_flag, dev); + } + } + } + } + + return rslt; +} + +/*! + * @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) +{ + int8_t rslt = BMI2_OK; + uint8_t dl_ready = 0; + uint8_t st_status = 0; + + while ((rslt == BMI2_OK) && (retry_complete--)) + { + rslt = get_rdy_for_dl(&dl_ready, dev); + if (download_ready != dl_ready) + { + break; + } + dev->delay_us(CRT_READY_FOR_DOWNLOAD_US); + } + 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); + if ((rslt == BMI2_OK) && (st_status == 0)) + { + rslt = BMI2_E_ST_ALREADY_RUNNING; + } + + } + + return rslt; +} + +/*! + * @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) +{ + uint8_t st_status = 1; + int8_t rslt = BMI2_OK; + + while (retry_complete--) + { + rslt = get_st_running(&st_status, dev); + if ((rslt == BMI2_OK) && (st_status == 0)) + { + break; + } + dev->delay_us(CRT_WAIT_RUNNING_US); + } + if ((rslt == BMI2_OK) && (st_status == 1)) + { + rslt = BMI2_E_ST_ALREADY_RUNNING; + } + + return rslt; +} + +/*! + * @brief This api is used to perform gyroscope self-test. + */ +int8_t bmi2_do_gyro_st(struct bmi2_dev *dev) +{ + int8_t rslt; + + rslt = do_gtrigger_test(BMI2_SELECT_GYRO_SELF_TEST, dev); + + return rslt; +} + +/*! + * @brief This API is to run the CRT process for both max burst length 0 and non zero condition. + */ +int8_t bmi2_do_crt(struct bmi2_dev *dev) +{ + int8_t rslt; + + rslt = do_gtrigger_test(BMI2_SELECT_CRT, dev); + + return rslt; +} + +/*! + * @brief This API is to run the crt process for both max burst length 0 and non zero condition. + */ +static int8_t do_gtrigger_test(uint8_t gyro_st_crt, struct bmi2_dev *dev) +{ + int8_t rslt; + int8_t rslt_crt = BMI2_OK; + uint8_t st_status = 0; + uint8_t max_burst_length = 0; + uint8_t download_ready = 0; + uint8_t cmd = BMI2_G_TRIGGER_CMD; + struct bmi2_gyro_self_test_status gyro_st_result = { 0 }; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + /* Check if the variant supports this feature */ + if (dev->variant_feature & BMI2_CRT_RTOSK_ENABLE) + { + /* 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); + } + + /* Get max burst length */ + if (rslt == BMI2_OK) + { + rslt = get_maxburst_len(&max_burst_length, dev); + } + + /* Checking for CRT running status */ + if (rslt == BMI2_OK) + { + rslt = get_st_running(&st_status, dev); + } + + /* CRT is not running and Max burst length is zero */ + if (st_status == 0) + { + if (rslt == BMI2_OK) + { + rslt = set_st_running(BMI2_ENABLE, dev); + } + + /* Preparing the setup */ + if (rslt == BMI2_OK) + { + rslt = crt_prepare_setup(dev); + } + + /* Enable the gyro self-test, CRT */ + if (rslt == BMI2_OK) + { + rslt = select_self_test(gyro_st_crt, dev); + } + + /* Check if FIFO is unchanged by checking the max burst length */ + if ((rslt == BMI2_OK) && (max_burst_length == 0)) + { + /* Trigger CRT */ + 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); + + /* CRT Running wait & check is successful */ + if (rslt == BMI2_OK) + { + rslt = crt_gyro_st_update_result(dev); + } + } + } + else + { + /* FIFO may be used */ + if (rslt == BMI2_OK) + { + if (dev->read_write_len < 2) + { + dev->read_write_len = 2; + } + if (dev->read_write_len > (CRT_MAX_BURST_WORD_LENGTH * 2)) + { + dev->read_write_len = 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); + } + + /* Trigger CRT */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_regs(BMI2_CMD_REG_ADDR, &cmd, 1, 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); + 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_crt = crt_gyro_st_update_result(dev); + if (rslt == BMI2_OK) + { + rslt = rslt_crt; + } + } + } + } + } + else + { + rslt = BMI2_E_ST_ALREADY_RUNNING; + } + + if (rslt == BMI2_OK) + { + if (gyro_st_crt == BMI2_SELECT_GYRO_SELF_TEST) + { + rslt = gyro_self_test_completed(&gyro_st_result, 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); + } + } + } + + return rslt; +} + +/*! + * @brief This API to set up environment for processing the crt. + */ +static int8_t crt_prepare_setup(struct bmi2_dev *dev) +{ + int8_t rslt; + + /* Variable to select the sensor */ + uint8_t sens_list = BMI2_GYRO; + + rslt = null_ptr_check(dev); + + if (rslt == BMI2_OK) + { + /* Disable gyroscope */ + rslt = bmi2_sensor_disable(&sens_list, 1, dev); + } + + /* Disable FIFO for all sensors */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + /* Enable accelerometer */ + 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); + rslt = abort_bmi2(BMI2_DISABLE, dev); + } + + return rslt; +} + +/*! + * @brief This API is to update the CRT or gyro self-test final result. + */ +static int8_t crt_gyro_st_update_result(struct bmi2_dev *dev) +{ + int8_t rslt; + struct bmi2_gyr_user_gain_status user_gain_stat = { 0, 0, 0, 0 }; + + rslt = null_ptr_check(dev); + + /* CRT status has to be read from the config register map */ + if (rslt == BMI2_OK) + { + rslt = get_gyro_gain_update_status(&user_gain_stat, dev); + } + if (rslt == BMI2_OK) + { + switch (user_gain_stat.g_trigger_status) + { + case BMI2_G_TRIGGER_NO_ERROR: + + /* CRT is successful - Reset the Max Burst Length */ + rslt = set_maxburst_len(0, dev); + break; + + case BMI2_G_TRIGGER_DL_ERROR: + + /* CRT is Download Error - Keep non zero value for Max Burst Length */ + rslt = set_maxburst_len(dev->read_write_len, dev); + if (rslt == BMI2_OK) + { + rslt = BMI2_E_DL_ERROR; + } + break; + case BMI2_G_TRIGGER_ABORT_ERROR: + + /* Command is aborted either by host via the block bit or due to motion + * detection. Keep non zero value for Max Burst Length + */ + rslt = set_maxburst_len(dev->read_write_len, dev); + if (rslt == BMI2_OK) + { + rslt = BMI2_E_ABORT_ERROR; + } + break; + + case BMI2_G_TRIGGER_PRECON_ERROR: + + /* Pre-condition to start the feature was not completed. */ + rslt = BMI2_E_PRECON_ERROR; + break; + + default: + rslt = BMI2_E_INVALID_STATUS; + + break; + } + } + + return rslt; +} + +/*! + * @brief This internal API gets the max burst length. + */ +static int8_t get_maxburst_len(uint8_t *max_burst_len, 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 = 0; + struct bmi2_feature_config maxburst_length_bytes = { 0, 0, 0 }; + uint8_t aps_stat; + + if ((dev->variant_feature & BMI2_CRT_IN_FIFO_NOT_REQ) != 0) + { + *max_burst_len = 0; + + return BMI2_OK; + } + + /* 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) + { + /* Search for max burst length */ + feat_found = 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); + if (rslt == BMI2_OK) + { + /* Define the offset for max burst length */ + idx = maxburst_length_bytes.start_addr; + + /* Get the max burst length */ + *max_burst_len = feat_config[idx]; + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + /* 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 sets the max burst length. + */ +static int8_t set_maxburst_len(const uint16_t write_len_byte, 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 reg_addr = 0; + uint8_t max_burst_len = 0; + uint8_t feat_found = 0; + struct bmi2_feature_config maxburst_length_bytes = { 0, 0, 0 }; + uint8_t aps_stat; + uint16_t burst_len = write_len_byte / 2; + + /* for variant that support crt outside fifo, do not modify the max burst len */ + if ((dev->variant_feature & BMI2_CRT_IN_FIFO_NOT_REQ) != 0) + { + return BMI2_OK; + } + + /* Max burst length is only 1 byte */ + if (burst_len > CRT_MAX_BURST_WORD_LENGTH) + { + max_burst_len = UINT8_C(0xFF); + } + else + { + max_burst_len = (uint8_t)burst_len; + } + + /* 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) + { + /* Search for axis-re-mapping and extract its configuration details */ + feat_found = 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); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes */ + idx = maxburst_length_bytes.start_addr; + + /* update Max burst length */ + feat_config[idx] = max_burst_len; + + /* Update the register address */ + reg_addr = BMI2_FEATURES_REG_ADDR + maxburst_length_bytes.start_addr; + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(reg_addr, &feat_config[maxburst_length_bytes.start_addr], 2, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + /* 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 api is used to trigger the preparation for system for NVM programming. + */ +static int8_t set_nvm_prep_prog(uint8_t nvm_prep, 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; + uint8_t reg_addr = 0; + + /* Initialize feature configuration for nvm preparation*/ + struct bmi2_feature_config nvm_config = { 0, 0, 0 }; + + /* 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); + 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); + 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); + + /* Update the register address */ + reg_addr = BMI2_FEATURES_REG_ADDR + nvm_config.start_addr - 1; + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(reg_addr, &feat_config[nvm_config.start_addr - 1], 2, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This api is used to enable the CRT. + */ +static int8_t select_self_test(uint8_t gyro_st_crt, struct bmi2_dev *dev) +{ + int8_t rslt; + + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + uint8_t idx = 0; + + uint8_t feat_found; + uint8_t reg_addr = 0; + + 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); + 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); + 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); + + /* Update the register address */ + reg_addr = BMI2_FEATURES_REG_ADDR + (gyro_self_test_crt_config.start_addr - 1); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(reg_addr, &feat_config[gyro_self_test_crt_config.start_addr - 1], 2, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This api is used to abort ongoing crt or gyro self-test. + */ +int8_t bmi2_abort_crt_gyro_st(struct bmi2_dev *dev) +{ + int8_t rslt = BMI2_OK; + uint8_t aps_stat; + uint8_t st_running = 0; + uint8_t cmd = BMI2_G_TRIGGER_CMD; + + /* 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); + } + + /* Checking for ST running status */ + if (rslt == BMI2_OK) + { + rslt = get_st_running(&st_running, dev); + if (rslt == BMI2_OK) + { + /* ST is not running */ + if (st_running == 0) + { + rslt = BMI2_E_ST_NOT_RUNING; + } + } + } + if (rslt == BMI2_OK) + { + rslt = abort_bmi2(BMI2_ENABLE, dev); + } + + /* send the g trigger command */ + if (rslt == BMI2_OK) + { + 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); + } + + /* Check G trigger status for error */ + if (rslt == BMI2_OK) + { + rslt = crt_gyro_st_update_result(dev); + if (rslt == BMI2_E_ABORT_ERROR) + { + rslt = BMI2_OK; + } + else + { + rslt = BMI2_E_ABORT_ERROR; + } + } + + /* 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 api is used to enable/disable abort. + */ +static int8_t abort_bmi2(uint8_t abort_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; + uint8_t reg_addr = 0; + + /* Initialize feature configuration for blocking a feature */ + 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); + if (feat_found) + { + /* Get the configuration from the page where abort(block) feature resides */ + rslt = 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); + + /* Update the register address */ + reg_addr = BMI2_FEATURES_REG_ADDR + (block_config.start_addr - 1); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(reg_addr, &feat_config[block_config.start_addr - 1], 2, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @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) +{ + int8_t rslt; + uint8_t reg_data; + + rslt = bmi2_get_regs(BMI2_GYR_SELF_TEST_AXES_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + gyro_st_result->gyr_st_axes_done = BMI2_GET_BIT_POS0(reg_data, BMI2_GYR_ST_AXES_DONE); + if (gyro_st_result->gyr_st_axes_done == 0x01) + { + gyro_st_result->gyr_axis_x_ok = BMI2_GET_BITS(reg_data, BMI2_GYR_AXIS_X_OK); + gyro_st_result->gyr_axis_y_ok = BMI2_GET_BITS(reg_data, BMI2_GYR_AXIS_Y_OK); + gyro_st_result->gyr_axis_z_ok = BMI2_GET_BITS(reg_data, BMI2_GYR_AXIS_Z_OK); + } + else + { + rslt = BMI2_E_SELF_TEST_NOT_DONE; + } + } + + return rslt; +} + +/*! + * @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, + struct bmi2_sens_axes_data avg_foc_data, + struct bmi2_dev *dev) +{ + int8_t rslt = BMI2_E_INVALID_INPUT; + + if (sens_list == BMI2_ACCEL) + { + if (accel_g_axis->x == 1) + { + rslt = validate_foc_accel_axis(avg_foc_data.x, dev); + } + else if (accel_g_axis->y == 1) + { + rslt = validate_foc_accel_axis(avg_foc_data.y, dev); + } + else + { + rslt = validate_foc_accel_axis(avg_foc_data.z, dev); + } + } + 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))) + { + rslt = BMI2_OK; + } + else + { + rslt = BMI2_E_INVALID_FOC_POSITION; + } + } + + return rslt; +} + +/*! + * @brief This api validates depends on accel foc access input + */ +static int8_t validate_foc_accel_axis(int16_t avg_foc_data, struct bmi2_dev *dev) +{ + struct bmi2_sens_config sens_cfg = { 0 }; + uint8_t range; + int8_t rslt; + + sens_cfg.type = BMI2_ACCEL; + rslt = bmi2_get_sensor_config(&sens_cfg, 1, 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)) + { + 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)) + { + 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)) + { + 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)) + { + rslt = BMI2_OK; + } + else + { + rslt = BMI2_E_INVALID_FOC_POSITION; + } + + return rslt; +} + +/*! @brief This api is used for programming the non volatile memory(nvm) */ +int8_t bmi2_nvm_prog(struct bmi2_dev *dev) +{ + int8_t rslt = BMI2_OK; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat; + uint8_t status; + uint8_t cmd_rdy; + uint8_t reg_data; + uint8_t write_timeout = 100; + + /* 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); + } + + /* Check the Write status and proceed only if there is no ongoing write cycle */ + if (rslt == BMI2_OK) + { + rslt = bmi2_get_status(&status, dev); + + cmd_rdy = BMI2_GET_BITS(status, BMI2_CMD_RDY); + if (cmd_rdy) + { + rslt = set_nvm_prep_prog(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->delay_us(40000); + + /* Set the NVM_CONF.nvm_prog_en bit in order to enable the NVM + * programming */ + reg_data = BMI2_NVM_UNLOCK_ENABLE; + rslt = bmi2_set_regs(BMI2_NVM_CONF_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + /* Send NVM prog command to command register */ + reg_data = BMI2_NVM_PROG_CMD; + rslt = bmi2_set_regs(BMI2_CMD_REG_ADDR, ®_data, 1, dev); + } + + /* Wait till write operation is completed */ + if (rslt == BMI2_OK) + { + while (write_timeout--) + { + rslt = bmi2_get_status(&status, dev); + if (rslt == BMI2_OK) + { + cmd_rdy = BMI2_GET_BITS(status, BMI2_CMD_RDY); + + /* Nvm is complete once cmd_rdy is 1, break if 1 */ + if (cmd_rdy) + { + break; + } + + /* Wait till cmd_rdy becomes 1 indicating + * nvm process completes */ + dev->delay_us(20000); + } + } + } + if ((rslt == BMI2_OK) && (cmd_rdy != BMI2_TRUE)) + { + rslt = BMI2_E_WRITE_CYCLE_ONGOING; + } + } + } + else + { + rslt = BMI2_E_WRITE_CYCLE_ONGOING; + } + } + if (rslt == BMI2_OK) + { + /* perform soft reset */ + rslt = bmi2_soft_reset(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); + } + + return rslt; +} + +/*! + * @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) +{ + int8_t rslt = 0; + struct bmi2_sensor_data sensor_data = { 0 }; + uint8_t sample_count = 0; + uint8_t datardy_try_cnt; + uint8_t drdy_status = 0; + uint8_t sensor_drdy = 0; + + sensor_data.type = sens_list; + if (sens_list == BMI2_ACCEL) + { + sensor_drdy = BMI2_DRDY_ACC; + } + else + { + sensor_drdy = BMI2_DRDY_GYR; + } + + /* Read sensor values before FOC */ + while (sample_count < BMI2_FOC_SAMPLE_LIMIT) + { + datardy_try_cnt = 5; + do + { + dev->delay_us(20000); + rslt = bmi2_get_status(&drdy_status, dev); + datardy_try_cnt--; + } while ((rslt == BMI2_OK) && (!(drdy_status & sensor_drdy)) && (datardy_try_cnt)); + + if ((rslt != BMI2_OK) || (datardy_try_cnt == 0)) + { + rslt = BMI2_E_DATA_RDY_INT_FAILED; + break; + } + rslt = bmi2_get_sensor_data(&sensor_data, 1, dev); + + if (rslt == BMI2_OK) + { + if (sensor_data.type == BMI2_ACCEL) + { + temp_foc_data->x += sensor_data.sens_data.acc.x; + temp_foc_data->y += sensor_data.sens_data.acc.y; + temp_foc_data->z += sensor_data.sens_data.acc.z; + } + else if (sensor_data.type == BMI2_GYRO) + { + temp_foc_data->x += sensor_data.sens_data.gyr.x; + temp_foc_data->y += sensor_data.sens_data.gyr.y; + temp_foc_data->z += sensor_data.sens_data.gyr.z; + } + } + else + { + break; + } + sample_count++; + } + if (rslt == BMI2_OK) + { + temp_foc_data->x = (temp_foc_data->x / BMI2_FOC_SAMPLE_LIMIT); + temp_foc_data->y = (temp_foc_data->y / BMI2_FOC_SAMPLE_LIMIT); + temp_foc_data->z = (temp_foc_data->z / BMI2_FOC_SAMPLE_LIMIT); + } + + 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) +{ + /* Variable to define the result */ + int8_t rslt = BMI2_OK; + + /* 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; + + /* Variable to define advance power save mode status */ + uint8_t aps_stat; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Initialize feature configuration for config file identification */ + struct bmi2_feature_config config_id = { 0, 0, 0 }; + + /* Check the power mode status */ + 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) + { + /* Search for config file identification feature and extract its configuration + * details */ + feat_found = 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); + if (rslt == BMI2_OK) + { + /* Define the offset for config file identification */ + idx = config_id.start_addr; + + /* Get word to calculate config file identification */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get major and minor version */ + *config_major = BMI2_GET_BITS(lsb_msb, BMI2_CONFIG_MAJOR); + *config_minor = BMI2_GET_BIT_POS0(lsb, BMI2_CONFIG_MINOR); + } + } + + /* 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 gets the output data of OIS. + */ +static int8_t get_ois_output(struct bmi2_ois_output *ois_output, 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 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 }; + + /* Search for OIS output feature and extract its configuration details */ + feat_found = extract_output_feat_config(&ois_config, BMI2_OIS_OUTPUT, dev); + if (feat_found) + { + /* Get the feature output configuration for OIS */ + rslt = get_feat_config(ois_config.page, feat_config, dev); + + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for OIS output start address */ + idx = ois_config.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; + + /* 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; + + /* 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; + + /* 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; + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable free-fall detection feature. + */ +static int8_t set_free_fall_det(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 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) + { + /* 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) + { + /* 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); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API sets free-fall detection configurations like + * free-fall accel settings, and output configuration. + */ +static int8_t set_free_fall_det_config(const struct bmi2_free_fall_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 define set accel settings in loop */ + uint8_t indx; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for free-fall */ + struct bmi2_feature_config freefall_config = { 0, 0, 0 }; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* 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) + { + /* Get the configuration from the page where low-g feature resides */ + rslt = get_feat_config(freefall_config.page, feat_config, 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++) + { + /* 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++; + } + + /* 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++) + { + 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; + } + } + } + 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. + */ +static int8_t get_free_fall_det_config(struct bmi2_free_fall_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 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) + { + /* 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) + { + /* 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; + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2.h b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2.h new file mode 100644 index 000000000..963df0e44 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2.h @@ -0,0 +1,1341 @@ +/** +* 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.h +* @date 2020-01-10 +* @version v2.46.1 +* +*/#ifndef BMI2_H_ +#define BMI2_H_ + +/*! CPP guard */ +#ifdef __cplusplus +extern "C" { +#endif + +/***************************************************************************/ + +/*! Header files + ****************************************************************************/ +#include "bmi2_defs.h" + +/***************************************************************************/ + +/*! BMI2XY User Interface function prototypes + ****************************************************************************/ + +/*! + * @brief 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 + */ +int8_t bmi2_sec_init(struct bmi2_dev *dev); + +/*! + * @brief This API reads the data from the given register address of bmi2 + * sensor. + * + * @param[in] reg_addr : Register address from which data is read. + * @param[out] data : Pointer to data buffer where read data is stored. + * @param[in] len : No. of bytes of data to be read. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @note For most of the registers auto address increment applies, with the + * exception of a few special registers, which trap the address. For e.g., + * 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 + */ +int8_t bmi2_get_regs(uint8_t reg_addr, uint8_t *data, uint16_t len, const struct bmi2_dev *dev); + +/*! + * @brief 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 + * is stored. + * @param[in] len : No. of bytes of data to be written. + * @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_regs(uint8_t reg_addr, const uint8_t *data, uint16_t len, struct bmi2_dev *dev); + +/*! + * @brief 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 + * interface back to SPI from default, after the soft reset command. + * + * @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_soft_reset(struct bmi2_dev *dev); + +/*! + * @brief 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 + */ +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. + * + * @param[in] enable : To enable/disable advance power mode. + * @param[in] dev : Structure instance of bmi2_dev. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables advance power save. + * BMI2_ENABLE | Enables advance power save. + * + * @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 + */ +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. + * + * @param[out] aps_status : Pointer to get the status of APS mode. + * @param[in] dev : Structure instance of bmi2_dev. + * + * aps_status | Description + * -------------|--------------- + * BMI2_DISABLE | Advance power save disabled. + * BMI2_ENABLE | Advance power save enabled. + * + * @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 + */ +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. + * + * @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 + */ +int8_t bmi2_write_config_file(struct bmi2_dev *dev); + +/*! + * @brief 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. + * + * @param[in] int_cfg : Structure instance of bmi2_int_pin_config. + * @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 + */ +int8_t bmi2_set_int_pin_config(const struct bmi2_int_pin_config *int_cfg, struct bmi2_dev *dev); + +/*! + * @brief 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. + * + * @param[in,out] int_cfg : Structure instance of bmi2_int_pin_config. + * @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 + */ +int8_t bmi2_get_int_pin_config(struct bmi2_int_pin_config *int_cfg, const struct bmi2_dev *dev); + +/*! + * @brief 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. + * + * int_status | Status + * -----------|------------ + * 0x00 | BIT0 + * 0x01 | BIT1 + * 0x02 | BIT2 + * 0x03 | BIT3 + * 0x04 | BIT4 + * 0x05 | BIT5 + * 0x06 | BIT6 + * 0x07 | BIT7 + * + * @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_get_int_status(uint16_t *int_status, const struct bmi2_dev *dev); + +/*! + * @brief 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. + * + * enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables FIFO configuration. + * BMI2_ENABLE | Enables FIFO configuration. + * + * @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_fifo_config(uint16_t config, uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief 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 + */ +int8_t bmi2_get_fifo_config(uint16_t *fifo_config, const 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. + * + * @param[in, out] fifo : Structure instance of bmi2_fifo_frame. + * @param[in] dev : Structure instance of 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 + */ +int8_t bmi2_read_fifo_data(struct bmi2_fifo_frame *fifo, const struct bmi2_dev *dev); + +/*! + * 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. + * + * @param[out] accel_data : Structure instance of bmi2_sens_axes_data + * where the parsed data bytes are stored. + * @param[in,out] accel_length : Number of accelerometer frames. + * @param[in,out] fifo : Structure instance of 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_W_FIFO_EMPTY - Warning : FIFO is empty + * @retval BMI2_W_PARTIAL_READ - Warning : There are more frames to be read + */ +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); + +/*! + * @brief 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 + * data bytes are stored. + * @param[in,out] aux_length : Number of auxiliary frames. + * @param[in,out] fifo : Structure instance of 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_W_FIFO_EMPTY - Warning : FIFO is empty + * @retval BMI2_W_PARTIAL_READ - Warning : There are more frames to be read + */ +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); + +/*! + * 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. + * + * @param[out] gyro_data : Structure instance of bmi2_sens_axes_data + * where the parsed data bytes are stored. + * @param[in,out] gyro_length : Number of gyroscope frames. + * @param[in,out] fifo : Structure instance of 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_W_FIFO_EMPTY - Warning : FIFO is empty + * @retval BMI2_W_PARTIAL_READ - Warning : There are more frames to be read + */ +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); + +/*! + * @brief 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. + * + * Commands | Values + * ---------------------|--------------------- + * BMI2_SOFT_RESET_CMD | 0xB6 + * BMI2_FIFO_FLUSH_CMD | 0xB0 + * BMI2_USR_GAIN_CMD | 0x03 + * + * @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_command_register(uint8_t command, struct bmi2_dev *dev); + +/*! + * @brief 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. + * + * fifo_self_wake_up | Description + * -------------------|--------------- + * BMI2_DISABLE | Disables self wake-up. + * BMI2_ENABLE | Enables self wake-up. + * + * @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_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. + * + * @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. + * + * fifo_self_wake_up | Description + * -------------------|--------------- + * BMI2_DISABLE | Self wake-up disabled + * BMI2_ENABLE | Self wake-up enabled. + * + * @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_get_fifo_self_wake_up(uint8_t *fifo_self_wake_up, const struct bmi2_dev *dev); + +/*! + * @brief 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 + */ +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. + * + * @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 + */ +int8_t bmi2_get_fifo_wm(uint16_t *fifo_wm, const struct bmi2_dev *dev); + +/*! + * @brief This API sets either filtered or un-filtered FIFO accelerometer or + * gyroscope data. + * + * @param[in] sens_sel : Selects either accelerometer or + * gyroscope sensor. + * @param[in] fifo_filter_data : Variable to set the filter data. + * @param[in] dev : Structure instance of bmi2_dev. + * + * sens_sel | values + * -----------------|---------- + * BMI2_ACCEL | 0x01 + * BMI2_GYRO | 0x02 + * + * + * Value | fifo_filter_data + * ---------|--------------------- + * 0x00 | Un-filtered data + * 0x01 | Filtered data + * + * @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 + */ +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. + * + * @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. + * + * sens_sel | values + * -----------------|---------- + * BMI2_ACCEL | 0x01 + * BMI2_GYRO | 0x02 + * + * + * Value | fifo_filter_data + * ---------|--------------------- + * 0x00 | Un-filtered data + * 0x01 | Filtered data + * + * @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_get_fifo_filter_data(uint8_t sens_sel, uint8_t *fifo_filter_data, const struct bmi2_dev *dev); + +/*! + * @brief This API sets the down sampling rate for FIFO accelerometer or + * gyroscope FIFO data. + * + * @param[in] sens_sel : Selects either either accelerometer or + * gyroscope sensor. + * @param[in] fifo_down_samp : Variable to set the down sampling rate. + * @param[in] dev : Structure instance of bmi2_dev. + * + * sens_sel | values + * ----------------|---------- + * BMI2_ACCEL | 0x01 + * BMI2_GYRO | 0x02 + * + * @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 + */ +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 + * accelerometer or gyroscope data. + * + * @param[in] sens_sel : Selects either either accelerometer or + * gyroscope sensor. + * @param[out] fifo_down_samp : Pointer variable to store the down sampling rate + * @param[in] dev : Structure instance of bmi2_dev. + * + * sens_sel | values + * ----------------|---------- + * BMI2_ACCEL | 0x01 + * BMI2_GYRO | 0x02 + * + * @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_get_fifo_down_sample(uint8_t sens_sel, uint8_t *fifo_down_samp, const struct bmi2_dev *dev); + +/*! + * @brief 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 + * counter. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @note The byte counter is updated each time a complete frame is read or + * 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 + */ +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 + * address of auxiliary sensor in manual mode. + * + * @param[in] reg_addr : Address from where data is read. + * @param[out] aux_data : Pointer to the stored buffer. + * @param[in] len : Total length of data to be read. + * @param[in] dev : Structure instance of bmi2_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. + */ +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 + * auxiliary sensor where data is to be written in manual mode. + * + * @param[in] reg_addr : AUX address where data is to be written. + * @param[in] aux_data : Pointer to data to be written. + * @param[in] len : Total length of data to be written. + * @param[in] dev : Structure instance of bmi2_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. + */ +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 + * auxiliary sensor where data is to be written, from an interleaved input, + * in manual mode. + * + * @param[in] reg_addr : AUX address where data is to be written. + * @param[in] aux_data : Pointer to data to be written. + * @param[in] len : Total length of data to be written. + * @param[in] dev : Structure instance of bmi2_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. + */ +int8_t bmi2_write_aux_interleaved(uint8_t reg_addr, const uint8_t *aux_data, uint16_t len, struct bmi2_dev *dev); + +/*! + * @brief 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. + * + * Value | Status + * ---------|--------------------- + * 0x80 | DRDY_ACC + * 0x40 | DRDY_GYR + * 0x20 | DRDY_AUX + * 0x10 | CMD_RDY + * 0x04 | AUX_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 + */ +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, + * frequency and phase, resolution ratio, sync time and delay time. + * + * @param[in] command : Sync command to be written. + * @param[in] n_comm : Length of the command. + * @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_write_sync_commands(const uint8_t *command, uint8_t n_comm, struct bmi2_dev *dev); + +/*! + * @brief 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 + */ +int8_t bmi2_perform_accel_self_test(struct bmi2_dev *dev); + +/*! + * @brief 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 + */ +int8_t bmi2_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev); + +/*! + * @brief 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. + * + * data_int | Mask values + * ---------------------|--------------------- + * BMI2_FFULL_INT | 0x01 + * BMI2_FWM_INT | 0x02 + * BMI2_DRDY_INT | 0x04 + * BMI2_ERR_INT | 0x08 + * + * @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 + */ +int8_t bmi2_map_data_int(uint8_t data_int, enum bmi2_hw_int_pin int_pin, struct bmi2_dev *dev); + +/*! + * @brief 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 + */ +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 + * 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 + */ +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 + */ +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 + * 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. + * + * enable | Description + * -------------|--------------- + * BMI2_ENABLE | Enables gyroscope offset compensation. + * BMI2_DISABLE | Disables gyroscope offset compensation. + * + * @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_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. + * + * @param[out] gyr_off_comp_axes: Structure to store gyroscope offset + * compensated 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_offset_comp_axes(struct bmi2_sens_axes_data *gyr_off_comp_axes, const struct bmi2_dev *dev); + +/*! + * @brief 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 + * compensated 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_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 + */ +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 + * 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 + */ +int8_t bmi2_get_gyro_cross_sense(struct bmi2_dev *dev); + +/*! + * @brief 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. + * + * Internal status | *int_stat + * ---------------------|--------------------- + * BMI2_NOT_INIT | 0x00 + * BMI2_INIT_OK | 0x01 + * BMI2_INIT_ERR | 0x02 + * BMI2_DRV_ERR | 0x03 + * BMI2_SNS_STOP | 0x04 + * BMI2_NVM_ERROR | 0x05 + * BMI2_START_UP_ERROR | 0x06 + * BMI2_COMPAT_ERROR | 0x07 + * BMI2_VFM_SKIPPED | 0x10 + * BMI2_AXES_MAP_ERROR | 0x20 + * BMI2_ODR_50_HZ_ERROR | 0x40 + * BMI2_ODR_HIGH_ERROR | 0x80 + * + * @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_get_internal_status(uint8_t *int_stat, const struct bmi2_dev *dev); + +/*! + * @brief This API performs Fast Offset Compensation for accelerometer. + * + * @param[in] accel_g_value : This parameter selects the accel foc + * axis to be performed + * + * input format is {x, y, z, sign}. '1' to enable. '0' to disable + * + * eg to choose x axis {1, 0, 0, 0} + * eg to choose -x axis {1, 0, 0, 1} + * + * + * @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 + */ +int8_t bmi2_perform_accel_foc(const struct accel_foc_g_value *accel_g_value, struct bmi2_dev *dev); + +/*! + * @brief 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 + */ +int8_t bmi2_perform_gyro_foc(struct bmi2_dev *dev); + +/*! + * @brief 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 + * + * @note CRT calibration takes approximately 500ms & maximum time out configured as 2 seconds + */ +int8_t bmi2_do_crt(struct bmi2_dev *dev); + +/*! + * @brief 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 + */ + +int8_t bmi2_abort_crt_gyro_st(struct bmi2_dev *dev); + +/*! + * @brief 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 + */ +int8_t bmi2_do_gyro_st(struct bmi2_dev *dev); + +/*! @brief 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 + */ +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) + * + * @param[in] sett : Structure instance of act_recg_sett. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status. + * + * @retval BMI2_OK - Success. + * @retval < 0 - Fail + */ +int8_t bmi2_get_act_recg_sett(struct act_recg_sett *sett, 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) + * + * @param[in] sett : Structure instance of act_recg_sett. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status. + * + * @retval BMI2_OK - Success. + * @retval < 0 - Fail + */ +int8_t bmi2_set_act_recg_sett(const struct act_recg_sett *sett, struct bmi2_dev *dev); + +/******************************************************************************/ +/*! @name C++ Guard Macros */ +/******************************************************************************/ +#ifdef __cplusplus +} +#endif /* End of CPP guard */ + +#endif /* BMI2_H_ */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.c b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.c new file mode 100644 index 000000000..3ada58a86 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.c @@ -0,0 +1,640 @@ +/** +* 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.c +* @date 2020-01-10 +* @version v2.46.1 +* +*//***************************************************************************/ + +/*! Header files + ****************************************************************************/ +#include "bmi270.h" + +/***************************************************************************/ + +/*! Global Variable + ****************************************************************************/ + +/*! @name Global array that stores the configuration file of BMI270 */ +const uint8_t bmi270_config_file[] = { + 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x3d, 0xb1, 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x91, 0x03, 0x80, 0x2e, 0xbc, + 0xb0, 0x80, 0x2e, 0xa3, 0x03, 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x00, 0xb0, 0x50, 0x30, 0x21, 0x2e, 0x59, 0xf5, + 0x10, 0x30, 0x21, 0x2e, 0x6a, 0xf5, 0x80, 0x2e, 0x3b, 0x03, 0x00, 0x00, 0x00, 0x00, 0x08, 0x19, 0x01, 0x00, 0x22, + 0x00, 0x75, 0x00, 0x00, 0x10, 0x00, 0x10, 0xd1, 0x00, 0xb3, 0x43, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0xe0, 0x5f, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x92, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x08, 0x19, 0x00, 0x00, 0x88, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, + 0xe0, 0xaa, 0x38, 0x05, 0xe0, 0x90, 0x30, 0xfa, 0x00, 0x96, 0x00, 0x4b, 0x09, 0x11, 0x00, 0x11, 0x00, 0x02, 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, 0x32, 0x00, 0x05, 0x00, 0xee, + 0x06, 0x04, 0x00, 0xc8, 0x00, 0x00, 0x00, 0x04, 0x00, 0xa8, 0x05, 0xee, 0x06, 0x00, 0x04, 0xbc, 0x02, 0xb3, 0x00, + 0x85, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0xb4, 0x00, 0x01, 0x00, 0xb9, 0x00, 0x01, 0x00, 0x98, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x01, 0x00, 0x80, 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, 0x00, 0x00, 0x80, 0x2e, 0x00, 0xc1, 0xfd, 0x2d, 0xde, + 0x00, 0xeb, 0x00, 0xda, 0x00, 0x00, 0x0c, 0xff, 0x0f, 0x00, 0x04, 0xc0, 0x00, 0x5b, 0xf5, 0xc9, 0x01, 0x1e, 0xf2, + 0x80, 0x00, 0x3f, 0xff, 0x19, 0xf4, 0x58, 0xf5, 0x66, 0xf5, 0x64, 0xf5, 0xc0, 0xf1, 0xf0, 0x00, 0xe0, 0x00, 0xcd, + 0x01, 0xd3, 0x01, 0xdb, 0x01, 0xff, 0x7f, 0xff, 0x01, 0xe4, 0x00, 0x74, 0xf7, 0xf3, 0x00, 0xfa, 0x00, 0xff, 0x3f, + 0xca, 0x03, 0x6c, 0x38, 0x56, 0xfe, 0x44, 0xfd, 0xbc, 0x02, 0xf9, 0x06, 0x00, 0xfc, 0x12, 0x02, 0xae, 0x01, 0x58, + 0xfa, 0x9a, 0xfd, 0x77, 0x05, 0xbb, 0x02, 0x96, 0x01, 0x95, 0x01, 0x7f, 0x01, 0x82, 0x01, 0x89, 0x01, 0x87, 0x01, + 0x88, 0x01, 0x8a, 0x01, 0x8c, 0x01, 0x8f, 0x01, 0x8d, 0x01, 0x92, 0x01, 0x91, 0x01, 0xdd, 0x00, 0x9f, 0x01, 0x7e, + 0x01, 0xdb, 0x00, 0xb6, 0x01, 0x70, 0x69, 0x26, 0xd3, 0x9c, 0x07, 0x1f, 0x05, 0x9d, 0x00, 0x00, 0x08, 0xbc, 0x05, + 0x37, 0xfa, 0xa2, 0x01, 0xaa, 0x01, 0xa1, 0x01, 0xa8, 0x01, 0xa0, 0x01, 0xa8, 0x05, 0xb4, 0x01, 0xb4, 0x01, 0xce, + 0x00, 0xd0, 0x00, 0xfc, 0x00, 0xc5, 0x01, 0xff, 0xfb, 0xb1, 0x00, 0x00, 0x38, 0x00, 0x30, 0xfd, 0xf5, 0xfc, 0xf5, + 0xcd, 0x01, 0xa0, 0x00, 0x5f, 0xff, 0x00, 0x40, 0xff, 0x00, 0x00, 0x80, 0x6d, 0x0f, 0xeb, 0x00, 0x7f, 0xff, 0xc2, + 0xf5, 0x68, 0xf7, 0xb3, 0xf1, 0x67, 0x0f, 0x5b, 0x0f, 0x61, 0x0f, 0x80, 0x0f, 0x58, 0xf7, 0x5b, 0xf7, 0x83, 0x0f, + 0x86, 0x00, 0x72, 0x0f, 0x85, 0x0f, 0xc6, 0xf1, 0x7f, 0x0f, 0x6c, 0xf7, 0x00, 0xe0, 0x00, 0xff, 0xd1, 0xf5, 0x87, + 0x0f, 0x8a, 0x0f, 0xff, 0x03, 0xf0, 0x3f, 0x8b, 0x00, 0x8e, 0x00, 0x90, 0x00, 0xb9, 0x00, 0x2d, 0xf5, 0xca, 0xf5, + 0xcb, 0x01, 0x20, 0xf2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x30, 0x50, 0x98, 0x2e, + 0xd7, 0x0e, 0x50, 0x32, 0x98, 0x2e, 0xfa, 0x03, 0x00, 0x30, 0xf0, 0x7f, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x00, + 0x2e, 0x01, 0x80, 0x08, 0xa2, 0xfb, 0x2f, 0x98, 0x2e, 0xba, 0x03, 0x21, 0x2e, 0x19, 0x00, 0x01, 0x2e, 0xee, 0x00, + 0x00, 0xb2, 0x07, 0x2f, 0x01, 0x2e, 0x19, 0x00, 0x00, 0xb2, 0x03, 0x2f, 0x01, 0x50, 0x03, 0x52, 0x98, 0x2e, 0x07, + 0xcc, 0x01, 0x2e, 0xdd, 0x00, 0x00, 0xb2, 0x27, 0x2f, 0x05, 0x2e, 0x8a, 0x00, 0x05, 0x52, 0x98, 0x2e, 0xc7, 0xc1, + 0x03, 0x2e, 0xe9, 0x00, 0x40, 0xb2, 0xf0, 0x7f, 0x08, 0x2f, 0x01, 0x2e, 0x19, 0x00, 0x00, 0xb2, 0x04, 0x2f, 0x00, + 0x30, 0x21, 0x2e, 0xe9, 0x00, 0x98, 0x2e, 0xb4, 0xb1, 0x01, 0x2e, 0x18, 0x00, 0x00, 0xb2, 0x10, 0x2f, 0x05, 0x50, + 0x98, 0x2e, 0x4d, 0xc3, 0x05, 0x50, 0x98, 0x2e, 0x5a, 0xc7, 0x98, 0x2e, 0xf9, 0xb4, 0x98, 0x2e, 0x54, 0xb2, 0x98, + 0x2e, 0x67, 0xb6, 0x98, 0x2e, 0x17, 0xb2, 0x10, 0x30, 0x21, 0x2e, 0x77, 0x00, 0x01, 0x2e, 0xef, 0x00, 0x00, 0xb2, + 0x04, 0x2f, 0x98, 0x2e, 0x7a, 0xb7, 0x00, 0x30, 0x21, 0x2e, 0xef, 0x00, 0x01, 0x2e, 0xd4, 0x00, 0x04, 0xae, 0x0b, + 0x2f, 0x01, 0x2e, 0xdd, 0x00, 0x00, 0xb2, 0x07, 0x2f, 0x05, 0x52, 0x98, 0x2e, 0x8e, 0x0e, 0x00, 0xb2, 0x02, 0x2f, + 0x10, 0x30, 0x21, 0x2e, 0x7d, 0x00, 0x01, 0x2e, 0x7d, 0x00, 0x00, 0x90, 0x90, 0x2e, 0xf1, 0x02, 0x01, 0x2e, 0xd7, + 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, 0xd4, 0x00, 0x00, 0x90, 0x02, 0x2f, 0x98, 0x2e, 0x1f, 0x0e, 0x09, 0x2d, 0x98, + 0x2e, 0x81, 0x0d, 0x01, 0x2e, 0xd4, 0x00, 0x04, 0x90, 0x02, 0x2f, 0x50, 0x32, 0x98, 0x2e, 0xfa, 0x03, 0x00, 0x30, + 0x21, 0x2e, 0x7b, 0x00, 0x01, 0x2e, 0x7c, 0x00, 0x00, 0xb2, 0x90, 0x2e, 0x09, 0x03, 0x01, 0x2e, 0x7c, 0x00, 0x01, + 0x31, 0x01, 0x08, 0x00, 0xb2, 0x04, 0x2f, 0x98, 0x2e, 0x47, 0xcb, 0x10, 0x30, 0x21, 0x2e, 0x77, 0x00, 0x81, 0x30, + 0x01, 0x2e, 0x7c, 0x00, 0x01, 0x08, 0x00, 0xb2, 0x61, 0x2f, 0x03, 0x2e, 0x89, 0x00, 0x01, 0x2e, 0xd4, 0x00, 0x98, + 0xbc, 0x98, 0xb8, 0x05, 0xb2, 0x0f, 0x58, 0x23, 0x2f, 0x07, 0x90, 0x09, 0x54, 0x00, 0x30, 0x37, 0x2f, 0x15, 0x41, + 0x04, 0x41, 0xdc, 0xbe, 0x44, 0xbe, 0xdc, 0xba, 0x2c, 0x01, 0x61, 0x00, 0x0f, 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, 0xc3, 0xb7, 0x21, 0x2d, 0x61, 0x30, 0x23, 0x2e, 0xd4, 0x00, 0x98, 0x2e, 0xc3, 0xb7, 0x00, 0x30, 0x21, + 0x2e, 0x5a, 0xf5, 0x18, 0x2d, 0xe1, 0x7f, 0x50, 0x30, 0x98, 0x2e, 0xfa, 0x03, 0x0f, 0x52, 0x07, 0x50, 0x50, 0x42, + 0x70, 0x30, 0x0d, 0x54, 0x42, 0x42, 0x7e, 0x82, 0xe2, 0x6f, 0x80, 0xb2, 0x42, 0x42, 0x05, 0x2f, 0x21, 0x2e, 0xd4, + 0x00, 0x10, 0x30, 0x98, 0x2e, 0xc3, 0xb7, 0x03, 0x2d, 0x60, 0x30, 0x21, 0x2e, 0xd4, 0x00, 0x01, 0x2e, 0xd4, 0x00, + 0x06, 0x90, 0x18, 0x2f, 0x01, 0x2e, 0x76, 0x00, 0x0b, 0x54, 0x07, 0x52, 0xe0, 0x7f, 0x98, 0x2e, 0x7a, 0xc1, 0xe1, + 0x6f, 0x08, 0x1a, 0x40, 0x30, 0x08, 0x2f, 0x21, 0x2e, 0xd4, 0x00, 0x20, 0x30, 0x98, 0x2e, 0xaf, 0xb7, 0x50, 0x32, + 0x98, 0x2e, 0xfa, 0x03, 0x05, 0x2d, 0x98, 0x2e, 0x38, 0x0e, 0x00, 0x30, 0x21, 0x2e, 0xd4, 0x00, 0x00, 0x30, 0x21, + 0x2e, 0x7c, 0x00, 0x18, 0x2d, 0x01, 0x2e, 0xd4, 0x00, 0x03, 0xaa, 0x01, 0x2f, 0x98, 0x2e, 0x45, 0x0e, 0x01, 0x2e, + 0xd4, 0x00, 0x3f, 0x80, 0x03, 0xa2, 0x01, 0x2f, 0x00, 0x2e, 0x02, 0x2d, 0x98, 0x2e, 0x5b, 0x0e, 0x30, 0x30, 0x98, + 0x2e, 0xce, 0xb7, 0x00, 0x30, 0x21, 0x2e, 0x7d, 0x00, 0x50, 0x32, 0x98, 0x2e, 0xfa, 0x03, 0x01, 0x2e, 0x77, 0x00, + 0x00, 0xb2, 0x24, 0x2f, 0x98, 0x2e, 0xf5, 0xcb, 0x03, 0x2e, 0xd5, 0x00, 0x11, 0x54, 0x01, 0x0a, 0xbc, 0x84, 0x83, + 0x86, 0x21, 0x2e, 0xc9, 0x01, 0xe0, 0x40, 0x13, 0x52, 0xc4, 0x40, 0x82, 0x40, 0xa8, 0xb9, 0x52, 0x42, 0x43, 0xbe, + 0x53, 0x42, 0x04, 0x0a, 0x50, 0x42, 0xe1, 0x7f, 0xf0, 0x31, 0x41, 0x40, 0xf2, 0x6f, 0x25, 0xbd, 0x08, 0x08, 0x02, + 0x0a, 0xd0, 0x7f, 0x98, 0x2e, 0xa8, 0xcf, 0x06, 0xbc, 0xd1, 0x6f, 0xe2, 0x6f, 0x08, 0x0a, 0x80, 0x42, 0x98, 0x2e, + 0x58, 0xb7, 0x00, 0x30, 0x21, 0x2e, 0xee, 0x00, 0x21, 0x2e, 0x77, 0x00, 0x21, 0x2e, 0xdd, 0x00, 0x80, 0x2e, 0xf4, + 0x01, 0x1a, 0x24, 0x22, 0x00, 0x80, 0x2e, 0xec, 0x01, 0x10, 0x50, 0xfb, 0x7f, 0x98, 0x2e, 0xf3, 0x03, 0x57, 0x50, + 0xfb, 0x6f, 0x01, 0x30, 0x71, 0x54, 0x11, 0x42, 0x42, 0x0e, 0xfc, 0x2f, 0xc0, 0x2e, 0x01, 0x42, 0xf0, 0x5f, 0x80, + 0x2e, 0x00, 0xc1, 0xfd, 0x2d, 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, 0x06, 0x32, 0x0f, 0x2e, 0x61, 0xf5, 0xfe, 0x09, 0xc0, 0xb3, 0x04, + 0x2f, 0x17, 0x30, 0x2f, 0x2e, 0xef, 0x00, 0x2d, 0x2e, 0x61, 0xf5, 0xf6, 0x6f, 0xe7, 0x6f, 0xe0, 0x5f, 0xc8, 0x2e, + 0x20, 0x50, 0xe7, 0x7f, 0xf6, 0x7f, 0x46, 0x30, 0x0f, 0x2e, 0xa4, 0xf1, 0xbe, 0x09, 0x80, 0xb3, 0x06, 0x2f, 0x0d, + 0x2e, 0xd4, 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, 0x01, 0x2e, 0x77, 0xf7, 0x09, 0xbc, 0x0f, 0xb8, 0x00, 0xb2, 0x10, + 0x50, 0xfb, 0x7f, 0x10, 0x30, 0x0b, 0x2f, 0x03, 0x2e, 0x8a, 0x00, 0x96, 0xbc, 0x9f, 0xb8, 0x40, 0xb2, 0x05, 0x2f, + 0x03, 0x2e, 0x68, 0xf7, 0x9e, 0xbc, 0x9f, 0xb8, 0x40, 0xb2, 0x07, 0x2f, 0x03, 0x2e, 0x7e, 0x00, 0x41, 0x90, 0x01, + 0x2f, 0x98, 0x2e, 0xdc, 0x03, 0x03, 0x2c, 0x00, 0x30, 0x21, 0x2e, 0x7e, 0x00, 0xfb, 0x6f, 0xf0, 0x5f, 0xb8, 0x2e, + 0x20, 0x50, 0xe0, 0x7f, 0xfb, 0x7f, 0x00, 0x2e, 0x27, 0x50, 0x98, 0x2e, 0x3b, 0xc8, 0x29, 0x50, 0x98, 0x2e, 0xa7, + 0xc8, 0x01, 0x50, 0x98, 0x2e, 0x55, 0xcc, 0xe1, 0x6f, 0x2b, 0x50, 0x98, 0x2e, 0xe0, 0xc9, 0xfb, 0x6f, 0x00, 0x30, + 0xe0, 0x5f, 0x21, 0x2e, 0x7e, 0x00, 0xb8, 0x2e, 0x73, 0x50, 0x01, 0x30, 0x57, 0x54, 0x11, 0x42, 0x42, 0x0e, 0xfc, + 0x2f, 0xb8, 0x2e, 0x21, 0x2e, 0x59, 0xf5, 0x10, 0x30, 0xc0, 0x2e, 0x21, 0x2e, 0x4a, 0xf1, 0x90, 0x50, 0xf7, 0x7f, + 0xe6, 0x7f, 0xd5, 0x7f, 0xc4, 0x7f, 0xb3, 0x7f, 0xa1, 0x7f, 0x90, 0x7f, 0x82, 0x7f, 0x7b, 0x7f, 0x98, 0x2e, 0x35, + 0xb7, 0x00, 0xb2, 0x90, 0x2e, 0x97, 0xb0, 0x03, 0x2e, 0x8f, 0x00, 0x07, 0x2e, 0x91, 0x00, 0x05, 0x2e, 0xb1, 0x00, + 0x3f, 0xba, 0x9f, 0xb8, 0x01, 0x2e, 0xb1, 0x00, 0xa3, 0xbd, 0x4c, 0x0a, 0x05, 0x2e, 0xb1, 0x00, 0x04, 0xbe, 0xbf, + 0xb9, 0xcb, 0x0a, 0x4f, 0xba, 0x22, 0xbd, 0x01, 0x2e, 0xb3, 0x00, 0xdc, 0x0a, 0x2f, 0xb9, 0x03, 0x2e, 0xb8, 0x00, + 0x0a, 0xbe, 0x9a, 0x0a, 0xcf, 0xb9, 0x9b, 0xbc, 0x01, 0x2e, 0x97, 0x00, 0x9f, 0xb8, 0x93, 0x0a, 0x0f, 0xbc, 0x91, + 0x0a, 0x0f, 0xb8, 0x90, 0x0a, 0x25, 0x2e, 0x18, 0x00, 0x05, 0x2e, 0xc1, 0xf5, 0x2e, 0xbd, 0x2e, 0xb9, 0x01, 0x2e, + 0x19, 0x00, 0x31, 0x30, 0x8a, 0x04, 0x00, 0x90, 0x07, 0x2f, 0x01, 0x2e, 0xd4, 0x00, 0x04, 0xa2, 0x03, 0x2f, 0x01, + 0x2e, 0x18, 0x00, 0x00, 0xb2, 0x0c, 0x2f, 0x19, 0x50, 0x05, 0x52, 0x98, 0x2e, 0x4d, 0xb7, 0x05, 0x2e, 0x78, 0x00, + 0x80, 0x90, 0x10, 0x30, 0x01, 0x2f, 0x21, 0x2e, 0x78, 0x00, 0x25, 0x2e, 0xdd, 0x00, 0x98, 0x2e, 0x3e, 0xb7, 0x00, + 0xb2, 0x02, 0x30, 0x01, 0x30, 0x04, 0x2f, 0x01, 0x2e, 0x19, 0x00, 0x00, 0xb2, 0x00, 0x2f, 0x21, 0x30, 0x01, 0x2e, + 0xea, 0x00, 0x08, 0x1a, 0x0e, 0x2f, 0x23, 0x2e, 0xea, 0x00, 0x33, 0x30, 0x1b, 0x50, 0x0b, 0x09, 0x01, 0x40, 0x17, + 0x56, 0x46, 0xbe, 0x4b, 0x08, 0x4c, 0x0a, 0x01, 0x42, 0x0a, 0x80, 0x15, 0x52, 0x01, 0x42, 0x00, 0x2e, 0x01, 0x2e, + 0x18, 0x00, 0x00, 0xb2, 0x1f, 0x2f, 0x03, 0x2e, 0xc0, 0xf5, 0xf0, 0x30, 0x48, 0x08, 0x47, 0xaa, 0x74, 0x30, 0x07, + 0x2e, 0x7a, 0x00, 0x61, 0x22, 0x4b, 0x1a, 0x05, 0x2f, 0x07, 0x2e, 0x66, 0xf5, 0xbf, 0xbd, 0xbf, 0xb9, 0xc0, 0x90, + 0x0b, 0x2f, 0x1d, 0x56, 0x2b, 0x30, 0xd2, 0x42, 0xdb, 0x42, 0x01, 0x04, 0xc2, 0x42, 0x04, 0xbd, 0xfe, 0x80, 0x81, + 0x84, 0x23, 0x2e, 0x7a, 0x00, 0x02, 0x42, 0x02, 0x32, 0x25, 0x2e, 0x62, 0xf5, 0x05, 0x2e, 0xd6, 0x00, 0x81, 0x84, + 0x25, 0x2e, 0xd6, 0x00, 0x02, 0x31, 0x25, 0x2e, 0x60, 0xf5, 0x05, 0x2e, 0x8a, 0x00, 0x0b, 0x50, 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, 0xc0, 0x50, 0x90, 0x7f, 0xe5, 0x7f, 0xd4, 0x7f, + 0xc3, 0x7f, 0xb1, 0x7f, 0xa2, 0x7f, 0x87, 0x7f, 0xf6, 0x7f, 0x7b, 0x7f, 0x00, 0x2e, 0x01, 0x2e, 0x60, 0xf5, 0x60, + 0x7f, 0x98, 0x2e, 0x35, 0xb7, 0x02, 0x30, 0x63, 0x6f, 0x15, 0x52, 0x50, 0x7f, 0x62, 0x7f, 0x5a, 0x2c, 0x02, 0x32, + 0x1a, 0x09, 0x00, 0xb3, 0x14, 0x2f, 0x00, 0xb2, 0x03, 0x2f, 0x09, 0x2e, 0x18, 0x00, 0x00, 0x91, 0x0c, 0x2f, 0x43, + 0x7f, 0x98, 0x2e, 0x97, 0xb7, 0x1f, 0x50, 0x02, 0x8a, 0x02, 0x32, 0x04, 0x30, 0x25, 0x2e, 0x64, 0xf5, 0x15, 0x52, + 0x50, 0x6f, 0x43, 0x6f, 0x44, 0x43, 0x25, 0x2e, 0x60, 0xf5, 0xd9, 0x08, 0xc0, 0xb2, 0x36, 0x2f, 0x98, 0x2e, 0x3e, + 0xb7, 0x00, 0xb2, 0x06, 0x2f, 0x01, 0x2e, 0x19, 0x00, 0x00, 0xb2, 0x02, 0x2f, 0x50, 0x6f, 0x00, 0x90, 0x0a, 0x2f, + 0x01, 0x2e, 0x79, 0x00, 0x00, 0x90, 0x19, 0x2f, 0x10, 0x30, 0x21, 0x2e, 0x79, 0x00, 0x00, 0x30, 0x98, 0x2e, 0xdc, + 0x03, 0x13, 0x2d, 0x01, 0x2e, 0xc3, 0xf5, 0x0c, 0xbc, 0x0f, 0xb8, 0x12, 0x30, 0x10, 0x04, 0x03, 0xb0, 0x26, 0x25, + 0x21, 0x50, 0x03, 0x52, 0x98, 0x2e, 0x4d, 0xb7, 0x10, 0x30, 0x21, 0x2e, 0xee, 0x00, 0x02, 0x30, 0x60, 0x7f, 0x25, + 0x2e, 0x79, 0x00, 0x60, 0x6f, 0x00, 0x90, 0x05, 0x2f, 0x00, 0x30, 0x21, 0x2e, 0xea, 0x00, 0x15, 0x50, 0x21, 0x2e, + 0x64, 0xf5, 0x15, 0x52, 0x23, 0x2e, 0x60, 0xf5, 0x02, 0x32, 0x50, 0x6f, 0x00, 0x90, 0x02, 0x2f, 0x03, 0x30, 0x27, + 0x2e, 0x78, 0x00, 0x07, 0x2e, 0x60, 0xf5, 0x1a, 0x09, 0x00, 0x91, 0xa3, 0x2f, 0x19, 0x09, 0x00, 0x91, 0xa0, 0x2f, + 0x90, 0x6f, 0xa2, 0x6f, 0xb1, 0x6f, 0xc3, 0x6f, 0xd4, 0x6f, 0xe5, 0x6f, 0x7b, 0x6f, 0xf6, 0x6f, 0x87, 0x6f, 0x40, + 0x5f, 0xc8, 0x2e, 0xc0, 0x50, 0xe7, 0x7f, 0xf6, 0x7f, 0x26, 0x30, 0x0f, 0x2e, 0x61, 0xf5, 0x2f, 0x2e, 0x7c, 0x00, + 0x0f, 0x2e, 0x7c, 0x00, 0xbe, 0x09, 0xa2, 0x7f, 0x80, 0x7f, 0x80, 0xb3, 0xd5, 0x7f, 0xc4, 0x7f, 0xb3, 0x7f, 0x91, + 0x7f, 0x7b, 0x7f, 0x0b, 0x2f, 0x23, 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, 0x7c, 0x00, 0x01, 0x08, 0x00, + 0xb2, 0x42, 0x2f, 0x03, 0x2e, 0x89, 0x00, 0x01, 0x2e, 0x89, 0x00, 0x97, 0xbc, 0x06, 0xbc, 0x9f, 0xb8, 0x0f, 0xb8, + 0x00, 0x90, 0x23, 0x2e, 0xd8, 0x00, 0x10, 0x30, 0x01, 0x30, 0x2a, 0x2f, 0x03, 0x2e, 0xd4, 0x00, 0x44, 0xb2, 0x05, + 0x2f, 0x47, 0xb2, 0x00, 0x30, 0x2d, 0x2f, 0x21, 0x2e, 0x7c, 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, 0x25, 0x54, 0x4a, 0x08, 0x40, 0x90, 0x08, 0x2f, 0x98, 0x2e, 0x35, 0xb7, 0x00, 0xb2, 0x10, 0x30, + 0x03, 0x2f, 0x50, 0x30, 0x21, 0x2e, 0xd4, 0x00, 0x10, 0x2d, 0x98, 0x2e, 0xaf, 0xb7, 0x00, 0x30, 0x21, 0x2e, 0x7c, + 0x00, 0x0a, 0x2d, 0x05, 0x2e, 0x69, 0xf7, 0x2d, 0xbd, 0x2f, 0xb9, 0x80, 0xb2, 0x01, 0x2f, 0x21, 0x2e, 0x7d, 0x00, + 0x23, 0x2e, 0x7c, 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, 0x60, 0x51, 0x0a, 0x25, 0x36, 0x88, + 0xf4, 0x7f, 0xeb, 0x7f, 0x00, 0x32, 0x31, 0x52, 0x32, 0x30, 0x13, 0x30, 0x98, 0x2e, 0x15, 0xcb, 0x0a, 0x25, 0x33, + 0x84, 0xd2, 0x7f, 0x43, 0x30, 0x05, 0x50, 0x2d, 0x52, 0x98, 0x2e, 0x95, 0xc1, 0xd2, 0x6f, 0x27, 0x52, 0x98, 0x2e, + 0xd7, 0xc7, 0x2a, 0x25, 0xb0, 0x86, 0xc0, 0x7f, 0xd3, 0x7f, 0xaf, 0x84, 0x29, 0x50, 0xf1, 0x6f, 0x98, 0x2e, 0x4d, + 0xc8, 0x2a, 0x25, 0xae, 0x8a, 0xaa, 0x88, 0xf2, 0x6e, 0x2b, 0x50, 0xc1, 0x6f, 0xd3, 0x6f, 0xf4, 0x7f, 0x98, 0x2e, + 0xb6, 0xc8, 0xe0, 0x6e, 0x00, 0xb2, 0x32, 0x2f, 0x33, 0x54, 0x83, 0x86, 0xf1, 0x6f, 0xc3, 0x7f, 0x04, 0x30, 0x30, + 0x30, 0xf4, 0x7f, 0xd0, 0x7f, 0xb2, 0x7f, 0xe3, 0x30, 0xc5, 0x6f, 0x56, 0x40, 0x45, 0x41, 0x28, 0x08, 0x03, 0x14, + 0x0e, 0xb4, 0x08, 0xbc, 0x82, 0x40, 0x10, 0x0a, 0x2f, 0x54, 0x26, 0x05, 0x91, 0x7f, 0x44, 0x28, 0xa3, 0x7f, 0x98, + 0x2e, 0xd9, 0xc0, 0x08, 0xb9, 0x33, 0x30, 0x53, 0x09, 0xc1, 0x6f, 0xd3, 0x6f, 0xf4, 0x6f, 0x83, 0x17, 0x47, 0x40, + 0x6c, 0x15, 0xb2, 0x6f, 0xbe, 0x09, 0x75, 0x0b, 0x90, 0x42, 0x45, 0x42, 0x51, 0x0e, 0x32, 0xbc, 0x02, 0x89, 0xa1, + 0x6f, 0x7e, 0x86, 0xf4, 0x7f, 0xd0, 0x7f, 0xb2, 0x7f, 0x04, 0x30, 0x91, 0x6f, 0xd6, 0x2f, 0xeb, 0x6f, 0xa0, 0x5e, + 0xb8, 0x2e, 0x03, 0x2e, 0x97, 0x00, 0x1b, 0xbc, 0x60, 0x50, 0x9f, 0xbc, 0x0c, 0xb8, 0xf0, 0x7f, 0x40, 0xb2, 0xeb, + 0x7f, 0x2b, 0x2f, 0x03, 0x2e, 0x7f, 0x00, 0x41, 0x40, 0x01, 0x2e, 0xc8, 0x00, 0x01, 0x1a, 0x11, 0x2f, 0x37, 0x58, + 0x23, 0x2e, 0xc8, 0x00, 0x10, 0x41, 0xa0, 0x7f, 0x38, 0x81, 0x01, 0x41, 0xd0, 0x7f, 0xb1, 0x7f, 0x98, 0x2e, 0x64, + 0xcf, 0xd0, 0x6f, 0x07, 0x80, 0xa1, 0x6f, 0x11, 0x42, 0x00, 0x2e, 0xb1, 0x6f, 0x01, 0x42, 0x11, 0x30, 0x01, 0x2e, + 0xfc, 0x00, 0x00, 0xa8, 0x03, 0x30, 0xcb, 0x22, 0x4a, 0x25, 0x01, 0x2e, 0x7f, 0x00, 0x3c, 0x89, 0x35, 0x52, 0x05, + 0x54, 0x98, 0x2e, 0xc4, 0xce, 0xc1, 0x6f, 0xf0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, 0x04, 0x2d, 0x01, 0x30, 0xf0, 0x6f, + 0x98, 0x2e, 0x95, 0xcf, 0xeb, 0x6f, 0xa0, 0x5f, 0xb8, 0x2e, 0x03, 0x2e, 0xb3, 0x00, 0x02, 0x32, 0xf0, 0x30, 0x03, + 0x31, 0x30, 0x50, 0x8a, 0x08, 0x08, 0x08, 0xcb, 0x08, 0xe0, 0x7f, 0x80, 0xb2, 0xf3, 0x7f, 0xdb, 0x7f, 0x25, 0x2f, + 0x03, 0x2e, 0xca, 0x00, 0x41, 0x90, 0x04, 0x2f, 0x01, 0x30, 0x23, 0x2e, 0xca, 0x00, 0x98, 0x2e, 0x3f, 0x03, 0xc0, + 0xb2, 0x05, 0x2f, 0x03, 0x2e, 0xda, 0x00, 0x00, 0x30, 0x41, 0x04, 0x23, 0x2e, 0xda, 0x00, 0x98, 0x2e, 0x92, 0xb2, + 0x10, 0x25, 0xf0, 0x6f, 0x00, 0xb2, 0x05, 0x2f, 0x01, 0x2e, 0xda, 0x00, 0x02, 0x30, 0x10, 0x04, 0x21, 0x2e, 0xda, + 0x00, 0x40, 0xb2, 0x01, 0x2f, 0x23, 0x2e, 0xc8, 0x01, 0xdb, 0x6f, 0xe0, 0x6f, 0xd0, 0x5f, 0x80, 0x2e, 0x95, 0xcf, + 0x01, 0x30, 0xe0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, 0x11, 0x30, 0x23, 0x2e, 0xca, 0x00, 0xdb, 0x6f, 0xd0, 0x5f, 0xb8, + 0x2e, 0xd0, 0x50, 0x0a, 0x25, 0x33, 0x84, 0x55, 0x50, 0xd2, 0x7f, 0xe2, 0x7f, 0x03, 0x8c, 0xc0, 0x7f, 0xbb, 0x7f, + 0x00, 0x30, 0x05, 0x5a, 0x39, 0x54, 0x51, 0x41, 0xa5, 0x7f, 0x96, 0x7f, 0x80, 0x7f, 0x98, 0x2e, 0xd9, 0xc0, 0x05, + 0x30, 0xf5, 0x7f, 0x20, 0x25, 0x91, 0x6f, 0x3b, 0x58, 0x3d, 0x5c, 0x3b, 0x56, 0x98, 0x2e, 0x67, 0xcc, 0xc1, 0x6f, + 0xd5, 0x6f, 0x52, 0x40, 0x50, 0x43, 0xc1, 0x7f, 0xd5, 0x7f, 0x10, 0x25, 0x98, 0x2e, 0xfe, 0xc9, 0x10, 0x25, 0x98, + 0x2e, 0x74, 0xc0, 0x86, 0x6f, 0x30, 0x28, 0x92, 0x6f, 0x82, 0x8c, 0xa5, 0x6f, 0x6f, 0x52, 0x69, 0x0e, 0x39, 0x54, + 0xdb, 0x2f, 0x19, 0xa0, 0x15, 0x30, 0x03, 0x2f, 0x00, 0x30, 0x21, 0x2e, 0x81, 0x01, 0x0a, 0x2d, 0x01, 0x2e, 0x81, + 0x01, 0x05, 0x28, 0x42, 0x36, 0x21, 0x2e, 0x81, 0x01, 0x02, 0x0e, 0x01, 0x2f, 0x98, 0x2e, 0xf3, 0x03, 0x57, 0x50, + 0x12, 0x30, 0x01, 0x40, 0x98, 0x2e, 0xfe, 0xc9, 0x51, 0x6f, 0x0b, 0x5c, 0x8e, 0x0e, 0x3b, 0x6f, 0x57, 0x58, 0x02, + 0x30, 0x21, 0x2e, 0x95, 0x01, 0x45, 0x6f, 0x2a, 0x8d, 0xd2, 0x7f, 0xcb, 0x7f, 0x13, 0x2f, 0x02, 0x30, 0x3f, 0x50, + 0xd2, 0x7f, 0xa8, 0x0e, 0x0e, 0x2f, 0xc0, 0x6f, 0x53, 0x54, 0x02, 0x00, 0x51, 0x54, 0x42, 0x0e, 0x10, 0x30, 0x59, + 0x52, 0x02, 0x30, 0x01, 0x2f, 0x00, 0x2e, 0x03, 0x2d, 0x50, 0x42, 0x42, 0x42, 0x12, 0x30, 0xd2, 0x7f, 0x80, 0xb2, + 0x03, 0x2f, 0x00, 0x30, 0x21, 0x2e, 0x80, 0x01, 0x12, 0x2d, 0x01, 0x2e, 0xc9, 0x00, 0x02, 0x80, 0x05, 0x2e, 0x80, + 0x01, 0x11, 0x30, 0x91, 0x28, 0x00, 0x40, 0x25, 0x2e, 0x80, 0x01, 0x10, 0x0e, 0x05, 0x2f, 0x01, 0x2e, 0x7f, 0x01, + 0x01, 0x90, 0x01, 0x2f, 0x98, 0x2e, 0xf3, 0x03, 0x00, 0x2e, 0xa0, 0x41, 0x01, 0x90, 0xa6, 0x7f, 0x90, 0x2e, 0xe3, + 0xb4, 0x01, 0x2e, 0x95, 0x01, 0x00, 0xa8, 0x90, 0x2e, 0xe3, 0xb4, 0x5b, 0x54, 0x95, 0x80, 0x82, 0x40, 0x80, 0xb2, + 0x02, 0x40, 0x2d, 0x8c, 0x3f, 0x52, 0x96, 0x7f, 0x90, 0x2e, 0xc2, 0xb3, 0x29, 0x0e, 0x76, 0x2f, 0x01, 0x2e, 0xc9, + 0x00, 0x00, 0x40, 0x81, 0x28, 0x45, 0x52, 0xb3, 0x30, 0x98, 0x2e, 0x0f, 0xca, 0x5d, 0x54, 0x80, 0x7f, 0x00, 0x2e, + 0xa1, 0x40, 0x72, 0x7f, 0x82, 0x80, 0x82, 0x40, 0x60, 0x7f, 0x98, 0x2e, 0xfe, 0xc9, 0x10, 0x25, 0x98, 0x2e, 0x74, + 0xc0, 0x62, 0x6f, 0x05, 0x30, 0x87, 0x40, 0xc0, 0x91, 0x04, 0x30, 0x05, 0x2f, 0x05, 0x2e, 0x83, 0x01, 0x80, 0xb2, + 0x14, 0x30, 0x00, 0x2f, 0x04, 0x30, 0x05, 0x2e, 0xc9, 0x00, 0x73, 0x6f, 0x81, 0x40, 0xe2, 0x40, 0x69, 0x04, 0x11, + 0x0f, 0xe1, 0x40, 0x16, 0x30, 0xfe, 0x29, 0xcb, 0x40, 0x02, 0x2f, 0x83, 0x6f, 0x83, 0x0f, 0x22, 0x2f, 0x47, 0x56, + 0x13, 0x0f, 0x12, 0x30, 0x77, 0x2f, 0x49, 0x54, 0x42, 0x0e, 0x12, 0x30, 0x73, 0x2f, 0x00, 0x91, 0x0a, 0x2f, 0x01, + 0x2e, 0x8b, 0x01, 0x19, 0xa8, 0x02, 0x30, 0x6c, 0x2f, 0x63, 0x50, 0x00, 0x2e, 0x17, 0x42, 0x05, 0x42, 0x68, 0x2c, + 0x12, 0x30, 0x0b, 0x25, 0x08, 0x0f, 0x50, 0x30, 0x02, 0x2f, 0x21, 0x2e, 0x83, 0x01, 0x03, 0x2d, 0x40, 0x30, 0x21, + 0x2e, 0x83, 0x01, 0x2b, 0x2e, 0x85, 0x01, 0x5a, 0x2c, 0x12, 0x30, 0x00, 0x91, 0x2b, 0x25, 0x04, 0x2f, 0x63, 0x50, + 0x02, 0x30, 0x17, 0x42, 0x17, 0x2c, 0x02, 0x42, 0x98, 0x2e, 0xfe, 0xc9, 0x10, 0x25, 0x98, 0x2e, 0x74, 0xc0, 0x05, + 0x2e, 0xc9, 0x00, 0x81, 0x84, 0x5b, 0x30, 0x82, 0x40, 0x37, 0x2e, 0x83, 0x01, 0x02, 0x0e, 0x07, 0x2f, 0x5f, 0x52, + 0x40, 0x30, 0x62, 0x40, 0x41, 0x40, 0x91, 0x0e, 0x01, 0x2f, 0x21, 0x2e, 0x83, 0x01, 0x05, 0x30, 0x2b, 0x2e, 0x85, + 0x01, 0x12, 0x30, 0x36, 0x2c, 0x16, 0x30, 0x15, 0x25, 0x81, 0x7f, 0x98, 0x2e, 0xfe, 0xc9, 0x10, 0x25, 0x98, 0x2e, + 0x74, 0xc0, 0x19, 0xa2, 0x16, 0x30, 0x15, 0x2f, 0x05, 0x2e, 0x97, 0x01, 0x80, 0x6f, 0x82, 0x0e, 0x05, 0x2f, 0x01, + 0x2e, 0x86, 0x01, 0x06, 0x28, 0x21, 0x2e, 0x86, 0x01, 0x0b, 0x2d, 0x03, 0x2e, 0x87, 0x01, 0x5f, 0x54, 0x4e, 0x28, + 0x91, 0x42, 0x00, 0x2e, 0x82, 0x40, 0x90, 0x0e, 0x01, 0x2f, 0x21, 0x2e, 0x88, 0x01, 0x02, 0x30, 0x13, 0x2c, 0x05, + 0x30, 0xc0, 0x6f, 0x08, 0x1c, 0xa8, 0x0f, 0x16, 0x30, 0x05, 0x30, 0x5b, 0x50, 0x09, 0x2f, 0x02, 0x80, 0x2d, 0x2e, + 0x82, 0x01, 0x05, 0x42, 0x05, 0x80, 0x00, 0x2e, 0x02, 0x42, 0x3e, 0x80, 0x00, 0x2e, 0x06, 0x42, 0x02, 0x30, 0x90, + 0x6f, 0x3e, 0x88, 0x01, 0x40, 0x04, 0x41, 0x4c, 0x28, 0x01, 0x42, 0x07, 0x80, 0x10, 0x25, 0x24, 0x40, 0x00, 0x40, + 0x00, 0xa8, 0xf5, 0x22, 0x23, 0x29, 0x44, 0x42, 0x7a, 0x82, 0x7e, 0x88, 0x43, 0x40, 0x04, 0x41, 0x00, 0xab, 0xf5, + 0x23, 0xdf, 0x28, 0x43, 0x42, 0xd9, 0xa0, 0x14, 0x2f, 0x00, 0x90, 0x02, 0x2f, 0xd2, 0x6f, 0x81, 0xb2, 0x05, 0x2f, + 0x63, 0x54, 0x06, 0x28, 0x90, 0x42, 0x85, 0x42, 0x09, 0x2c, 0x02, 0x30, 0x5b, 0x50, 0x03, 0x80, 0x29, 0x2e, 0x7e, + 0x01, 0x2b, 0x2e, 0x82, 0x01, 0x05, 0x42, 0x12, 0x30, 0x2b, 0x2e, 0x83, 0x01, 0x45, 0x82, 0x00, 0x2e, 0x40, 0x40, + 0x7a, 0x82, 0x02, 0xa0, 0x08, 0x2f, 0x63, 0x50, 0x3b, 0x30, 0x15, 0x42, 0x05, 0x42, 0x37, 0x80, 0x37, 0x2e, 0x7e, + 0x01, 0x05, 0x42, 0x12, 0x30, 0x01, 0x2e, 0xc9, 0x00, 0x02, 0x8c, 0x40, 0x40, 0x84, 0x41, 0x7a, 0x8c, 0x04, 0x0f, + 0x03, 0x2f, 0x01, 0x2e, 0x8b, 0x01, 0x19, 0xa4, 0x04, 0x2f, 0x2b, 0x2e, 0x82, 0x01, 0x98, 0x2e, 0xf3, 0x03, 0x12, + 0x30, 0x81, 0x90, 0x61, 0x52, 0x08, 0x2f, 0x65, 0x42, 0x65, 0x42, 0x43, 0x80, 0x39, 0x84, 0x82, 0x88, 0x05, 0x42, + 0x45, 0x42, 0x85, 0x42, 0x05, 0x43, 0x00, 0x2e, 0x80, 0x41, 0x00, 0x90, 0x90, 0x2e, 0xe1, 0xb4, 0x65, 0x54, 0xc1, + 0x6f, 0x80, 0x40, 0x00, 0xb2, 0x43, 0x58, 0x69, 0x50, 0x44, 0x2f, 0x55, 0x5c, 0xb7, 0x87, 0x8c, 0x0f, 0x0d, 0x2e, + 0x96, 0x01, 0xc4, 0x40, 0x36, 0x2f, 0x41, 0x56, 0x8b, 0x0e, 0x2a, 0x2f, 0x0b, 0x52, 0xa1, 0x0e, 0x0a, 0x2f, 0x05, + 0x2e, 0x8f, 0x01, 0x14, 0x25, 0x98, 0x2e, 0xfe, 0xc9, 0x4b, 0x54, 0x02, 0x0f, 0x69, 0x50, 0x05, 0x30, 0x65, 0x54, + 0x15, 0x2f, 0x03, 0x2e, 0x8e, 0x01, 0x4d, 0x5c, 0x8e, 0x0f, 0x3a, 0x2f, 0x05, 0x2e, 0x8f, 0x01, 0x98, 0x2e, 0xfe, + 0xc9, 0x4f, 0x54, 0x82, 0x0f, 0x05, 0x30, 0x69, 0x50, 0x65, 0x54, 0x30, 0x2f, 0x6d, 0x52, 0x15, 0x30, 0x42, 0x8c, + 0x45, 0x42, 0x04, 0x30, 0x2b, 0x2c, 0x84, 0x43, 0x6b, 0x52, 0x42, 0x8c, 0x00, 0x2e, 0x85, 0x43, 0x15, 0x30, 0x24, + 0x2c, 0x45, 0x42, 0x8e, 0x0f, 0x20, 0x2f, 0x0d, 0x2e, 0x8e, 0x01, 0xb1, 0x0e, 0x1c, 0x2f, 0x23, 0x2e, 0x8e, 0x01, + 0x1a, 0x2d, 0x0e, 0x0e, 0x17, 0x2f, 0xa1, 0x0f, 0x15, 0x2f, 0x23, 0x2e, 0x8d, 0x01, 0x13, 0x2d, 0x98, 0x2e, 0x74, + 0xc0, 0x43, 0x54, 0xc2, 0x0e, 0x0a, 0x2f, 0x65, 0x50, 0x04, 0x80, 0x0b, 0x30, 0x06, 0x82, 0x0b, 0x42, 0x79, 0x80, + 0x41, 0x40, 0x12, 0x30, 0x25, 0x2e, 0x8c, 0x01, 0x01, 0x42, 0x05, 0x30, 0x69, 0x50, 0x65, 0x54, 0x84, 0x82, 0x43, + 0x84, 0xbe, 0x8c, 0x84, 0x40, 0x86, 0x41, 0x26, 0x29, 0x94, 0x42, 0xbe, 0x8e, 0xd5, 0x7f, 0x19, 0xa1, 0x43, 0x40, + 0x0b, 0x2e, 0x8c, 0x01, 0x84, 0x40, 0xc7, 0x41, 0x5d, 0x29, 0x27, 0x29, 0x45, 0x42, 0x84, 0x42, 0xc2, 0x7f, 0x01, + 0x2f, 0xc0, 0xb3, 0x1d, 0x2f, 0x05, 0x2e, 0x94, 0x01, 0x99, 0xa0, 0x01, 0x2f, 0x80, 0xb3, 0x13, 0x2f, 0x80, 0xb3, + 0x18, 0x2f, 0xc0, 0xb3, 0x16, 0x2f, 0x12, 0x40, 0x01, 0x40, 0x92, 0x7f, 0x98, 0x2e, 0x74, 0xc0, 0x92, 0x6f, 0x10, + 0x0f, 0x20, 0x30, 0x03, 0x2f, 0x10, 0x30, 0x21, 0x2e, 0x7e, 0x01, 0x0a, 0x2d, 0x21, 0x2e, 0x7e, 0x01, 0x07, 0x2d, + 0x20, 0x30, 0x21, 0x2e, 0x7e, 0x01, 0x03, 0x2d, 0x10, 0x30, 0x21, 0x2e, 0x7e, 0x01, 0xc2, 0x6f, 0x01, 0x2e, 0xc9, + 0x00, 0xbc, 0x84, 0x02, 0x80, 0x82, 0x40, 0x00, 0x40, 0x90, 0x0e, 0xd5, 0x6f, 0x02, 0x2f, 0x15, 0x30, 0x98, 0x2e, + 0xf3, 0x03, 0x41, 0x91, 0x05, 0x30, 0x07, 0x2f, 0x67, 0x50, 0x3d, 0x80, 0x2b, 0x2e, 0x8f, 0x01, 0x05, 0x42, 0x04, + 0x80, 0x00, 0x2e, 0x05, 0x42, 0x02, 0x2c, 0x00, 0x30, 0x00, 0x30, 0xa2, 0x6f, 0x98, 0x8a, 0x86, 0x40, 0x80, 0xa7, + 0x05, 0x2f, 0x98, 0x2e, 0xf3, 0x03, 0xc0, 0x30, 0x21, 0x2e, 0x95, 0x01, 0x06, 0x25, 0x1a, 0x25, 0xe2, 0x6f, 0x76, + 0x82, 0x96, 0x40, 0x56, 0x43, 0x51, 0x0e, 0xfb, 0x2f, 0xbb, 0x6f, 0x30, 0x5f, 0xb8, 0x2e, 0x01, 0x2e, 0xb8, 0x00, + 0x01, 0x31, 0x41, 0x08, 0x40, 0xb2, 0x20, 0x50, 0xf2, 0x30, 0x02, 0x08, 0xfb, 0x7f, 0x01, 0x30, 0x10, 0x2f, 0x05, + 0x2e, 0xcc, 0x00, 0x81, 0x90, 0xe0, 0x7f, 0x03, 0x2f, 0x23, 0x2e, 0xcc, 0x00, 0x98, 0x2e, 0x55, 0xb6, 0x98, 0x2e, + 0x1d, 0xb5, 0x10, 0x25, 0xfb, 0x6f, 0xe0, 0x6f, 0xe0, 0x5f, 0x80, 0x2e, 0x95, 0xcf, 0x98, 0x2e, 0x95, 0xcf, 0x10, + 0x30, 0x21, 0x2e, 0xcc, 0x00, 0xfb, 0x6f, 0xe0, 0x5f, 0xb8, 0x2e, 0x00, 0x51, 0x05, 0x58, 0xeb, 0x7f, 0x2a, 0x25, + 0x89, 0x52, 0x6f, 0x5a, 0x89, 0x50, 0x13, 0x41, 0x06, 0x40, 0xb3, 0x01, 0x16, 0x42, 0xcb, 0x16, 0x06, 0x40, 0xf3, + 0x02, 0x13, 0x42, 0x65, 0x0e, 0xf5, 0x2f, 0x05, 0x40, 0x14, 0x30, 0x2c, 0x29, 0x04, 0x42, 0x08, 0xa1, 0x00, 0x30, + 0x90, 0x2e, 0x52, 0xb6, 0xb3, 0x88, 0xb0, 0x8a, 0xb6, 0x84, 0xa4, 0x7f, 0xc4, 0x7f, 0xb5, 0x7f, 0xd5, 0x7f, 0x92, + 0x7f, 0x73, 0x30, 0x04, 0x30, 0x55, 0x40, 0x42, 0x40, 0x8a, 0x17, 0xf3, 0x08, 0x6b, 0x01, 0x90, 0x02, 0x53, 0xb8, + 0x4b, 0x82, 0xad, 0xbe, 0x71, 0x7f, 0x45, 0x0a, 0x09, 0x54, 0x84, 0x7f, 0x98, 0x2e, 0xd9, 0xc0, 0xa3, 0x6f, 0x7b, + 0x54, 0xd0, 0x42, 0xa3, 0x7f, 0xf2, 0x7f, 0x60, 0x7f, 0x20, 0x25, 0x71, 0x6f, 0x75, 0x5a, 0x77, 0x58, 0x79, 0x5c, + 0x75, 0x56, 0x98, 0x2e, 0x67, 0xcc, 0xb1, 0x6f, 0x62, 0x6f, 0x50, 0x42, 0xb1, 0x7f, 0xb3, 0x30, 0x10, 0x25, 0x98, + 0x2e, 0x0f, 0xca, 0x84, 0x6f, 0x20, 0x29, 0x71, 0x6f, 0x92, 0x6f, 0xa5, 0x6f, 0x76, 0x82, 0x6a, 0x0e, 0x73, 0x30, + 0x00, 0x30, 0xd0, 0x2f, 0xd2, 0x6f, 0xd1, 0x7f, 0xb4, 0x7f, 0x98, 0x2e, 0x2b, 0xb7, 0x15, 0xbd, 0x0b, 0xb8, 0x02, + 0x0a, 0xc2, 0x6f, 0xc0, 0x7f, 0x98, 0x2e, 0x2b, 0xb7, 0x15, 0xbd, 0x0b, 0xb8, 0x42, 0x0a, 0xc0, 0x6f, 0x08, 0x17, + 0x41, 0x18, 0x89, 0x16, 0xe1, 0x18, 0xd0, 0x18, 0xa1, 0x7f, 0x27, 0x25, 0x16, 0x25, 0x98, 0x2e, 0x79, 0xc0, 0x8b, + 0x54, 0x90, 0x7f, 0xb3, 0x30, 0x82, 0x40, 0x80, 0x90, 0x0d, 0x2f, 0x7d, 0x52, 0x92, 0x6f, 0x98, 0x2e, 0x0f, 0xca, + 0xb2, 0x6f, 0x90, 0x0e, 0x06, 0x2f, 0x8b, 0x50, 0x14, 0x30, 0x42, 0x6f, 0x51, 0x6f, 0x14, 0x42, 0x12, 0x42, 0x01, + 0x42, 0x00, 0x2e, 0x31, 0x6f, 0x98, 0x2e, 0x74, 0xc0, 0x41, 0x6f, 0x80, 0x7f, 0x98, 0x2e, 0x74, 0xc0, 0x82, 0x6f, + 0x10, 0x04, 0x43, 0x52, 0x01, 0x0f, 0x05, 0x2e, 0xcb, 0x00, 0x00, 0x30, 0x04, 0x30, 0x21, 0x2f, 0x51, 0x6f, 0x43, + 0x58, 0x8c, 0x0e, 0x04, 0x30, 0x1c, 0x2f, 0x85, 0x88, 0x41, 0x6f, 0x04, 0x41, 0x8c, 0x0f, 0x04, 0x30, 0x16, 0x2f, + 0x84, 0x88, 0x00, 0x2e, 0x04, 0x41, 0x04, 0x05, 0x8c, 0x0e, 0x04, 0x30, 0x0f, 0x2f, 0x82, 0x88, 0x31, 0x6f, 0x04, + 0x41, 0x04, 0x05, 0x8c, 0x0e, 0x04, 0x30, 0x08, 0x2f, 0x83, 0x88, 0x00, 0x2e, 0x04, 0x41, 0x8c, 0x0f, 0x04, 0x30, + 0x02, 0x2f, 0x21, 0x2e, 0xad, 0x01, 0x14, 0x30, 0x00, 0x91, 0x14, 0x2f, 0x03, 0x2e, 0xa1, 0x01, 0x41, 0x90, 0x0e, + 0x2f, 0x03, 0x2e, 0xad, 0x01, 0x14, 0x30, 0x4c, 0x28, 0x23, 0x2e, 0xad, 0x01, 0x46, 0xa0, 0x06, 0x2f, 0x81, 0x84, + 0x8d, 0x52, 0x48, 0x82, 0x82, 0x40, 0x21, 0x2e, 0xa1, 0x01, 0x42, 0x42, 0x5c, 0x2c, 0x02, 0x30, 0x05, 0x2e, 0xaa, + 0x01, 0x80, 0xb2, 0x02, 0x30, 0x55, 0x2f, 0x03, 0x2e, 0xa9, 0x01, 0x92, 0x6f, 0xb3, 0x30, 0x98, 0x2e, 0x0f, 0xca, + 0xb2, 0x6f, 0x90, 0x0f, 0x00, 0x30, 0x02, 0x30, 0x4a, 0x2f, 0xa2, 0x6f, 0x87, 0x52, 0x91, 0x00, 0x85, 0x52, 0x51, + 0x0e, 0x02, 0x2f, 0x00, 0x2e, 0x43, 0x2c, 0x02, 0x30, 0xc2, 0x6f, 0x7f, 0x52, 0x91, 0x0e, 0x02, 0x30, 0x3c, 0x2f, + 0x51, 0x6f, 0x81, 0x54, 0x98, 0x2e, 0xfe, 0xc9, 0x10, 0x25, 0xb3, 0x30, 0x21, 0x25, 0x98, 0x2e, 0x0f, 0xca, 0x32, + 0x6f, 0xc0, 0x7f, 0xb3, 0x30, 0x12, 0x25, 0x98, 0x2e, 0x0f, 0xca, 0x42, 0x6f, 0xb0, 0x7f, 0xb3, 0x30, 0x12, 0x25, + 0x98, 0x2e, 0x0f, 0xca, 0xb2, 0x6f, 0x90, 0x28, 0x83, 0x52, 0x98, 0x2e, 0xfe, 0xc9, 0xc2, 0x6f, 0x90, 0x0f, 0x00, + 0x30, 0x02, 0x30, 0x1d, 0x2f, 0x05, 0x2e, 0xa1, 0x01, 0x80, 0xb2, 0x12, 0x30, 0x0f, 0x2f, 0x42, 0x6f, 0x03, 0x2e, + 0xab, 0x01, 0x91, 0x0e, 0x02, 0x30, 0x12, 0x2f, 0x52, 0x6f, 0x03, 0x2e, 0xac, 0x01, 0x91, 0x0f, 0x02, 0x30, 0x0c, + 0x2f, 0x21, 0x2e, 0xaa, 0x01, 0x0a, 0x2c, 0x12, 0x30, 0x03, 0x2e, 0xcb, 0x00, 0x8d, 0x58, 0x08, 0x89, 0x41, 0x40, + 0x11, 0x43, 0x00, 0x43, 0x25, 0x2e, 0xa1, 0x01, 0xd4, 0x6f, 0x8f, 0x52, 0x00, 0x43, 0x3a, 0x89, 0x00, 0x2e, 0x10, + 0x43, 0x10, 0x43, 0x61, 0x0e, 0xfb, 0x2f, 0x03, 0x2e, 0xa0, 0x01, 0x11, 0x1a, 0x02, 0x2f, 0x02, 0x25, 0x21, 0x2e, + 0xa0, 0x01, 0xeb, 0x6f, 0x00, 0x5f, 0xb8, 0x2e, 0x91, 0x52, 0x10, 0x30, 0x02, 0x30, 0x95, 0x56, 0x52, 0x42, 0x4b, + 0x0e, 0xfc, 0x2f, 0x8d, 0x54, 0x88, 0x82, 0x93, 0x56, 0x80, 0x42, 0x53, 0x42, 0x40, 0x42, 0x42, 0x86, 0x83, 0x54, + 0xc0, 0x2e, 0xc2, 0x42, 0x00, 0x2e, 0xa3, 0x52, 0x00, 0x51, 0x52, 0x40, 0x47, 0x40, 0x1a, 0x25, 0x01, 0x2e, 0x97, + 0x00, 0x8f, 0xbe, 0x72, 0x86, 0xfb, 0x7f, 0x0b, 0x30, 0x7c, 0xbf, 0xa5, 0x50, 0x10, 0x08, 0xdf, 0xba, 0x70, 0x88, + 0xf8, 0xbf, 0xcb, 0x42, 0xd3, 0x7f, 0x6c, 0xbb, 0xfc, 0xbb, 0xc5, 0x0a, 0x90, 0x7f, 0x1b, 0x7f, 0x0b, 0x43, 0xc0, + 0xb2, 0xe5, 0x7f, 0xb7, 0x7f, 0xa6, 0x7f, 0xc4, 0x7f, 0x90, 0x2e, 0x1c, 0xb7, 0x07, 0x2e, 0xd2, 0x00, 0xc0, 0xb2, + 0x0b, 0x2f, 0x97, 0x52, 0x01, 0x2e, 0xcd, 0x00, 0x82, 0x7f, 0x98, 0x2e, 0xbb, 0xcc, 0x0b, 0x30, 0x37, 0x2e, 0xd2, + 0x00, 0x82, 0x6f, 0x90, 0x6f, 0x1a, 0x25, 0x00, 0xb2, 0x8b, 0x7f, 0x14, 0x2f, 0xa6, 0xbd, 0x25, 0xbd, 0xb6, 0xb9, + 0x2f, 0xb9, 0x80, 0xb2, 0xd4, 0xb0, 0x0c, 0x2f, 0x99, 0x54, 0x9b, 0x56, 0x0b, 0x30, 0x0b, 0x2e, 0xb1, 0x00, 0xa1, + 0x58, 0x9b, 0x42, 0xdb, 0x42, 0x6c, 0x09, 0x2b, 0x2e, 0xb1, 0x00, 0x8b, 0x42, 0xcb, 0x42, 0x86, 0x7f, 0x73, 0x84, + 0xa7, 0x56, 0xc3, 0x08, 0x39, 0x52, 0x05, 0x50, 0x72, 0x7f, 0x63, 0x7f, 0x98, 0x2e, 0xc2, 0xc0, 0xe1, 0x6f, 0x62, + 0x6f, 0xd1, 0x0a, 0x01, 0x2e, 0xcd, 0x00, 0xd5, 0x6f, 0xc4, 0x6f, 0x72, 0x6f, 0x97, 0x52, 0x9d, 0x5c, 0x98, 0x2e, + 0x06, 0xcd, 0x23, 0x6f, 0x90, 0x6f, 0x99, 0x52, 0xc0, 0xb2, 0x04, 0xbd, 0x54, 0x40, 0xaf, 0xb9, 0x45, 0x40, 0xe1, + 0x7f, 0x02, 0x30, 0x06, 0x2f, 0xc0, 0xb2, 0x02, 0x30, 0x03, 0x2f, 0x9b, 0x5c, 0x12, 0x30, 0x94, 0x43, 0x85, 0x43, + 0x03, 0xbf, 0x6f, 0xbb, 0x80, 0xb3, 0x20, 0x2f, 0x06, 0x6f, 0x26, 0x01, 0x16, 0x6f, 0x6e, 0x03, 0x45, 0x42, 0xc0, + 0x90, 0x29, 0x2e, 0xce, 0x00, 0x9b, 0x52, 0x14, 0x2f, 0x9b, 0x5c, 0x00, 0x2e, 0x93, 0x41, 0x86, 0x41, 0xe3, 0x04, + 0xae, 0x07, 0x80, 0xab, 0x04, 0x2f, 0x80, 0x91, 0x0a, 0x2f, 0x86, 0x6f, 0x73, 0x0f, 0x07, 0x2f, 0x83, 0x6f, 0xc0, + 0xb2, 0x04, 0x2f, 0x54, 0x42, 0x45, 0x42, 0x12, 0x30, 0x04, 0x2c, 0x11, 0x30, 0x02, 0x2c, 0x11, 0x30, 0x11, 0x30, + 0x02, 0xbc, 0x0f, 0xb8, 0xd2, 0x7f, 0x00, 0xb2, 0x0a, 0x2f, 0x01, 0x2e, 0xfc, 0x00, 0x05, 0x2e, 0xc7, 0x01, 0x10, + 0x1a, 0x02, 0x2f, 0x21, 0x2e, 0xc7, 0x01, 0x03, 0x2d, 0x02, 0x2c, 0x01, 0x30, 0x01, 0x30, 0xb0, 0x6f, 0x98, 0x2e, + 0x95, 0xcf, 0xd1, 0x6f, 0xa0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, 0xe2, 0x6f, 0x9f, 0x52, 0x01, 0x2e, 0xce, 0x00, 0x82, + 0x40, 0x50, 0x42, 0x0c, 0x2c, 0x42, 0x42, 0x11, 0x30, 0x23, 0x2e, 0xd2, 0x00, 0x01, 0x30, 0xb0, 0x6f, 0x98, 0x2e, + 0x95, 0xcf, 0xa0, 0x6f, 0x01, 0x30, 0x98, 0x2e, 0x95, 0xcf, 0x00, 0x2e, 0xfb, 0x6f, 0x00, 0x5f, 0xb8, 0x2e, 0x83, + 0x86, 0x01, 0x30, 0x00, 0x30, 0x94, 0x40, 0x24, 0x18, 0x06, 0x00, 0x53, 0x0e, 0x4f, 0x02, 0xf9, 0x2f, 0xb8, 0x2e, + 0xa9, 0x52, 0x00, 0x2e, 0x60, 0x40, 0x41, 0x40, 0x0d, 0xbc, 0x98, 0xbc, 0xc0, 0x2e, 0x01, 0x0a, 0x0f, 0xb8, 0xab, + 0x52, 0x53, 0x3c, 0x52, 0x40, 0x40, 0x40, 0x4b, 0x00, 0x82, 0x16, 0x26, 0xb9, 0x01, 0xb8, 0x41, 0x40, 0x10, 0x08, + 0x97, 0xb8, 0x01, 0x08, 0xc0, 0x2e, 0x11, 0x30, 0x01, 0x08, 0x43, 0x86, 0x25, 0x40, 0x04, 0x40, 0xd8, 0xbe, 0x2c, + 0x0b, 0x22, 0x11, 0x54, 0x42, 0x03, 0x80, 0x4b, 0x0e, 0xf6, 0x2f, 0xb8, 0x2e, 0x9f, 0x50, 0x10, 0x50, 0xad, 0x52, + 0x05, 0x2e, 0xd3, 0x00, 0xfb, 0x7f, 0x00, 0x2e, 0x13, 0x40, 0x93, 0x42, 0x41, 0x0e, 0xfb, 0x2f, 0x98, 0x2e, 0xa5, + 0xb7, 0x98, 0x2e, 0x87, 0xcf, 0x01, 0x2e, 0xd9, 0x00, 0x00, 0xb2, 0xfb, 0x6f, 0x0b, 0x2f, 0x01, 0x2e, 0x69, 0xf7, + 0xb1, 0x3f, 0x01, 0x08, 0x01, 0x30, 0xf0, 0x5f, 0x23, 0x2e, 0xd9, 0x00, 0x21, 0x2e, 0x69, 0xf7, 0x80, 0x2e, 0x7a, + 0xb7, 0xf0, 0x5f, 0xb8, 0x2e, 0x01, 0x2e, 0xc0, 0xf8, 0x03, 0x2e, 0xfc, 0xf5, 0x15, 0x54, 0xaf, 0x56, 0x82, 0x08, + 0x0b, 0x2e, 0x69, 0xf7, 0xcb, 0x0a, 0xb1, 0x58, 0x80, 0x90, 0xdd, 0xbe, 0x4c, 0x08, 0x5f, 0xb9, 0x59, 0x22, 0x80, + 0x90, 0x07, 0x2f, 0x03, 0x34, 0xc3, 0x08, 0xf2, 0x3a, 0x0a, 0x08, 0x02, 0x35, 0xc0, 0x90, 0x4a, 0x0a, 0x48, 0x22, + 0xc0, 0x2e, 0x23, 0x2e, 0xfc, 0xf5, 0x10, 0x50, 0xfb, 0x7f, 0x98, 0x2e, 0x56, 0xc7, 0x98, 0x2e, 0x49, 0xc3, 0x10, + 0x30, 0xfb, 0x6f, 0xf0, 0x5f, 0x21, 0x2e, 0xcc, 0x00, 0x21, 0x2e, 0xca, 0x00, 0xb8, 0x2e, 0x03, 0x2e, 0xd3, 0x00, + 0x16, 0xb8, 0x02, 0x34, 0x4a, 0x0c, 0x21, 0x2e, 0x2d, 0xf5, 0xc0, 0x2e, 0x23, 0x2e, 0xd3, 0x00, 0x03, 0xbc, 0x21, + 0x2e, 0xd5, 0x00, 0x03, 0x2e, 0xd5, 0x00, 0x40, 0xb2, 0x10, 0x30, 0x21, 0x2e, 0x77, 0x00, 0x01, 0x30, 0x05, 0x2f, + 0x05, 0x2e, 0xd8, 0x00, 0x80, 0x90, 0x01, 0x2f, 0x23, 0x2e, 0x6f, 0xf5, 0xc0, 0x2e, 0x21, 0x2e, 0xd9, 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, 0x7c, + 0x00, 0xfb, 0x7f, 0x98, 0x2e, 0xc3, 0xb7, 0x40, 0x30, 0x21, 0x2e, 0xd4, 0x00, 0xfb, 0x6f, 0xf0, 0x5f, 0x03, 0x25, + 0x80, 0x2e, 0xaf, 0xb7, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 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, 0xb3, 0x58, 0x02, 0x2f, 0x90, 0xa1, 0x26, 0x13, 0x20, 0x23, 0x80, 0x90, + 0x10, 0x30, 0x01, 0x2f, 0xcc, 0x0e, 0x00, 0x2f, 0x00, 0x30, 0xb8, 0x2e, 0xb5, 0x50, 0x18, 0x08, 0x08, 0xbc, 0x88, + 0xb6, 0x0d, 0x17, 0xc6, 0xbd, 0x56, 0xbc, 0xb7, 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, 0x2d, 0x50, 0x48, 0x0f, 0x09, 0x2f, 0xbf, 0xa0, + 0x04, 0x2f, 0xbf, 0x90, 0x06, 0x2f, 0xb7, 0x54, 0xca, 0x0f, 0x03, 0x2f, 0x00, 0x2e, 0x02, 0x2c, 0xb7, 0x52, 0x2d, + 0x52, 0xf2, 0x33, 0x98, 0x2e, 0xd9, 0xc0, 0xfb, 0x6f, 0xf1, 0x37, 0xc0, 0x2e, 0x01, 0x08, 0xf0, 0x5f, 0xbf, 0x56, + 0xb9, 0x54, 0xd0, 0x40, 0xc4, 0x40, 0x0b, 0x2e, 0xfd, 0xf3, 0xbf, 0x52, 0x90, 0x42, 0x94, 0x42, 0x95, 0x42, 0x05, + 0x30, 0xc1, 0x50, 0x0f, 0x88, 0x06, 0x40, 0x04, 0x41, 0x96, 0x42, 0xc5, 0x42, 0x48, 0xbe, 0x73, 0x30, 0x0d, 0x2e, + 0xd8, 0x00, 0x4f, 0xba, 0x84, 0x42, 0x03, 0x42, 0x81, 0xb3, 0x02, 0x2f, 0x2b, 0x2e, 0x6f, 0xf5, 0x06, 0x2d, 0x05, + 0x2e, 0x77, 0xf7, 0xbd, 0x56, 0x93, 0x08, 0x25, 0x2e, 0x77, 0xf7, 0xbb, 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, 0xc3, 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, 0xcb, 0x58, 0x32, 0x87, 0xc4, + 0x7f, 0x65, 0x89, 0x6b, 0x8d, 0xc5, 0x5a, 0x65, 0x7f, 0xe1, 0x7f, 0x83, 0x7f, 0xa6, 0x7f, 0x74, 0x7f, 0xd0, 0x7f, + 0xb6, 0x7f, 0x94, 0x7f, 0x17, 0x30, 0xc7, 0x52, 0xc9, 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, 0xb7, 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, 0xd1, 0x56, 0x63, 0x0e, 0x74, 0x6f, 0x51, 0x43, 0xa5, 0x7f, 0x8a, + 0x2f, 0x09, 0x2e, 0xd8, 0x00, 0x01, 0xb3, 0x21, 0x2f, 0xcb, 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, 0xcd, 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, 0xcf, 0x54, 0x5a, 0x0e, 0xef, 0x2f, 0x15, + 0x54, 0x09, 0x2e, 0x77, 0xf7, 0x22, 0x0b, 0x29, 0x2e, 0x77, 0xf7, 0xfb, 0x6f, 0x50, 0x5e, 0xb8, 0x2e, 0x10, 0x50, + 0x01, 0x2e, 0xd4, 0x00, 0x00, 0xb2, 0xfb, 0x7f, 0x51, 0x2f, 0x01, 0xb2, 0x48, 0x2f, 0x02, 0xb2, 0x42, 0x2f, 0x03, + 0x90, 0x56, 0x2f, 0xd7, 0x52, 0x79, 0x80, 0x42, 0x40, 0x81, 0x84, 0x00, 0x40, 0x42, 0x42, 0x98, 0x2e, 0x93, 0x0c, + 0xd9, 0x54, 0xd7, 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, 0x15, 0x54, 0x4a, 0x0e, + 0x3a, 0x2f, 0x3a, 0x82, 0x00, 0x30, 0x41, 0x40, 0x21, 0x2e, 0x85, 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, 0xce, 0xb7, + 0xdd, 0x52, 0xd3, 0x54, 0x42, 0x42, 0x4f, 0x84, 0x73, 0x30, 0xdb, 0x52, 0x83, 0x42, 0x1b, 0x30, 0x6b, 0x42, 0x23, + 0x30, 0x27, 0x2e, 0xd7, 0x00, 0x37, 0x2e, 0xd4, 0x00, 0x21, 0x2e, 0xd6, 0x00, 0x7a, 0x84, 0x17, 0x2c, 0x42, 0x42, + 0x30, 0x30, 0x21, 0x2e, 0xd4, 0x00, 0x12, 0x2d, 0x21, 0x30, 0x00, 0x30, 0x23, 0x2e, 0xd4, 0x00, 0x21, 0x2e, 0x7b, + 0xf7, 0x0b, 0x2d, 0x17, 0x30, 0x98, 0x2e, 0x51, 0x0c, 0xd5, 0x50, 0x0c, 0x82, 0x72, 0x30, 0x2f, 0x2e, 0xd4, 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, 0xb5, 0x56, 0xa5, 0x6f, 0xab, 0x08, 0x91, 0x6f, + 0x4b, 0x08, 0xdf, 0x56, 0xc4, 0x6f, 0x23, 0x09, 0x4d, 0xba, 0x93, 0xbc, 0x8c, 0x0b, 0xd1, 0x6f, 0x0b, 0x09, 0xcb, + 0x52, 0xe1, 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, 0xd1, 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, 0xd4, 0x00, 0x31, 0x30, 0x08, 0x04, 0xfb, 0x6f, 0x01, 0x30, 0xf0, 0x5f, 0x23, 0x2e, 0xd6, 0x00, 0x21, 0x2e, + 0xd7, 0x00, 0xb8, 0x2e, 0x01, 0x2e, 0xd7, 0x00, 0x03, 0x2e, 0xd6, 0x00, 0x48, 0x0e, 0x01, 0x2f, 0x80, 0x2e, 0x1f, + 0x0e, 0xb8, 0x2e, 0xe3, 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, 0xe3, 0x54, 0xf0, 0x3b, 0x83, 0x40, 0xd8, 0x08, 0xe5, + 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, 0xdd, 0x52, 0x00, + 0x30, 0x40, 0x42, 0x7c, 0x86, 0xb9, 0x52, 0x09, 0x2e, 0x70, 0x0f, 0xbf, 0x54, 0xc4, 0x42, 0xd3, 0x86, 0x54, 0x40, + 0x55, 0x40, 0x94, 0x42, 0x85, 0x42, 0x21, 0x2e, 0xd7, 0x00, 0x42, 0x40, 0x25, 0x2e, 0xfd, 0xf3, 0xc0, 0x42, 0x7e, + 0x82, 0x05, 0x2e, 0x7d, 0x00, 0x80, 0xb2, 0x14, 0x2f, 0x05, 0x2e, 0x89, 0x00, 0x27, 0xbd, 0x2f, 0xb9, 0x80, 0x90, + 0x02, 0x2f, 0x21, 0x2e, 0x6f, 0xf5, 0x0c, 0x2d, 0x07, 0x2e, 0x71, 0x0f, 0x14, 0x30, 0x1c, 0x09, 0x05, 0x2e, 0x77, + 0xf7, 0xbd, 0x56, 0x47, 0xbe, 0x93, 0x08, 0x94, 0x0a, 0x25, 0x2e, 0x77, 0xf7, 0xe7, 0x54, 0x50, 0x42, 0x4a, 0x0e, + 0xfc, 0x2f, 0xb8, 0x2e, 0x50, 0x50, 0x02, 0x30, 0x43, 0x86, 0xe5, 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, 0x86, 0x0f, 0x40, 0x90, 0x11, 0x30, 0x03, 0x2f, 0x23, 0x2e, 0x86, 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, 0x47, 0x58, 0xc2, 0x6f, 0x94, 0x09, + 0xeb, 0x58, 0x6a, 0xbb, 0xdc, 0x08, 0xb4, 0xb9, 0xb1, 0xbd, 0xe9, 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, + 0xf5, 0x50, 0x31, 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, 0xd3, 0x00, 0x23, 0x52, 0xe2, 0x7f, 0xd3, 0x7f, 0xc0, 0x7f, + 0x98, 0x2e, 0xb6, 0x0e, 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, 0xa5, 0xb7, 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, 0xed, 0x50, 0x98, + 0x2e, 0x44, 0xcb, 0xef, 0x50, 0x98, 0x2e, 0x46, 0xc3, 0xf1, 0x50, 0x98, 0x2e, 0x53, 0xc7, 0x35, 0x50, 0x98, 0x2e, + 0x64, 0xcf, 0x10, 0x30, 0x98, 0x2e, 0xdc, 0x03, 0x20, 0x26, 0xc0, 0x6f, 0x02, 0x31, 0x12, 0x42, 0xab, 0x33, 0x0b, + 0x42, 0x37, 0x80, 0x01, 0x30, 0x01, 0x42, 0xf3, 0x37, 0xf7, 0x52, 0xfb, 0x50, 0x44, 0x40, 0xa2, 0x0a, 0x42, 0x42, + 0x8b, 0x31, 0x09, 0x2e, 0x5e, 0xf7, 0xf9, 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, 0x58, 0xb7, 0xd1, 0x6f, + 0x80, 0x30, 0x40, 0x42, 0x03, 0x30, 0xe0, 0x6f, 0xf3, 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, 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 +}; + +/*! @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_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 }, + { .type = BMI2_AXIS_MAP, .page = BMI2_PAGE_1, .start_addr = BMI270_AXIS_MAP_STRT_ADDR }, + { .type = BMI2_GYRO_SELF_OFF, .page = BMI2_PAGE_1, .start_addr = BMI270_GYRO_SELF_OFF_STRT_ADDR }, + { .type = BMI2_NVM_PROG_PREP, .page = BMI2_PAGE_1, .start_addr = BMI270_NVM_PROG_PREP_STRT_ADDR }, + { .type = BMI2_GYRO_GAIN_UPDATE, .page = BMI2_PAGE_1, .start_addr = BMI270_GYRO_GAIN_UPDATE_STRT_ADDR }, + { .type = BMI2_ANY_MOTION, .page = BMI2_PAGE_1, .start_addr = BMI270_ANY_MOT_STRT_ADDR }, + { .type = BMI2_NO_MOTION, .page = BMI2_PAGE_2, .start_addr = BMI270_NO_MOT_STRT_ADDR }, + { .type = BMI2_SIG_MOTION, .page = BMI2_PAGE_2, .start_addr = BMI270_SIG_MOT_STRT_ADDR }, + { .type = BMI2_STEP_COUNTER_PARAMS, .page = BMI2_PAGE_3, .start_addr = BMI270_STEP_CNT_1_STRT_ADDR }, + { .type = BMI2_STEP_DETECTOR, .page = BMI2_PAGE_6, .start_addr = BMI270_STEP_CNT_4_STRT_ADDR }, + { .type = BMI2_STEP_COUNTER, .page = BMI2_PAGE_6, .start_addr = BMI270_STEP_CNT_4_STRT_ADDR }, + { .type = BMI2_STEP_ACTIVITY, .page = BMI2_PAGE_6, .start_addr = BMI270_STEP_CNT_4_STRT_ADDR }, + { .type = BMI2_WRIST_GESTURE, .page = BMI2_PAGE_6, .start_addr = BMI270_WRIST_GEST_STRT_ADDR }, + { .type = BMI2_WRIST_WEAR_WAKE_UP, .page = BMI2_PAGE_7, .start_addr = BMI270_WRIST_WEAR_WAKE_UP_STRT_ADDR }, +}; + +/*! @name Global array that stores the feature output configuration */ +const struct bmi2_feature_config bmi270_feat_out[BMI270_MAX_FEAT_OUT] = { + { .type = BMI2_STEP_COUNTER, .page = BMI2_PAGE_0, .start_addr = BMI270_STEP_CNT_OUT_STRT_ADDR }, + { .type = BMI2_STEP_ACTIVITY, .page = BMI2_PAGE_0, .start_addr = BMI270_STEP_ACT_OUT_STRT_ADDR }, + { .type = BMI2_WRIST_GESTURE, .page = BMI2_PAGE_0, .start_addr = BMI270_WRIST_GEST_OUT_STRT_ADDR }, + { .type = BMI2_GYRO_GAIN_UPDATE, .page = BMI2_PAGE_0, .start_addr = BMI270_GYR_USER_GAIN_OUT_STRT_ADDR }, + { .type = BMI2_GYRO_CROSS_SENSE, .page = BMI2_PAGE_0, .start_addr = BMI270_GYRO_CROSS_SENSE_STRT_ADDR }, + { .type = BMI2_NVM_STATUS, .page = BMI2_PAGE_0, .start_addr = BMI270_NVM_VFRM_OUT_STRT_ADDR }, + { .type = BMI2_VFRM_STATUS, .page = BMI2_PAGE_0, .start_addr = BMI270_NVM_VFRM_OUT_STRT_ADDR } +}; + +/******************************************************************************/ + +/*! 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 zero -> Success / +ve value -> Warning / -ve value -> Error + */ +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_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 */ + dev->chip_id = BMI270_CHIP_ID; + + /* get the size of config array */ + dev->config_size = sizeof(bmi270_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_INTERFACE) + { + 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_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_feat_in; + + /* Assign the offsets of the feature output to + * the device structure + */ + dev->feat_output = bmi270_feat_out; + + /* Assign the maximum number of pages to the + * device structure + */ + dev->page_max = BMI270_MAX_PAGE_NUM; + + /* Assign maximum number of input sensors/ + * features to device structure + */ + dev->input_sens = BMI270_MAX_FEAT_IN; + + /* Assign maximum number of output sensors/ + * features to device structure + */ + dev->out_sens = BMI270_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 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; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.h b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.h new file mode 100644 index 000000000..7179ac8ed --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.h @@ -0,0 +1,132 @@ +/** +* 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.h +* @date 2020-01-10 +* @version v2.46.1 +* +*/#ifndef BMI270_H_ +#define BMI270_H_ + +/*! CPP guard */ +#ifdef __cplusplus +extern "C" { +#endif + +/***************************************************************************/ + +/*! Header files + ****************************************************************************/ +#include "bmi2.h" + +/***************************************************************************/ + +/*! Macro definitions + ****************************************************************************/ + +/*! @name BMI270 Chip identifier */ +#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) + +/*! @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) + +/*! @name Defines maximum number of pages */ +#define BMI270_MAX_PAGE_NUM UINT8_C(8) + +/*! @name Defines maximum number of feature input configurations */ +#define BMI270_MAX_FEAT_IN UINT8_C(16) + +/*! @name Defines maximum number of feature outputs */ +#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) + +/***************************************************************************/ + +/*! BMI270 User Interface function prototypes + ****************************************************************************/ + +/*! + * @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. + * + * @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 + */ +int8_t bmi270_init(struct bmi2_dev *dev); + +/******************************************************************************/ +/*! @name C++ Guard Macros */ +/******************************************************************************/ +#ifdef __cplusplus +} +#endif /* End of CPP guard */ + +#endif /* BMI270_H_ */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_defs.h b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_defs.h new file mode 100644 index 000000000..64bd472f3 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_defs.h @@ -0,0 +1,2052 @@ +/** +* 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_defs.h +* @date 2020-01-10 +* @version v2.46.1 +* +*/#ifndef BMI2_DEFS_H_ +#define BMI2_DEFS_H_ + +/******************************************************************************/ +/*! @name Header includes */ +/******************************************************************************/ +#ifdef __KERNEL__ +#include +#include +#else +#include +#include +#endif + +/******************************************************************************/ +/*! @name Common macros */ +/******************************************************************************/ +#ifdef __KERNEL__ +#if !defined(UINT8_C) && !defined(INT8_C) +#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) +#endif + +#if !defined(INT32_C) && !defined(UINT32_C) +#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) +#endif +#endif + +/*! @name C standard macros */ +#ifndef NULL +#ifdef __cplusplus +#define NULL 0 +#else +#define NULL ((void *) 0) +#endif +#endif + +/******************************************************************************/ +/*! @name General Macro Definitions */ +/******************************************************************************/ +/*! @name Utility macros */ +#define BMI2_SET_BITS(reg_data, bitname, data) \ + ((reg_data & ~(bitname##_MASK)) | \ + ((data << bitname##_POS) & bitname##_MASK)) + +#define BMI2_GET_BITS(reg_data, bitname) \ + ((reg_data & (bitname##_MASK)) >> \ + (bitname##_POS)) + +#define BMI2_SET_BIT_POS0(reg_data, bitname, data) \ + ((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)) + +/*! @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) + +/*! @name For defining absolute values */ +#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) + +/*! @name For enable and disable */ +#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) + +/*! @name To define success code */ +#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) + +/*! @name To define warnings for FIFO activity */ +#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) + +/*! @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) + +/*! @name BMI2 I2C address */ +#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) + +/*! @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) + +/*! @name BMI2 configuration load status */ +#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) + +/*! @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) + +/*! @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) + +/*! @name Mask definitions for SPI read/write address */ +#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) + +/*! @name Mask definitions for initialization control register */ +#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) + +/*! @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) + +/*! @name Mask definitions to switch page */ +#define BMI2_SWITCH_PAGE_EN_MASK UINT8_C(0x07) + +/*! @name Accelerometer and Gyroscope Filter/Noise performance modes */ +/* Power optimized mode */ +#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) + +/*! @name index for config major minor information */ +#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) + +/*! @name Macro to define accelerometer configuration value for FOC */ +#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) + +/*! @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) + +/******************************************************************************/ +/*! @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) + +/* 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) + +/*! @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) + +/*! @name Bit wise selection of BMI2 sensors */ +#define BMI2_MAIN_SENSORS \ + (BMI2_ACCEL_SENS_SEL | BMI2_GYRO_SENS_SEL \ + | BMI2_AUX_SENS_SEL | BMI2_TEMP_SENS_SEL) + +/*! @name Maximum number of BMI2 main sensors */ +#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) + +/*! @name Macro to specify the number of free-fall accel setting parameters */ +#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) + +/*! @name Macro for NVM enable */ +#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) + +/******************************************************************************/ +/*! @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) + +/*! @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) + +/*! @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) + +/*! @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) + +/*! @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) + +/*! @name Self test macro to define range */ +#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) + +/*! @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) + +/*! @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) + +/*! @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) + +/*! @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) + +/******************************************************************************/ +/*! @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) + +/*! @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) + +/*! @name Gyroscope OIS Range */ +#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) + +/*! @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) + +/*! @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) + +/******************************************************************************/ +/*! @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) + +/*! @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) + +/*! @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) + +/*! @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) + +/******************************************************************************/ +/*! @name FIFO Macro Definitions */ +/******************************************************************************/ +/*! @name Macros to define virtual FIFO frame mode */ +#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) + +/*! @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) + +/*! @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) + +/*! @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) + +/*! @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) + +/*! @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) + +/*! @name BMI2 FIFO data filter modes */ +#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) + +/*! @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) + +/*! @name FIFO self wake-up mask definition */ +#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) + +/*! @name FIFO down sampling bit positions */ +#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) + +/*! @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) + +/*! @name FIFO byte counter mask definition */ +#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) + +/*! @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) + +/*! @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) + +/******************************************************************************/ +/*! @name Interrupt Macro Definitions */ +/******************************************************************************/ +/*! @name BMI2 Interrupt Modes */ +/* Non latched */ +#define BMI2_INT_NON_LATCH UINT8_C(0) + +/* Permanently latched */ +#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) + +/*! @name BMI2 Interrupt Pin Level */ +#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) + +/*! @name BMI2 Interrupt Input Enable */ +#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) + +/*! @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) + +/*! @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) + +/*! @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) + +/*! @name Maximum number of interrupt pins */ +#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) + +/******************************************************************************/ +/*! @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) + +/*! @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) + +/******************************************************************************/ +/*! @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) + +/******************************************************************************/ +/*! @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) + +/*! @name Bit positions of gyroscope offset compensation registers */ +#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) + +/*! @name Bit positions of gyroscope offset compensation registers */ +#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) + +/******************************************************************************/ +/*! @name error status form gyro gain update status. */ +/******************************************************************************/ +#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) + +/******************************************************************************/ +/*! @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) + +/******************************************************************************/ +/*! @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); + +/*! For interfacing to the delay function */ +typedef void (*bmi2_delay_fptr_t)(uint32_t period); + +/******************************************************************************/ +/*! @name Enum Declarations */ +/******************************************************************************/ +/*! @name Enum to define BMI2 sensor interfaces */ +enum bmi2_intf_type { + BMI2_SPI_INTERFACE = 1, + BMI2_I2C_INTERFACE, + BMI2_I3C_INTERFACE +}; + +/*! @name Enum to define BMI2 sensor configuration errors for accelerometer + * and gyroscope + */ +enum bmi2_sensor_config_error { + BMI2_NO_ERROR, + BMI2_ACC_ERROR, + BMI2_GYR_ERROR, + BMI2_ACC_GYR_ERROR +}; + +/*! @name Enum to define interrupt lines */ +enum bmi2_hw_int_pin { + BMI2_INT_NONE, + BMI2_INT1, + BMI2_INT2, + BMI2_INT_BOTH, + BMI2_INT_PIN_MAX +}; + +/*! @name Enum for the position of the wearable device */ +enum bmi2_wear_arm_pos { + BMI2_ARM_LEFT, + BMI2_ARM_RIGHT +}; + +/*! @name Enum to display type of activity recognition */ +enum bmi2_act_recog_type { + BMI2_ACT_UNKNOWN, + BMI2_ACT_STILL, + BMI2_ACT_WALK, + BMI2_ACT_RUN, + BMI2_ACT_BIKE, + BMI2_ACT_VEHICLE, + BMI2_ACT_TILTED +}; + +/*! @name Enum to display activity recognition status */ +enum bmi2_act_recog_stat { + BMI2_ACT_START = 1, + BMI2_ACT_END +}; + +/******************************************************************************/ +/*! @name Structure Declarations */ +/******************************************************************************/ +/*! @name Structure to store the compensated user-gain data of gyroscope */ +struct bmi2_gyro_user_gain_data +{ + /*! x-axis */ + int8_t x; + + /*! y-axis */ + int8_t y; + + /*! z-axis */ + int8_t z; +}; + +/*! @name Structure to store the re-mapped axis */ +struct bmi2_remap +{ + /*! Re-mapped x-axis */ + uint8_t x; + + /*! Re-mapped y-axis */ + uint8_t y; + + /*! Re-mapped z-axis */ + uint8_t z; +}; + +/*! @name Structure to store the value of re-mapped axis and its sign */ +struct bmi2_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 */ + int16_t x_axis_sign; + + /*! Re-mapped y-axis sign */ + int16_t y_axis_sign; + + /*! Re-mapped z-axis sign */ + int16_t z_axis_sign; +}; + +/*! @name Structure to define the type of sensor and its interrupt pin */ +struct bmi2_sens_int_config +{ + /*! Defines the type of sensor */ + uint8_t type; + + /*! Type of interrupt pin */ + 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 +{ + /*! Time stamp */ + uint32_t time_stamp; + + /*! current activity */ + uint8_t curr_act; + + /*! previous activity */ + uint8_t prev_act; +}; + +/*! @name Structure to define FIFO frame configuration */ +struct bmi2_fifo_frame +{ + /*! Pointer to FIFO data */ + uint8_t *data; + + /*! Number of user defined bytes of FIFO to be read */ + uint16_t length; + + /*! Defines header/header-less mode */ + uint8_t header_enable; + + /*! Enables type of data to be streamed - accelerometer, auxiliary or + * gyroscope + */ + uint16_t data_enable; + + /*! To index accelerometer bytes */ + uint16_t acc_byte_start_idx; + + /*! To index activity output bytes */ + uint16_t act_recog_byte_start_idx; + + /*! To index auxiliary bytes */ + uint16_t aux_byte_start_idx; + + /*! To index gyroscope bytes */ + uint16_t gyr_byte_start_idx; + + /*! FIFO sensor time */ + uint32_t sensor_time; + + /*! Skipped frame count */ + uint8_t skipped_frame_count; + + /*! Type of data interrupt to be mapped */ + uint8_t data_int_map; + + /*! Water-mark level for water-mark interrupt */ + uint16_t wm_lvl; + + /*! Accelerometer frame length */ + uint8_t acc_frm_len; + + /*! Gyroscope frame length */ + uint8_t gyr_frm_len; + + /*! Auxiliary frame length */ + uint8_t aux_frm_len; + + /*! Accelerometer and gyroscope frame length */ + uint8_t acc_gyr_frm_len; + + /*! Accelerometer and auxiliary frame length */ + uint8_t acc_aux_frm_len; + + /*! Gyroscope and auxiliary frame length */ + uint8_t aux_gyr_frm_len; + + /*! Accelerometer, Gyroscope and auxiliary frame length */ + uint8_t all_frm_len; +}; + +/*! @name Structure to define Interrupt pin configuration */ +struct bmi2_int_pin_cfg +{ + /*! Configure level of interrupt pin */ + uint8_t lvl; + + /*! Configure behavior of interrupt pin */ + uint8_t od; + + /*! Output enable for interrupt pin */ + uint8_t output_en; + + /*! Input enable for interrupt pin */ + uint8_t input_en; +}; + +/*! @name Structure to define interrupt pin type, mode and configurations */ +struct bmi2_int_pin_config +{ + /*! Interrupt pin type: INT1 or INT2 or BOTH */ + uint8_t pin_type; + + /*! Latched or non-latched mode*/ + uint8_t int_latch; + + /*! Structure to define Interrupt pin configuration */ + struct bmi2_int_pin_cfg pin_cfg[BMI2_INT_PIN_MAX_NUM]; +}; + +/*! @name Structure to define an array of 8 auxiliary data bytes */ +struct bmi2_aux_fifo_data +{ + /*! Auxiliary data */ + uint8_t data[8]; + + /*! Sensor time for virtual frames */ + uint32_t virt_sens_time; +}; + +/*! @name Structure to define accelerometer and gyroscope sensor axes and + * sensor time for virtual frames + */ +struct bmi2_sens_axes_data +{ + /*! Data in x-axis */ + int16_t x; + + /*! Data in y-axis */ + int16_t y; + + /*! Data in z-axis */ + int16_t z; + + /*! Sensor time for virtual frames */ + uint32_t virt_sens_time; +}; + +/*! @name Structure to define gyroscope saturation status of user gain */ +struct bmi2_gyr_user_gain_status +{ + /*! Status in x-axis */ + uint8_t sat_x; + + /*! Status in y-axis */ + uint8_t sat_y; + + /*! Status in z-axis */ + uint8_t sat_z; + + /*! G trigger status */ + uint8_t g_trigger_status; +}; + +/*! @name Structure to store the status of gyro self test result */ +struct bmi2_gyro_self_test_status +{ + /*! gyro self test axes done */ + uint8_t gyr_st_axes_done : 1; + + /*! status of gyro X-axis self test */ + uint8_t gyr_axis_x_ok : 1; + + /*! status of gyro Y-axis self test */ + uint8_t gyr_axis_y_ok : 1; + + /*! status of gyro Z-axis self test */ + uint8_t gyr_axis_z_ok : 1; +}; + +/*! @name Structure to define NVM error status */ +struct bmi2_nvm_err_status +{ + /*! NVM load action error */ + uint8_t load_error; + + /*! NVM program action error */ + uint8_t prog_error; + + /*! NVM erase action error */ + uint8_t erase_error; + + /*! NVM program limit exceeded */ + uint8_t exceed_error; + + /*! NVM privilege error */ + uint8_t privil_error; +}; + +/*! @name Structure to define VFRM error status */ +struct bmi2_vfrm_err_status +{ + /*! VFRM lock acquire error */ + uint8_t lock_error; + + /*! VFRM write error */ + uint8_t write_error; + + /*! VFRM fatal err */ + uint8_t fatal_error; +}; + +/*! @name Structure to define accelerometer self test feature status */ +struct bmi2_acc_self_test_status +{ + /*! Accelerometer test completed */ + uint8_t acc_self_test_done; + + /*! Bit is set to 1 when accelerometer X-axis test passed */ + uint8_t acc_x_ok; + + /*! Bit is set to 1 when accelerometer y-axis test passed */ + uint8_t acc_y_ok; + + /*! Bit is set to 1 when accelerometer z-axis test passed */ + uint8_t acc_z_ok; +}; + +/*! @name Structure to define orientation output */ +struct bmi2_orientation_output +{ + /*! Orientation portrait landscape */ + uint8_t portrait_landscape; + + /*! Orientation face-up down */ + uint8_t faceup_down; +}; + +/*! @name Structure to define OIS output */ +struct bmi2_ois_output +{ + /*! OIS accel x axis */ + int16_t ois_acc_x; + + /*! OIS accel y axis */ + int16_t ois_acc_y; + + /*! OIS accel z axis */ + int16_t ois_acc_z; + + /*! ois gyro x axis */ + int16_t ois_gyro_x; + + /*! OIS gyro y axis */ + int16_t ois_gyro_y; + + /*! OIS gyro z axis */ + int16_t ois_gyro_z; +}; + +/*! @name Union to define BMI2 sensor data */ +union bmi2_sens_data +{ + /*! Accelerometer axes data */ + struct bmi2_sens_axes_data acc; + + /*! Gyroscope axes data */ + struct bmi2_sens_axes_data gyr; + + /*! Auxiliary sensor data */ + uint8_t aux_data[BMI2_AUX_NUM_BYTES]; + + /*! Step counter output */ + uint32_t step_counter_output; + + /*! Step activity output */ + uint8_t activity_output; + + /*! Orientation output */ + struct bmi2_orientation_output orient_output; + + /*! High-g output */ + uint8_t high_g_output; + + /*! Gyroscope user gain saturation status */ + struct bmi2_gyr_user_gain_status gyro_user_gain_status; + + /*! NVM error status */ + struct bmi2_nvm_err_status nvm_status; + + /*! Virtual frame error status */ + struct bmi2_vfrm_err_status vfrm_status; + + /*! Wrist gesture output */ + uint8_t wrist_gesture_output; + + /*! Gyroscope cross sense value of z axis */ + int16_t correction_factor_zx; + + /*! Accelerometer self test feature status */ + struct bmi2_acc_self_test_status accel_self_test_output; + + /*! OIS output */ + struct bmi2_ois_output ois_output; +}; + +/*! @name Structure to define type of sensor and their respective data */ +struct bmi2_sensor_data +{ + /*! Defines the type of sensor */ + uint8_t type; + + /*! Defines various sensor data */ + union bmi2_sens_data sens_data; +}; + +/*! @name Structure to define accelerometer configuration */ +struct bmi2_accel_config +{ + /*! Output data rate in Hz */ + uint8_t odr; + + /*! Bandwidth parameter */ + uint8_t bwp; + + /*! Filter performance mode */ + uint8_t filter_perf; + + /*! g-range */ + uint8_t range; +}; + +/*! @name Structure to define gyroscope configuration */ +struct bmi2_gyro_config +{ + /*! Output data rate in Hz */ + uint8_t odr; + + /*! Bandwidth parameter */ + uint8_t bwp; + + /*! Filter performance mode */ + uint8_t filter_perf; + + /*! OIS Range */ + uint8_t ois_range; + + /*! Gyroscope Range */ + uint8_t range; + + /*! Selects noise performance */ + uint8_t noise_perf; +}; + +/*! @name Structure to define auxiliary sensor configuration */ +struct bmi2_aux_config +{ + /*! Enable/Disable auxiliary interface */ + uint8_t aux_en; + + /*! Manual or Auto mode*/ + uint8_t manual_en; + + /*! Enables FCU write command on auxiliary interface */ + uint8_t fcu_write_en; + + /*! Read burst length for manual mode */ + uint8_t man_rd_burst; + + /*! Read burst length for data mode */ + uint8_t aux_rd_burst; + + /*! Output data rate */ + uint8_t odr; + + /*! Read-out offset */ + uint8_t offset; + + /*! I2c address of auxiliary sensor */ + uint8_t i2c_device_addr; + + /*! Read address of auxiliary sensor */ + uint8_t read_addr; +}; + +/*! @name Structure to define any-motion configuration */ +struct bmi2_any_motion_config +{ + /*! Duration in 50Hz samples(20msec) */ + uint16_t duration; + + /*! Acceleration slope threshold */ + uint16_t threshold; + + /*! To select per x-axis */ + uint16_t select_x; + + /*! To select per y-axis */ + uint16_t select_y; + + /*! 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 */ +struct bmi2_no_motion_config +{ + /*! Duration in 50Hz samples(20msec) */ + uint16_t duration; + + /*! Acceleration slope threshold */ + uint16_t threshold; + + /*! To select per x-axis */ + uint16_t select_x; + + /*! To select per y-axis */ + uint16_t select_y; + + /*! 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 */ +struct bmi2_sig_motion_config +{ + /*! Block size */ + uint16_t block_size; + + /*! Parameter 2 */ + uint16_t param_2; + + /*! Parameter 3 */ + uint16_t param_3; + + /*! Parameter 4 */ + uint16_t param_4; + + /*! 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 */ +struct bmi2_step_config +{ + /*! Water-mark level */ + uint16_t watermark_level; + + /*! 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; + uint8_t step_buffer_size; +}; + +/*! @name Structure to define gyroscope user gain configuration */ +struct bmi2_gyro_user_gain_config +{ + /*! Gain update value for x-axis */ + uint16_t ratio_x; + + /*! Gain update value for y-axis */ + uint16_t ratio_y; + + /*! Gain update value for z-axis */ + 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 */ + uint16_t sensitivity; + + /*! Enable -> Single Tap; Disable -> Double Tap */ + uint16_t single_tap_en; + + /*! Enable bits for enabling output into the register status bits */ + uint16_t out_conf; +}; + +/*! @name Structure to define orientation configuration */ +struct bmi2_orient_config +{ + /*! Upside/down detection */ + uint16_t ud_en; + + /*! Symmetrical, high or low Symmetrical */ + uint16_t mode; + + /*! Blocking mode */ + uint16_t blocking; + + /*! Threshold angle */ + uint16_t theta; + + /*! 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 */ +struct bmi2_high_g_config +{ + /*! Acceleration threshold */ + uint16_t threshold; + + /*! Hysteresis */ + uint16_t hysteresis; + + /*! To select per x-axis */ + uint16_t select_x; + + /*! To select per y-axis */ + uint16_t select_y; + + /*! To select per z-axis */ + uint16_t select_z; + + /*! 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 */ +struct bmi2_low_g_config +{ + /*! Acceleration threshold */ + uint16_t threshold; + + /*! Hysteresis */ + uint16_t hysteresis; + + /*! 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 */ +struct bmi2_flat_config +{ + /*! Theta angle for flat detection */ + uint16_t theta; + + /*! Blocking mode */ + uint16_t blocking; + + /*! Hysteresis for theta flat detection */ + uint16_t hysteresis; + + /*! 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 */ +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. */ + uint16_t min_flick_peak; + + /*! 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 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; +}; + +/*! @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. + * Default is 1448. */ + uint16_t min_angle_focus; + + /*! Cosine of min expected attitude change of the device within 1 second time window when + * moving from non-focus to focus position. + * The parameter is scaled by 2048 i.e. 2048 * cos(angle). Range is 1448 to 1856. + * Default value is 1774. */ + uint16_t min_angle_nonfocus; + + /*! Sine of the max allowed downward tilt angle in landscape right direction of the device, + * when it is in focus position + * (i.e. user is able to comfortably look at the dial of wear device). + * The configuration parameter is scaled by 2048 i.e. 2048 * sin(angle). Range is 700 to 1024. + * Default value is 1024. */ + uint16_t max_tilt_lr; + + /*! Sine of the max allowed downward tilt angle in landscape left direction of the device, + * when it is in focus position + * (i.e. user is able to comfortably look at the dial of wear device). + * The configuration parameter is scaled by 2048 i.e. 2048 * sin(angle). Range is 700 to + * 1024. Default value is 700. */ + uint16_t max_tilt_ll; + + /*! Sine of the max allowed backward tilt angle in portrait down direction of the device, + * when it is in focus position + * (i.e. user is able to comfortably look at the dial of wear device). + * The configuration parameter is scaled by 2048 i.e. 2048 * sin(angle). Range is 0 to179. + * Default value is 179. */ + uint16_t max_tilt_pd; + + /*! Sine of the maximum allowed forward tilt angle in portrait up direction of the + * device, when it is in focus position + * (i.e. user is able to comfortably look at the dial of wear device). + * The configuration parameter is scaled by 2048 i.e. 2048 * sin(angle). Range is 1774 to 1978. + * Default value is 1925. */ + uint16_t max_tilt_pu; +}; + +/*! @name Structure to define wrist gesture configuration for wearable variant */ +struct bmi2_wrist_gest_w_config +{ + /*! Wearable arm (left or right) */ + uint8_t wearable_arm : 1; + + /*! Enable bits for enabling output into the register status bits */ + uint8_t out_conf : 4; + + /*! 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; + + /*! 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 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; + + /*! 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; +}; + +/*! @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. + * Default is 1448. */ + uint16_t min_angle_focus; + + /*! Cosine of min expected attitude change of the device within 1 second time window when + * moving from non-focus to focus position. + * The parameter is scaled by 2048 i.e. 2048 * cos(angle). Range is 1448 to 1856. + * Default value is 1774. */ + uint16_t min_angle_nonfocus; + + /*! Sine of the max allowed downward tilt angle in landscape right direction of the device, + * when it is in focus position (i.e. user is able to comfortably look at the dial of wear device). + * The configuration parameter is scaled by 256 i.e. 256 * sin(angle). Range is 88 to 128. + * Default value is 128. */ + uint8_t angle_lr; + + /*! Sine of the max allowed downward tilt angle in landscape left direction of the device, + * when it is in focus position (i.e. user is able to comfortably look at the dial of wear device). + * The configuration parameter is scaled by 256 i.e. 256 * sin(angle). Range is 88 to 128. + * Default value is 128. */ + uint8_t angle_ll; + + /*! Sine of the max allowed backward tilt angle in portrait down direction of the device, + * when it is in focus position (i.e. user is able to comfortably look at the dial of wear device). + * The configuration parameter is scaled by 256 i.e. 256 * sin(angle). Range is 0 to 179. + * Default value is 22. */ + uint8_t angle_pd; + + /*! Sine of the maximum allowed forward tilt angle in portrait up direction of the device, + * when it is in focus position (i.e. user is able to comfortably look at the dial of wear device). + * The configuration parameter is scaled by 256 i.e. 256 * sin(angle). Range is 222 to 247. + * Default value is 241. */ + uint8_t angle_pu; + + /*! Minimum duration the arm should be moved while performing gesture. Range: 1 to 10, + * resolution = 20 ms. + * Default 2(40 ms)*/ + uint8_t min_dur_mov; + + /*! Minimum duration the arm should be static between two consecutive gestures. Range: 1 to + * 10, resolution = 20 ms + * Default 2(40 ms)*/ + uint8_t min_dur_quite; +}; + +/*! @name Structure to define primary OIS configuration */ +struct bmi2_primary_ois_config +{ + uint8_t lp_filter_enable; + + uint8_t lp_filter_config; + + uint8_t primary_ois_reserved; + + uint8_t primary_ois_gyro_en; + + uint8_t primary_ois_accel_en; +}; + +/*! @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]; +}; + +/*! @name Union to define the sensor configurations */ +union bmi2_sens_config_types +{ + /*! Accelerometer configuration */ + struct bmi2_accel_config acc; + + /*! Gyroscope configuration */ + struct bmi2_gyro_config gyr; + + /*! Auxiliary configuration */ + struct bmi2_aux_config aux; + + /*! Any-motion configuration */ + struct bmi2_any_motion_config any_motion; + + /*! No-motion configuration */ + struct bmi2_no_motion_config no_motion; + + /*! Sig_motion configuration */ + struct bmi2_sig_motion_config sig_motion; + + /*! Step counter parameter configuration */ + uint16_t step_counter_params[BMI2_STEP_CNT_N_PARAMS]; + + /*! Step counter/detector/activity configuration */ + struct bmi2_step_config step_counter; + + /*! 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; + + /*! Orientation configuration */ + struct bmi2_orient_config orientation; + + /*! High-g configuration */ + struct bmi2_high_g_config high_g; + + /*! Low-g configuration */ + struct bmi2_low_g_config low_g; + + /*! 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; + + /*! Wrist wear wake-up configuration */ + struct bmi2_wrist_wear_wake_up_config wrist_wear_wake_up; + + /*! Wrist gesture configuration for wearable variant */ + struct bmi2_wrist_gest_w_config wrist_gest_w; + + /*! Wrist wear wake-up configuration for wearable variant */ + struct bmi2_wrist_wear_wake_up_wh_config wrist_wear_wake_up_wh; + + /*! Primary OIS configuration */ + struct bmi2_primary_ois_config primary_ois; + + /* Free-fall detection configurations */ + struct bmi2_free_fall_det_config free_fall_det; +}; + +/*! @name Structure to define the type of the sensor and its configurations */ +struct bmi2_sens_config +{ + /*! Defines the type of sensor */ + uint8_t type; + + /*! Defines various sensor configurations */ + union bmi2_sens_config_types cfg; +}; + +/*! @name Structure to define the feature configuration */ +struct bmi2_feature_config +{ + /*! Defines the type of sensor */ + uint8_t type; + + /*! Page to where the feature is mapped */ + uint8_t page; + + /*! Address of the feature */ + uint8_t start_addr; +}; + +/*! @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; + + /*! To store warnings */ + uint8_t info; + + /*! Type of Interface */ + enum bmi2_intf_type intf; + + /*! For switching from I2C to SPI */ + uint8_t dummy_byte; + + /*! Resolution for FOC */ + uint8_t resolution; + + /*! User set read/write length */ + uint16_t read_write_len; + + /*! Pointer to the configuration data buffer address */ + const uint8_t *config_file_ptr; + + /*! To define maximum page number */ + uint8_t page_max; + + /*! To define maximum number of input sensors/features */ + uint8_t input_sens; + + /*! To define maximum number of output sensors/features */ + uint8_t out_sens; + + /*! Indicate manual enable for auxiliary communication */ + uint8_t aux_man_en; + + /*! Defines manual read burst length for auxiliary communication */ + uint8_t aux_man_rd_burst_len; + + /*! Array of feature input configuration structure */ + const struct bmi2_feature_config *feat_config; + + /*! 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; + + /*! Read function pointer */ + bmi2_read_fptr_t read; + + /*! Write function pointer */ + bmi2_write_fptr_t write; + + /*! Delay function pointer */ + bmi2_delay_fptr_t delay_us; + + /*! To store the gyroscope cross sensitivity value */ + int16_t gyr_cross_sens_zx; + + /* gyro enable status, used as a flag in CRT enabling and aborting */ + uint8_t gyro_en : 1; + + /* advance power saving mode status, used as a flag in CRT enabling and aborting */ + uint8_t aps_status; + + /* used as a flag to enable variant specific features like crt */ + uint16_t variant_feature; + + /* To store hold the size of config file */ + uint16_t config_size; +}; + +/*! @name Structure to enable an accel axis for foc */ +struct accel_foc_g_value +{ + /* '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 */ + uint8_t y; + + /* '0' to disable z axis and '1' to enable z axis */ + uint8_t z; + + /* '0' for positive input and '1' for negative input */ + uint8_t sign; +}; + +/*! @name Structure to configure activity recognition settings */ +struct act_recg_sett +{ + /* 1 to enable & 0 to disable post processing */ + uint8_t act_rec_1 : 1; + + uint16_t act_rec_2; + + uint16_t act_rec_3; + + uint8_t act_rec_4 : 4; + + uint8_t act_rec_5 : 4; +}; + +#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 new file mode 100644 index 000000000..407982fa8 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_ois.c @@ -0,0 +1,409 @@ +/** +* 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 +* +*//******************************************************************************/ + +/*! @name Header Files */ +/******************************************************************************/ +#include "bmi2_ois.h" + +/******************************************************************************/ + +/*! Local Function Prototypes + ******************************************************************************/ + +/*! + * @brief This internal API gets the OIS accelerometer and the gyroscope data. + * + * @param[out] ois_data : Structure instance of bmi2_sens_axes_data. + * @param[in] reg_addr : Register address where data is stored. + * @param[in] ois_dev : Structure instance of bmi2_ois_dev. + * @param[in] ois_gyr_cross_sens_zx :"gyroscope cross sensitivity value which was calculated during + * 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 + */ +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, + int16_t ois_gyr_cross_sens_zx); + +/*! + * @brief This internal API is used to validate the OIS device pointer for null + * conditions. + * + * @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 + */ +static int8_t null_ptr_check(const struct bmi2_ois_dev *ois_dev); + +/*! + * @brief This internal API corrects the gyroscope cross-axis sensitivity + * between the z and the x axis. + * + * @param[in] ois_dev : Structure instance of bmi2_ois_dev. + * @param[in] ois_gyr_cross_sens_zx : "gyroscope cross sensitivity value which was calculated during bmi2xy_init(), + * refer the example ois_accel_gyro.c for more info" + * + */ +static void comp_gyro_cross_axis_sensitivity(struct bmi2_ois_sens_axes_data *ois_data, int16_t ois_gyr_cross_sens_zx); + +/******************************************************************************/ +/*! @name User Interface Definitions */ +/******************************************************************************/ + +/*! + * @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) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define dummy byte for SPI read */ + uint8_t dummy_byte = 1; + + /* Variable to define temporary length */ + uint16_t temp_len = data_len + dummy_byte; + + /* Variable to define temporary buffer */ + uint8_t temp_buf[temp_len]; + + /* Variable to index bytes read */ + uint16_t index = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(ois_dev); + if ((rslt == BMI2_OIS_OK) && (ois_reg_data != NULL)) + { + /* Configuring reg_addr for SPI Interface */ + 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) + { + /* Read the data from the position next to dummy byte */ + while (index < data_len) + { + ois_reg_data[index] = temp_buf[index + dummy_byte]; + index++; + } + } + else + { + rslt = BMI2_OIS_E_COM_FAIL; + } + } + else + { + rslt = BMI2_OIS_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @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, + uint16_t data_len, + const struct bmi2_ois_dev *ois_dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(ois_dev); + if ((rslt == BMI2_OIS_OK) && (ois_reg_data != NULL)) + { + /* Configuring reg_addr for SPI Interface */ + 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) + { + rslt = BMI2_OIS_E_COM_FAIL; + } + } + else + { + rslt = BMI2_OIS_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @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) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store data */ + uint8_t reg_data = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(ois_dev); + if (rslt == BMI2_OIS_OK) + { + rslt = bmi2_ois_get_regs(BMI2_OIS_CONFIG_ADDR, ®_data, 1, ois_dev); + if (rslt == BMI2_OIS_OK) + { + /* Enable/Disable Low pass filter */ + reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_OIS_LP_FILTER_EN, ois_dev->lp_filter_en); + + /* Configures Low pass filter cut-off frequency */ + reg_data = BMI2_OIS_SET_BITS(reg_data, BMI2_OIS_LP_FILTER_CONFIG, ois_dev->lp_filter_config); + + /* Low pass filter - mute on secondary interface */ + reg_data = BMI2_OIS_SET_BITS(reg_data, BMI2_OIS_LP_FILTER_MUTE, ois_dev->lp_filter_mute); + + /* Enable/Disable ois on accelerometer */ + reg_data = BMI2_OIS_SET_BITS(reg_data, BMI2_OIS_ACC_EN, ois_dev->acc_en); + + /* Enable/Disable ois on gyroscope */ + reg_data = BMI2_OIS_SET_BITS(reg_data, BMI2_OIS_GYR_EN, ois_dev->gyr_en); + + /* Set the OIS configurations */ + rslt = bmi2_ois_set_regs(BMI2_OIS_CONFIG_ADDR, ®_data, 1, ois_dev); + } + } + + return rslt; +} + +/*! + * @brief This API gets the status of accelerometer/gyroscope enable for data + * read through OIS interface. + */ +int8_t bmi2_ois_get_config(struct bmi2_ois_dev *ois_dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store data */ + uint8_t reg_data = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(ois_dev); + if (rslt == BMI2_OIS_OK) + { + rslt = bmi2_ois_get_regs(BMI2_OIS_CONFIG_ADDR, ®_data, 1, ois_dev); + if (rslt == BMI2_OIS_OK) + { + /* Get the status of Low pass filter enable */ + ois_dev->lp_filter_en = BMI2_GET_BIT_POS0(reg_data, BMI2_OIS_LP_FILTER_EN); + + /* Get the status of Low pass filter cut-off frequency */ + ois_dev->lp_filter_config = BMI2_OIS_GET_BITS(reg_data, BMI2_OIS_LP_FILTER_CONFIG); + + /* Get the status of Low pass filter mute */ + ois_dev->lp_filter_mute = BMI2_OIS_GET_BITS(reg_data, BMI2_OIS_LP_FILTER_MUTE); + + /* Get the status of accelerometer enable */ + ois_dev->acc_en = BMI2_OIS_GET_BITS(reg_data, BMI2_OIS_ACC_EN); + + /* Get the status of gyroscope enable */ + ois_dev->gyr_en = BMI2_OIS_GET_BITS(reg_data, BMI2_OIS_GYR_EN); + } + } + + return rslt; +} + +/*! + * @brief This API reads accelerometer/gyroscope data through OIS interface. + */ +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) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop = 0; + + /* Variable to update register address */ + uint8_t reg_addr = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(ois_dev); + if ((rslt == BMI2_OIS_OK) && (sens_sel != NULL)) + { + + for (loop = 0; loop < n_sens; loop++) + { + switch (sens_sel[loop]) + { + case BMI2_OIS_ACCEL: + + /* Update OIS accelerometer address */ + reg_addr = BMI2_OIS_ACC_X_LSB_ADDR; + + /* Get OIS accelerometer data */ + rslt = get_ois_acc_gyr_data(&ois_dev->acc_data, reg_addr, ois_dev, 0); + break; + case BMI2_OIS_GYRO: + + /* Update OIS gyroscope address */ + reg_addr = BMI2_OIS_GYR_X_LSB_ADDR; + + /* Get OIS gyroscope data */ + rslt = get_ois_acc_gyr_data(&ois_dev->gyr_data, reg_addr, ois_dev, gyr_cross_sens_zx); + break; + default: + rslt = BMI2_OIS_E_INVALID_SENSOR; + break; + } + + /* Return error if any of the get sensor data fails */ + if (rslt != BMI2_OIS_OK) + { + break; + } + } + } + else + { + rslt = BMI2_OIS_E_NULL_PTR; + } + + return rslt; +} + +/***************************************************************************/ + +/*! Local Function Definitions + ****************************************************************************/ + +/*! + * @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, + int16_t ois_gyr_cross_sens_zx) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variables to store MSB value */ + uint8_t msb; + + /* Variables to store LSB value */ + uint8_t lsb; + + /* Variables to store both MSB and LSB value */ + uint16_t msb_lsb; + + /* Variables to define index */ + uint8_t index = 0; + + /* Array to define data stored in register */ + uint8_t reg_data[BMI2_OIS_ACC_GYR_NUM_BYTES] = { 0 }; + + /* Read the sensor data */ + rslt = bmi2_ois_get_regs(reg_addr, reg_data, BMI2_OIS_ACC_GYR_NUM_BYTES, ois_dev); + if (rslt == BMI2_OIS_OK) + { + /* Read x-axis data */ + lsb = reg_data[index++]; + msb = reg_data[index++]; + msb_lsb = ((uint16_t)msb << 8) | (uint16_t)lsb; + ois_data->x = (int16_t)msb_lsb; + + /* Read y-axis data */ + lsb = reg_data[index++]; + msb = reg_data[index++]; + msb_lsb = ((uint16_t)msb << 8) | (uint16_t)lsb; + ois_data->y = (int16_t)msb_lsb; + + /* Read z-axis data */ + lsb = reg_data[index++]; + msb = reg_data[index++]; + msb_lsb = ((uint16_t)msb << 8) | (uint16_t)lsb; + ois_data->z = (int16_t)msb_lsb; + + comp_gyro_cross_axis_sensitivity(ois_data, ois_gyr_cross_sens_zx); + } + + return rslt; +} + +/*! + * @brief This internal API is used to validate the device structure pointer for + * null conditions. + */ +static int8_t null_ptr_check(const struct bmi2_ois_dev *ois_dev) +{ + /* Variable to define error */ + 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)) + { + /* Device structure pointer is NULL */ + rslt = BMI2_OIS_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This internal API corrects the gyroscope cross-axis sensitivity + * between the z and the x axis. + */ +static void comp_gyro_cross_axis_sensitivity(struct bmi2_ois_sens_axes_data *ois_data, int16_t ois_gyr_cross_sens_zx) +{ + + /* 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); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_ois.h b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_ois.h new file mode 100644 index 000000000..cd1e0f56d --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_ois.h @@ -0,0 +1,311 @@ +/** +* 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 +* +*/ + +#ifndef BMI2_OIS_H_ +#define BMI2_OIS_H_ + +/*! CPP guard */ +#ifdef __cplusplus +extern "C" { +#endif + +/***************************************************************************/ + +/*! Header files + ****************************************************************************/ +#ifdef __KERNEL__ +#include +#include +#else +#include +#include +#endif + +/******************************************************************************/ +/*! @name Macros */ +/******************************************************************************/ +/*! @name Utility macros */ +#define BMI2_OIS_SET_BITS(reg_data, bitname, data) \ + ((reg_data & ~(bitname##_MASK)) | \ + ((data << bitname##_POS) & bitname##_MASK)) + +#define BMI2_OIS_GET_BITS(reg_data, bitname) \ + ((reg_data & (bitname##_MASK)) >> \ + (bitname##_POS)) + +#define BMI2_SET_BIT_POS0(reg_data, bitname, data) \ + ((reg_data & ~(bitname##_MASK)) | \ + (data & bitname##_MASK)) + +#define BMI2_GET_BIT_POS0(reg_data, bitname) (reg_data & (bitname##_MASK)) + +/*! @name To define success code */ +#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) + +/*! @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) + +/*! @name BMI2 OIS data bytes */ +#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) + +/*! @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) + +/*! @name Mask definitions for OIS configurations */ +#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) + +/*! 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_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) + +/******************************************************************************/ +/*! @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); + +/******************************************************************************/ +/*! @name Structure Declarations */ +/******************************************************************************/ +/*! @name Structure to define accelerometer and gyroscope sensor axes for OIS */ +struct bmi2_ois_sens_axes_data +{ + /*! Data in x-axis */ + int16_t x; + + /*! Data in y-axis */ + int16_t y; + + /*! Data in z-axis */ + int16_t z; + +}; + +/*! @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; + + /*! Write function pointer */ + bmi2_ois_com_fptr_t ois_write; + + /*! Delay function pointer */ + bmi2_ois_delay_fptr_t ois_delay_ms; + + /*! Low pass filter en/dis */ + uint8_t lp_filter_en; + + /*! Low pass filter cut-off frequency */ + uint8_t lp_filter_config; + + /*! Low pass filter mute */ + uint8_t lp_filter_mute; + + /*! Accelerometer enable for OIS */ + uint8_t acc_en; + + /*! Gyroscope enable for OIS */ + uint8_t gyr_en; + + /*! Accelerometer data axes */ + struct bmi2_ois_sens_axes_data acc_data; + + /*! Gyroscope data axes */ + struct bmi2_ois_sens_axes_data gyr_data; + +}; + +/***************************************************************************/ + +/*! BMI2 OIS User Interface function prototypes + ****************************************************************************/ + +/*! + * @brief 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. + * @param[out] ois_reg_data : Pointer to data buffer where read data is stored. + * @param[in] data_len : No. of bytes of data to be read. + * @param[in] ois_dev : Structure instance of bmi2_ois_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +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); + +/*! + * @brief 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 + * is stored. + * @param[in] data_len : No. of bytes of data to be written. + * @param[in] ois_dev : Structure instance of bmi2_ois_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +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); + +/*! + * @brief This API enables/disables 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 + */ +int8_t bmi2_ois_set_config(const struct bmi2_ois_dev *ois_dev); + +/*! + * @brief 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. + * + * @note Enabling and disabling is done during OIS structure initialization. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +int8_t bmi2_ois_get_config(struct bmi2_ois_dev *ois_dev); + +/*! + * @brief 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. + * @param[in, out] ois_dev : Structure instance of bmi2_ois_dev. + * @param[in] gyr_cross_sens_zx : Store the gyroscope cross sensitivity values taken from the bmi2xy + * (refer bmi2_ois example). + * + * sens_sel | Value + * ---------------|--------------- + * BMI2_OIS_ACCEL | 0x01 + * BMI2_OIS_GYRO | 0x02 + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +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_ */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/example/basic.c b/lib/main/BoschSensortec/BMI270-Sensor-API/example/basic.c new file mode 100644 index 000000000..98c832785 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/example/basic.c @@ -0,0 +1,177 @@ +#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 new file mode 100644 index 000000000..e2388f3eb --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/example/crt_ex.c @@ -0,0 +1,219 @@ +#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 new file mode 100644 index 000000000..54b97a313 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/example/motion_interrupt.c @@ -0,0 +1,230 @@ +#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 new file mode 100644 index 000000000..9856fc32b --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/example/step_counter.c @@ -0,0 +1,238 @@ +#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 new file mode 100644 index 000000000..c70b25260 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/example/wrist_gesture.c @@ -0,0 +1,244 @@ +#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 new file mode 100644 index 000000000..82410619a --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/example/wrist_wear_wakeup.c @@ -0,0 +1,235 @@ +#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/make/source.mk b/make/source.mk index 2ffb17bb7..aeb306868 100644 --- a/make/source.mk +++ b/make/source.mk @@ -188,7 +188,8 @@ COMMON_SRC = \ io/vtx_rtc6705.c \ io/vtx_smartaudio.c \ io/vtx_tramp.c \ - io/vtx_control.c + io/vtx_control.c \ + ./lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.c COMMON_DEVICE_SRC = \ $(CMSIS_SRC) \ diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 6815c75af..fe28f516e 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -127,14 +127,14 @@ const char * const lookupTableAccHardware[] = { "AUTO", "NONE", "ADXL345", "MPU6050", "MMA8452", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20649", "ICM20689", "ICM42605", - "BMI160", "FAKE" + "BMI160", "BMI270", "FAKE" }; // sync with gyroHardware_e const char * const lookupTableGyroHardware[] = { "AUTO", "NONE", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20649", "ICM20689", "ICM42605", - "BMI160", "FAKE" + "BMI160", "BMI270", "FAKE" }; #if defined(USE_SENSOR_NAMES) || defined(USE_BARO) diff --git a/src/main/drivers/accgyro/accgyro.h b/src/main/drivers/accgyro/accgyro.h index 4b206cff8..d87a10b57 100644 --- a/src/main/drivers/accgyro/accgyro.h +++ b/src/main/drivers/accgyro/accgyro.h @@ -54,6 +54,7 @@ typedef enum { GYRO_ICM20689, GYRO_ICM42605, GYRO_BMI160, + GYRO_BMI270, GYRO_FAKE } gyroHardware_e; @@ -68,6 +69,7 @@ typedef enum { GYRO_RATE_1_kHz, GYRO_RATE_1100_Hz, GYRO_RATE_3200_Hz, + GYRO_RATE_6400_Hz, GYRO_RATE_8_kHz, GYRO_RATE_9_kHz, GYRO_RATE_32_kHz, diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index 9d8c1ef75..17f22e039 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -47,6 +47,7 @@ #include "drivers/accgyro/accgyro_mpu6050.h" #include "drivers/accgyro/accgyro_mpu6500.h" #include "drivers/accgyro/accgyro_spi_bmi160.h" +#include "drivers/accgyro/accgyro_spi_bmi270.h" #include "drivers/accgyro/accgyro_spi_icm20649.h" #include "drivers/accgyro/accgyro_spi_icm20689.h" #include "drivers/accgyro/accgyro_spi_icm42605.h" @@ -205,17 +206,20 @@ static gyroSpiDetectFn_t gyroSpiDetectFnTable[] = { #ifdef USE_GYRO_SPI_MPU9250 mpu9250SpiDetect, #endif -#ifdef USE_GYRO_SPI_ICM20649 - icm20649SpiDetect, -#endif #ifdef USE_GYRO_SPI_ICM20689 icm20689SpiDetect, // icm20689SpiDetect detects ICM20602 and ICM20689 #endif +#ifdef USE_ACCGYRO_BMI160 + bmi160Detect, +#endif +#ifdef USE_ACCGYRO_BMI270 + bmi270Detect, +#endif #ifdef USE_GYRO_SPI_ICM42605 icm42605SpiDetect, #endif -#ifdef USE_ACCGYRO_BMI160 - bmi160Detect, +#ifdef USE_GYRO_SPI_ICM20649 + icm20649SpiDetect, #endif #ifdef USE_GYRO_L3GD20 l3gd20Detect, diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h index ea546db60..83453de20 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.h +++ b/src/main/drivers/accgyro/accgyro_mpu.h @@ -201,6 +201,7 @@ typedef enum { ICM_20689_SPI, ICM_42605_SPI, BMI_160_SPI, + BMI_270_SPI, L3GD20_SPI, } mpuSensor_e; diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi270.c b/src/main/drivers/accgyro/accgyro_spi_bmi270.c new file mode 100644 index 000000000..2bea5a843 --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_spi_bmi270.c @@ -0,0 +1,455 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include +#include + +#include "platform.h" + +#ifdef USE_ACCGYRO_BMI270 + +#include "drivers/accgyro/accgyro.h" +#include "drivers/accgyro/accgyro_spi_bmi270.h" +#include "drivers/bus_spi.h" +#include "drivers/exti.h" +#include "drivers/io.h" +#include "drivers/io_impl.h" +#include "drivers/nvic.h" +#include "drivers/sensor.h" +#include "drivers/time.h" + +// Include the device config (microcode) that must be uploaded to the sensor +#include "../../../../lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.h" + +#define BMI270_SPI_DIVISOR 16 +#define BMI270_FIFO_FRAME_SIZE 6 + +// BMI270 registers (not the complete list) +typedef enum { + BMI270_REG_CHIP_ID = 0x00, + BMI270_REG_ERR_REG = 0x02, + BMI270_REG_STATUS = 0x03, + BMI270_REG_ACC_DATA_X_LSB = 0x0C, + BMI270_REG_GYR_DATA_X_LSB = 0x12, + BMI270_REG_SENSORTIME_0 = 0x18, + BMI270_REG_SENSORTIME_1 = 0x19, + BMI270_REG_SENSORTIME_2 = 0x1A, + BMI270_REG_EVENT = 0x1B, + BMI270_REG_INT_STATUS_0 = 0x1C, + BMI270_REG_INT_STATUS_1 = 0x1D, + BMI270_REG_INTERNAL_STATUS = 0x21, + BMI270_REG_TEMPERATURE_LSB = 0x22, + BMI270_REG_TEMPERATURE_MSB = 0x23, + BMI270_REG_FIFO_LENGTH_LSB = 0x24, + BMI270_REG_FIFO_LENGTH_MSB = 0x25, + BMI270_REG_FIFO_DATA = 0x26, + BMI270_REG_ACC_CONF = 0x40, + BMI270_REG_ACC_RANGE = 0x41, + BMI270_REG_GYRO_CONF = 0x42, + BMI270_REG_GYRO_RANGE = 0x43, + BMI270_REG_AUX_CONF = 0x44, + BMI270_REG_FIFO_DOWNS = 0x45, + BMI270_REG_FIFO_WTM_0 = 0x46, + BMI270_REG_FIFO_WTM_1 = 0x47, + BMI270_REG_FIFO_CONFIG_0 = 0x48, + BMI270_REG_FIFO_CONFIG_1 = 0x49, + BMI270_REG_SATURATION = 0x4A, + BMI270_REG_INT1_IO_CTRL = 0x53, + BMI270_REG_INT2_IO_CTRL = 0x54, + BMI270_REG_INT_LATCH = 0x55, + BMI270_REG_INT1_MAP_FEAT = 0x56, + BMI270_REG_INT2_MAP_FEAT = 0x57, + BMI270_REG_INT_MAP_DATA = 0x58, + BMI270_REG_INIT_CTRL = 0x59, + BMI270_REG_INIT_DATA = 0x5E, + BMI270_REG_ACC_SELF_TEST = 0x6D, + BMI270_REG_GYR_SELF_TEST_AXES = 0x6E, + BMI270_REG_PWR_CONF = 0x7C, + BMI270_REG_PWR_CTRL = 0x7D, + BMI270_REG_CMD = 0x7E, +} bmi270Register_e; + +// BMI270 register configuration values +typedef enum { + BMI270_VAL_CMD_SOFTRESET = 0xB6, + BMI270_VAL_CMD_FIFOFLUSH = 0xB0, + BMI270_VAL_PWR_CTRL = 0x0E, // enable gyro, acc and temp sensors + BMI270_VAL_PWR_CONF = 0x02, // disable advanced power save, enable FIFO self-wake + BMI270_VAL_ACC_CONF_ODR800 = 0x0B, // set acc sample rate to 800hz + BMI270_VAL_ACC_CONF_ODR1600 = 0x0C, // set acc sample rate to 1600hz + BMI270_VAL_ACC_CONF_BWP = 0x02, // set acc filter in normal mode + BMI270_VAL_ACC_CONF_HP = 0x01, // set acc in high performance mode + BMI270_VAL_ACC_RANGE_8G = 0x02, // set acc to 8G full scale + BMI270_VAL_ACC_RANGE_16G = 0x03, // set acc to 16G full scale + BMI270_VAL_GYRO_CONF_ODR3200 = 0x0D, // set gyro sample rate to 3200hz + BMI270_VAL_GYRO_CONF_BWP = 0x02, // set gyro filter in normal mode + BMI270_VAL_GYRO_CONF_NOISE_PERF = 0x01, // set gyro in high performance noise mode + BMI270_VAL_GYRO_CONF_FILTER_PERF = 0x01, // set gyro in high performance filter mode + + BMI270_VAL_GYRO_RANGE_2000DPS = 0x08, // set gyro to 2000dps full scale + // for some reason you have to enable the ois_range bit (bit 3) for 2000dps as well + // or else the gyro scale will be 250dps when in prefiltered FIFO mode (not documented in datasheet!) + + BMI270_VAL_INT_MAP_DATA_DRDY_INT1 = 0x04,// enable the data ready interrupt pin 1 + BMI270_VAL_INT_MAP_FIFO_WM_INT1 = 0x02, // enable the FIFO watermark interrupt pin 1 + BMI270_VAL_INT1_IO_CTRL_PINMODE = 0x0A, // active high, push-pull, output enabled, input disabled + BMI270_VAL_FIFO_CONFIG_0 = 0x00, // don't stop when full, disable sensortime frame + BMI270_VAL_FIFO_CONFIG_1 = 0x80, // only gyro data in FIFO, use headerless mode + BMI270_VAL_FIFO_DOWNS = 0x00, // select unfiltered gyro data with no downsampling (6.4KHz samples) + BMI270_VAL_FIFO_WTM_0 = 0x06, // set the FIFO watermark level to 1 gyro sample (6 bytes) + BMI270_VAL_FIFO_WTM_1 = 0x00, // FIFO watermark MSB +} bmi270ConfigValues_e; + +// BMI270 register reads are 16bits with the first byte a "dummy" value 0 +// that must be ignored. The result is in the second byte. +static uint8_t bmi270RegisterRead(const busDevice_t *bus, bmi270Register_e registerId) +{ + uint8_t data[2] = { 0, 0 }; + + if (spiBusReadRegisterBuffer(bus, registerId, data, 2)) { + return data[1]; + } else { + return 0; + } +} + +static void bmi270RegisterWrite(const busDevice_t *bus, bmi270Register_e registerId, uint8_t value, unsigned delayMs) +{ + spiBusWriteRegister(bus, registerId, value); + if (delayMs) { + delay(delayMs); + } +} + +// Toggle the CS to switch the device into SPI mode. +// Device switches initializes as I2C and switches to SPI on a low to high CS transition +static void bmi270EnableSPI(const busDevice_t *bus) +{ + IOLo(bus->busdev_u.spi.csnPin); + delay(1); + IOHi(bus->busdev_u.spi.csnPin); + delay(10); +} + +uint8_t bmi270Detect(const busDevice_t *bus) +{ + spiSetDivisor(bus->busdev_u.spi.instance, BMI270_SPI_DIVISOR); + bmi270EnableSPI(bus); + + if (bmi270RegisterRead(bus, BMI270_REG_CHIP_ID) == BMI270_CHIP_ID) { + return BMI_270_SPI; + } + + return MPU_NONE; +} + +static void bmi270UploadConfig(const busDevice_t *bus) +{ + bmi270RegisterWrite(bus, BMI270_REG_PWR_CONF, 0, 1); + bmi270RegisterWrite(bus, BMI270_REG_INIT_CTRL, 0, 1); + + // 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, BMI270_CONFIG_SIZE); + IOHi(bus->busdev_u.spi.csnPin); + + delay(10); + bmi270RegisterWrite(bus, BMI270_REG_INIT_CTRL, 1, 1); +} + +static void bmi270Config(const gyroDev_t *gyro) +{ + const busDevice_t *bus = &gyro->bus; + + // If running in hardware_lpf experimental mode then switch to FIFO-based, + // 6.4KHz sampling, unfiltered data vs. the default 3.2KHz with hardware filtering +#ifdef USE_GYRO_DLPF_EXPERIMENTAL + const bool fifoMode = (gyro->hardware_lpf == GYRO_HARDWARE_LPF_EXPERIMENTAL); +#else + const bool fifoMode = false; +#endif + + // Perform a soft reset to set all configuration to default + // Delay 100ms before continuing configuration + bmi270RegisterWrite(bus, BMI270_REG_CMD, BMI270_VAL_CMD_SOFTRESET, 100); + + // Toggle the chip into SPI mode + bmi270EnableSPI(bus); + + bmi270UploadConfig(bus); + + // Configure the FIFO + if (fifoMode) { + bmi270RegisterWrite(bus, BMI270_REG_FIFO_CONFIG_0, BMI270_VAL_FIFO_CONFIG_0, 1); + bmi270RegisterWrite(bus, BMI270_REG_FIFO_CONFIG_1, BMI270_VAL_FIFO_CONFIG_1, 1); + bmi270RegisterWrite(bus, BMI270_REG_FIFO_DOWNS, BMI270_VAL_FIFO_DOWNS, 1); + bmi270RegisterWrite(bus, BMI270_REG_FIFO_WTM_0, BMI270_VAL_FIFO_WTM_0, 1); + bmi270RegisterWrite(bus, BMI270_REG_FIFO_WTM_1, BMI270_VAL_FIFO_WTM_1, 1); + } + + // Configure the accelerometer + bmi270RegisterWrite(bus, BMI270_REG_ACC_CONF, (BMI270_VAL_ACC_CONF_HP << 7) | (BMI270_VAL_ACC_CONF_BWP << 4) | BMI270_VAL_ACC_CONF_ODR800, 1); + + // Configure the accelerometer full-scale range + bmi270RegisterWrite(bus, BMI270_REG_ACC_RANGE, BMI270_VAL_ACC_RANGE_16G, 1); + + // Configure the gyro + bmi270RegisterWrite(bus, BMI270_REG_GYRO_CONF, (BMI270_VAL_GYRO_CONF_FILTER_PERF << 7) | (BMI270_VAL_GYRO_CONF_NOISE_PERF << 6) | (BMI270_VAL_GYRO_CONF_BWP << 4) | BMI270_VAL_GYRO_CONF_ODR3200, 1); + + // Configure the gyro full-range scale + bmi270RegisterWrite(bus, BMI270_REG_GYRO_RANGE, BMI270_VAL_GYRO_RANGE_2000DPS, 1); + + // Configure the gyro data ready interrupt + if (fifoMode) { + // Interrupt driven by FIFO watermark level + bmi270RegisterWrite(bus, BMI270_REG_INT_MAP_DATA, BMI270_VAL_INT_MAP_FIFO_WM_INT1, 1); + } else { + // Interrupt driven by data ready + bmi270RegisterWrite(bus, BMI270_REG_INT_MAP_DATA, BMI270_VAL_INT_MAP_DATA_DRDY_INT1, 1); + } + + // Configure the behavior of the INT1 pin + bmi270RegisterWrite(bus, BMI270_REG_INT1_IO_CTRL, BMI270_VAL_INT1_IO_CTRL_PINMODE, 1); + + // Configure the device for performance mode + bmi270RegisterWrite(bus, BMI270_REG_PWR_CONF, BMI270_VAL_PWR_CONF, 1); + + // Enable the gyro, accelerometer and temperature sensor - disable aux interface + bmi270RegisterWrite(bus, BMI270_REG_PWR_CTRL, BMI270_VAL_PWR_CTRL, 1); + + // Flush the FIFO + if (fifoMode) { + bmi270RegisterWrite(bus, BMI270_REG_CMD, BMI270_VAL_CMD_FIFOFLUSH, 1); + } +} + +extiCallbackRec_t bmi270IntCallbackRec; + +#if defined(USE_GYRO_EXTI) && defined(USE_MPU_DATA_READY_SIGNAL) +void bmi270ExtiHandler(extiCallbackRec_t *cb) +{ + gyroDev_t *gyro = container_of(cb, gyroDev_t, exti); + gyro->dataReady = true; +} + +static void bmi270IntExtiInit(gyroDev_t *gyro) +{ + if (gyro->mpuIntExtiTag == IO_TAG_NONE) { + return; + } + + IO_t mpuIntIO = IOGetByTag(gyro->mpuIntExtiTag); + + IOInit(mpuIntIO, OWNER_GYRO_EXTI, 0); + EXTIHandlerInit(&gyro->exti, bmi270ExtiHandler); + EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IN_FLOATING, BETAFLIGHT_EXTI_TRIGGER_RISING); + EXTIEnable(mpuIntIO, true); +} +#endif + +static bool bmi270AccRead(accDev_t *acc) +{ + enum { + IDX_REG = 0, + IDX_SKIP, + IDX_ACCEL_XOUT_L, + IDX_ACCEL_XOUT_H, + IDX_ACCEL_YOUT_L, + IDX_ACCEL_YOUT_H, + IDX_ACCEL_ZOUT_L, + IDX_ACCEL_ZOUT_H, + BUFFER_SIZE, + }; + + uint8_t bmi270_rx_buf[BUFFER_SIZE]; + static const uint8_t bmi270_tx_buf[BUFFER_SIZE] = {BMI270_REG_ACC_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0, 0}; + + IOLo(acc->bus.busdev_u.spi.csnPin); + spiTransfer(acc->bus.busdev_u.spi.instance, bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE); // receive response + IOHi(acc->bus.busdev_u.spi.csnPin); + + acc->ADCRaw[X] = (int16_t)((bmi270_rx_buf[IDX_ACCEL_XOUT_H] << 8) | bmi270_rx_buf[IDX_ACCEL_XOUT_L]); + acc->ADCRaw[Y] = (int16_t)((bmi270_rx_buf[IDX_ACCEL_YOUT_H] << 8) | bmi270_rx_buf[IDX_ACCEL_YOUT_L]); + acc->ADCRaw[Z] = (int16_t)((bmi270_rx_buf[IDX_ACCEL_ZOUT_H] << 8) | bmi270_rx_buf[IDX_ACCEL_ZOUT_L]); + + return true; +} + +static bool bmi270GyroReadRegister(gyroDev_t *gyro) +{ + enum { + IDX_REG = 0, + IDX_SKIP, + IDX_GYRO_XOUT_L, + IDX_GYRO_XOUT_H, + IDX_GYRO_YOUT_L, + IDX_GYRO_YOUT_H, + IDX_GYRO_ZOUT_L, + IDX_GYRO_ZOUT_H, + BUFFER_SIZE, + }; + + uint8_t bmi270_rx_buf[BUFFER_SIZE]; + static const uint8_t bmi270_tx_buf[BUFFER_SIZE] = {BMI270_REG_GYR_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0, 0}; + + IOLo(gyro->bus.busdev_u.spi.csnPin); + spiTransfer(gyro->bus.busdev_u.spi.instance, bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE); // receive response + IOHi(gyro->bus.busdev_u.spi.csnPin); + + gyro->gyroADCRaw[X] = (int16_t)((bmi270_rx_buf[IDX_GYRO_XOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_XOUT_L]); + gyro->gyroADCRaw[Y] = (int16_t)((bmi270_rx_buf[IDX_GYRO_YOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_YOUT_L]); + gyro->gyroADCRaw[Z] = (int16_t)((bmi270_rx_buf[IDX_GYRO_ZOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_ZOUT_L]); + + return true; +} + +#ifdef USE_GYRO_DLPF_EXPERIMENTAL +static bool bmi270GyroReadFifo(gyroDev_t *gyro) +{ + enum { + IDX_REG = 0, + IDX_SKIP, + IDX_FIFO_LENGTH_L, + IDX_FIFO_LENGTH_H, + IDX_GYRO_XOUT_L, + IDX_GYRO_XOUT_H, + IDX_GYRO_YOUT_L, + IDX_GYRO_YOUT_H, + IDX_GYRO_ZOUT_L, + IDX_GYRO_ZOUT_H, + BUFFER_SIZE, + }; + + bool dataRead = false; + static const uint8_t bmi270_tx_buf[BUFFER_SIZE] = {BMI270_REG_FIFO_LENGTH_LSB | 0x80, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + uint8_t bmi270_rx_buf[BUFFER_SIZE]; + + // Burst read the FIFO length followed by the next 6 bytes containing the gyro axis data for + // the first sample in the queue. It's possible for the FIFO to be empty so we need to check the + // length before using the sample. + IOLo(gyro->bus.busdev_u.spi.csnPin); + spiTransfer(gyro->bus.busdev_u.spi.instance, bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE); // receive response + IOHi(gyro->bus.busdev_u.spi.csnPin); + + int fifoLength = (uint16_t)((bmi270_rx_buf[IDX_FIFO_LENGTH_H] << 8) | bmi270_rx_buf[IDX_FIFO_LENGTH_L]); + + if (fifoLength >= BMI270_FIFO_FRAME_SIZE) { + + const int16_t gyroX = (int16_t)((bmi270_rx_buf[IDX_GYRO_XOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_XOUT_L]); + const int16_t gyroY = (int16_t)((bmi270_rx_buf[IDX_GYRO_YOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_YOUT_L]); + const int16_t gyroZ = (int16_t)((bmi270_rx_buf[IDX_GYRO_ZOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_ZOUT_L]); + + // If the FIFO data is invalid then the returned values will be 0x8000 (-32768) (pg. 43 of datasheet). + // This shouldn't happen since we're only using the data if the FIFO length indicates + // that data is available, but this safeguard is needed to prevent bad things in + // case it does happen. + if ((gyroX != INT16_MIN) || (gyroY != INT16_MIN) || (gyroZ != INT16_MIN)) { + gyro->gyroADCRaw[X] = gyroX; + gyro->gyroADCRaw[Y] = gyroY; + gyro->gyroADCRaw[Z] = gyroZ; + dataRead = true; + } + fifoLength -= BMI270_FIFO_FRAME_SIZE; + } + + // If there are additional samples in the FIFO then we don't use those for now and simply + // flush the FIFO. Under normal circumstances we only expect one sample in the FIFO since + // the gyro loop is running at the native sample rate of 6.4KHz. + // However the way the FIFO works in the sensor is that if a frame is partially read then + // it remains in the queue instead of bein removed. So if we ever got into a state where there + // was a partial frame or other unexpected data in the FIFO is may never get cleared and we + // would end up in a lock state of always re-reading the same partial or invalid sample. + if (fifoLength > 0) { + // Partial or additional frames left - flush the FIFO + bmi270RegisterWrite(&gyro->bus, BMI270_REG_CMD, BMI270_VAL_CMD_FIFOFLUSH, 0); + } + + return dataRead; +} +#endif + +static bool bmi270GyroRead(gyroDev_t *gyro) +{ +#ifdef USE_GYRO_DLPF_EXPERIMENTAL + if (gyro->hardware_lpf == GYRO_HARDWARE_LPF_EXPERIMENTAL) { + // running in 6.4KHz FIFO mode + return bmi270GyroReadFifo(gyro); + } else +#endif + { + // running in 3.2KHz register mode + return bmi270GyroReadRegister(gyro); + } +} + +static void bmi270SpiGyroInit(gyroDev_t *gyro) +{ + bmi270Config(gyro); + +#if defined(USE_GYRO_EXTI) && defined(USE_MPU_DATA_READY_SIGNAL) + bmi270IntExtiInit(gyro); +#endif +} + +static void bmi270SpiAccInit(accDev_t *acc) +{ + // sensor is configured during gyro init + acc->acc_1G = 512 * 4; // 16G sensor scale +} + +bool bmi270SpiAccDetect(accDev_t *acc) +{ + if (acc->mpuDetectionResult.sensor != BMI_270_SPI) { + return false; + } + + acc->initFn = bmi270SpiAccInit; + acc->readFn = bmi270AccRead; + + return true; +} + + +bool bmi270SpiGyroDetect(gyroDev_t *gyro) +{ + if (gyro->mpuDetectionResult.sensor != BMI_270_SPI) { + return false; + } + + gyro->initFn = bmi270SpiGyroInit; + gyro->readFn = bmi270GyroRead; + gyro->scale = 1.0f / 16.4f; + + return true; +} + +// Used to query the status register to determine what event caused the EXTI to fire. +// When in 3.2KHz mode the interrupt is mapped to the data ready state. However the data ready +// trigger will fire for both gyro and accelerometer. So it's necessary to check this register +// to determine which event caused the interrupt. +// When in 6.4KHz mode the interrupt is configured to be the FIFO watermark size of 6 bytes. +// Since in this mode we only put gyro data in the FIFO it's sufficient to check for the FIFO +// watermark reason as an idication of gyro data ready. +uint8_t bmi270InterruptStatus(gyroDev_t *gyro) +{ + return bmi270RegisterRead(&gyro->bus, BMI270_REG_INT_STATUS_1); +} +#endif // USE_ACCGYRO_BMI270 diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi270.h b/src/main/drivers/accgyro/accgyro_spi_bmi270.h new file mode 100644 index 000000000..c0ea0495f --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_spi_bmi270.h @@ -0,0 +1,28 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +#include "drivers/bus.h" + +uint8_t bmi270Detect(const busDevice_t *bus); +bool bmi270SpiAccDetect(accDev_t *acc); +bool bmi270SpiGyroDetect(gyroDev_t *gyro); +uint8_t bmi270InterruptStatus(gyroDev_t *gyro); diff --git a/src/main/drivers/accgyro/gyro_sync.c b/src/main/drivers/accgyro/gyro_sync.c index 0f66bbe5e..249142f18 100644 --- a/src/main/drivers/accgyro/gyro_sync.c +++ b/src/main/drivers/accgyro/gyro_sync.c @@ -58,6 +58,20 @@ uint16_t gyroSetSampleRate(gyroDev_t *gyro) gyroSampleRateHz = 3200; accSampleRateHz = 800; break; + case BMI_270_SPI: +#ifdef USE_GYRO_DLPF_EXPERIMENTAL + if (gyro->hardware_lpf == GYRO_HARDWARE_LPF_EXPERIMENTAL) { + // 6.4KHz sampling, but data is unfiltered (no hardware DLPF) + gyro->gyroRateKHz = GYRO_RATE_6400_Hz; + gyroSampleRateHz = 6400; + } else +#endif + { + gyro->gyroRateKHz = GYRO_RATE_3200_Hz; + gyroSampleRateHz = 3200; + } + accSampleRateHz = 800; + break; case ICM_20649_SPI: gyro->gyroRateKHz = GYRO_RATE_9_kHz; gyroSampleRateHz = 9000; diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index ddb2968ae..4d912c83e 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -43,6 +43,7 @@ #include "drivers/accgyro/accgyro_mpu6050.h" #include "drivers/accgyro/accgyro_mpu6500.h" #include "drivers/accgyro/accgyro_spi_bmi160.h" +#include "drivers/accgyro/accgyro_spi_bmi270.h" #include "drivers/accgyro/accgyro_spi_icm20649.h" #include "drivers/accgyro/accgyro_spi_icm20689.h" #include "drivers/accgyro/accgyro_spi_icm42605.h" @@ -292,6 +293,15 @@ retry: FALLTHROUGH; #endif +#ifdef USE_ACCGYRO_BMI270 + case ACC_BMI270: + if (bmi270SpiAccDetect(dev)) { + accHardware = ACC_BMI270; + break; + } + FALLTHROUGH; +#endif + #ifdef USE_FAKE_ACC case ACC_FAKE: if (fakeAccDetect(dev)) { diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index b060b7626..bf9a646f5 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -44,6 +44,7 @@ typedef enum { ACC_ICM20689, ACC_ICM42605, ACC_BMI160, + ACC_BMI270, ACC_FAKE } accelerationSensor_e; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 403b7eee4..53aa348d5 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -45,6 +45,7 @@ #include "drivers/accgyro/accgyro_mpu6050.h" #include "drivers/accgyro/accgyro_mpu6500.h" #include "drivers/accgyro/accgyro_spi_bmi160.h" +#include "drivers/accgyro/accgyro_spi_bmi270.h" #include "drivers/accgyro/accgyro_spi_icm20649.h" #include "drivers/accgyro/accgyro_spi_icm20689.h" #include "drivers/accgyro/accgyro_spi_icm20689.h" @@ -352,6 +353,15 @@ STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev) FALLTHROUGH; #endif +#ifdef USE_ACCGYRO_BMI270 + case GYRO_BMI270: + if (bmi270SpiGyroDetect(dev)) { + gyroHardware = GYRO_BMI270; + break; + } + FALLTHROUGH; +#endif + #ifdef USE_FAKE_GYRO case GYRO_FAKE: if (fakeGyroDetect(dev)) { @@ -376,7 +386,8 @@ STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev) static void gyroPreInitSensor(const gyroDeviceConfig_t *config) { #if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \ - || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) || defined(USE_GYRO_SPI_ICM20689) + || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) \ + || defined(USE_GYRO_SPI_ICM20689) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) mpuPreInit(config); #else UNUSED(config); @@ -386,7 +397,8 @@ static void gyroPreInitSensor(const gyroDeviceConfig_t *config) static bool gyroDetectSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config) { #if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \ - || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_L3GD20) + || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) \ + || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_L3GD20) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) bool gyroFound = mpuDetect(&gyroSensor->gyroDev, config); @@ -413,10 +425,10 @@ static void gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *c gyroSensor->gyroDev.gyroAlign = config->alignment; buildRotationMatrixFromAlignment(&config->customAlignment, &gyroSensor->gyroDev.rotationMatrix); gyroSensor->gyroDev.mpuIntExtiTag = config->extiTag; + gyroSensor->gyroDev.hardware_lpf = gyroConfig()->gyro_hardware_lpf; // The targetLooptime gets set later based on the active sensor's gyroSampleRateHz and pid_process_denom gyroSensor->gyroDev.gyroSampleRateHz = gyroSetSampleRate(&gyroSensor->gyroDev); - gyroSensor->gyroDev.hardware_lpf = gyroConfig()->gyro_hardware_lpf; gyroSensor->gyroDev.initFn(&gyroSensor->gyroDev); // As new gyros are supported, be sure to add them below based on whether they are subject to the overflow/inversion bug @@ -430,6 +442,7 @@ static void gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *c case GYRO_MPU3050: case GYRO_L3GD20: case GYRO_BMI160: + case GYRO_BMI270: case GYRO_MPU6000: case GYRO_MPU6500: case GYRO_MPU9250: diff --git a/src/main/target/common_unified.h b/src/main/target/common_unified.h index fa3adbe87..f6bf25d72 100644 --- a/src/main/target/common_unified.h +++ b/src/main/target/common_unified.h @@ -44,6 +44,9 @@ #define USE_GYRO_SPI_MPU6500 #define USE_ACC_SPI_ICM20689 #define USE_GYRO_SPI_ICM20689 +#if (TARGET_FLASH_SIZE > 512) +#define USE_ACCGYRO_BMI270 +#endif // Other USE_ACCs and USE_GYROs should follow #define USE_MAG