diff --git a/LICENSE b/LICENSE index 49b2d50..df241f2 100644 --- a/LICENSE +++ b/LICENSE @@ -1,39 +1,30 @@ -Copyright (C) 2015 - 2016 Bosch Sensortec GmbH +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: -Redistributions of source code must retain the above copyright -notice, this list of conditions and the following disclaimer. - -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. +1. Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. -Neither the name of the copyright holder nor the names of the -contributors may be used to endorse or promote products derived from -this software without specific prior written permission. +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. -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 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 +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. -The information provided is believed to be accurate and reliable. -The copyright holder assumes no responsibility -for the consequences of use -of such information nor for any infringement of patents or -other rights of third parties which may result from its use. -No license is granted by implication or otherwise under any patent or -patent rights of the copyright holder. \ No newline at end of file +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/README.md b/README.md index bf54576..f032e24 100644 --- a/README.md +++ b/README.md @@ -1,793 +1,793 @@ -# BMI160 sensor API -## Introduction -This package contains the Bosch Sensortec's BMI160 sensor driver (sensor API) - -The sensor driver package includes bmi160.h, bmi160.c and bmi160_defs.h files - -## Version -File | Version | Date ---------------|---------|--------------- -bmi160.c | 3.7.7 | 13 Mar 2019 -bmi160.h | 3.7.7 | 13 Mar 2019 -bmi160_defs.h | 3.7.7 | 13 Mar 2019 - -## Integration details -* Integrate bmi160.h, bmi160_defs.h and bmi160.c file in to your project. -* Include the bmi160.h file in your code like below. -``` c -#include "bmi160.h" -``` - -## File information -* bmi160_defs.h : This header file has the constants, macros and datatype declarations. -* bmi160.h : This header file contains the declarations of the sensor driver APIs. -* bmi160.c : This source file contains the definitions of the sensor driver APIs. - -## Supported sensor interface -* SPI 4-wire -* I2C - -## Usage guide -### Initializing the sensor -To initialize the sensor, you will first need to create a device structure. You -can do this by creating an instance of the structure bmi160_dev. Then go on to -fill in the various parameters as shown below. - -#### Example for SPI 4-Wire -``` c -struct bmi160_dev sensor; - -/* You may assign a chip select identifier to be handled later */ -sensor.id = 0; -sensor.interface = BMI160_SPI_INTF; -sensor.read = user_spi_read; -sensor.write = user_spi_write; -sensor.delay_ms = user_delay_ms; - - -int8_t rslt = BMI160_OK; -rslt = bmi160_init(&sensor); -/* After the above function call, accel_cfg and gyro_cfg parameters in the device -structure are set with default values, found in the datasheet of the sensor */ -``` - -#### Example for I2C -``` c -struct bmi160_dev sensor; - -sensor.id = BMI160_I2C_ADDR; -sensor.interface = BMI160_I2C_INTF; -sensor.read = user_i2c_read; -sensor.write = user_i2c_write; -sensor.delay_ms = user_delay_ms; - -int8_t rslt = BMI160_OK; -rslt = bmi160_init(&sensor); -/* After the above function call, accel and gyro parameters in the device structure -are set with default values, found in the datasheet of the sensor */ -``` - -### Configuring accel and gyro sensor -#### Example for configuring accel and gyro sensors in normal mode -``` c - -int8_t rslt = BMI160_OK; - -/* Select the Output data rate, range of accelerometer sensor */ -sensor.accel_cfg.odr = BMI160_ACCEL_ODR_1600HZ; -sensor.accel_cfg.range = BMI160_ACCEL_RANGE_2G; -sensor.accel_cfg.bw = BMI160_ACCEL_BW_NORMAL_AVG4; - -/* Select the power mode of accelerometer sensor */ -sensor.accel_cfg.power = BMI160_ACCEL_NORMAL_MODE; - -/* Select the Output data rate, range of Gyroscope sensor */ -sensor.gyro_cfg.odr = BMI160_GYRO_ODR_3200HZ; -sensor.gyro_cfg.range = BMI160_GYRO_RANGE_2000_DPS; -sensor.gyro_cfg.bw = BMI160_GYRO_BW_NORMAL_MODE; - -/* Select the power mode of Gyroscope sensor */ -sensor.gyro_cfg.power = BMI160_GYRO_NORMAL_MODE; - -/* Set the sensor configuration */ -rslt = bmi160_set_sens_conf(&sensor); -``` - -### Reading sensor data -#### Example for reading sensor data -``` c - -int8_t rslt = BMI160_OK; -struct bmi160_sensor_data accel; -struct bmi160_sensor_data gyro; - -/* To read only Accel data */ -rslt = bmi160_get_sensor_data(BMI160_ACCEL_SEL, &accel, NULL, &sensor); - -/* To read only Gyro data */ -rslt = bmi160_get_sensor_data(BMI160_GYRO_SEL, NULL, &gyro, &sensor); - -/* To read both Accel and Gyro data */ -bmi160_get_sensor_data((BMI160_ACCEL_SEL | BMI160_GYRO_SEL), &accel, &gyro, &sensor); - -/* To read Accel data along with time */ -rslt = bmi160_get_sensor_data((BMI160_ACCEL_SEL | BMI160_TIME_SEL) , &accel, NULL, &sensor); - -/* To read Gyro data along with time */ -rslt = bmi160_get_sensor_data((BMI160_GYRO_SEL | BMI160_TIME_SEL), NULL, &gyro, &sensor); - -/* To read both Accel and Gyro data along with time*/ -bmi160_get_sensor_data((BMI160_ACCEL_SEL | BMI160_GYRO_SEL | BMI160_TIME_SEL), &accel, &gyro, &sensor); -``` - -### Setting the power mode of sensors -#### Example for setting power mode of accel and gyro -``` c - -int8_t rslt = BMI160_OK; - -/* Select the power mode */ -sensor.accel_cfg.power = BMI160_ACCEL_SUSPEND_MODE; -sensor.gyro_cfg.power = BMI160_GYRO_FASTSTARTUP_MODE; - -/* Set the Power mode */ -rslt = bmi160_set_power_mode(&sensor); - -/* Select the power mode */ -sensor.accel_cfg.power = BMI160_ACCEL_NORMAL_MODE; -sensor.gyro_cfg.power = BMI160_GYRO_NORMAL_MODE; - -/* Set the Power mode */ -rslt = bmi160_set_power_mode(&sensor); - -``` - -### Reading sensor data register -#### Example for reading Chip Address -``` c - -int8_t rslt = BMI160_OK; -uint8_t reg_addr = BMI160_CHIP_ID_ADDR; -uint8_t data; -uint16_t len = 1; -rslt = bmi160_get_regs(reg_addr, &data, len, &sensor); -``` - - -### Writing to sensor data register -#### Example for writing data to any motion threshold register -``` c - -int8_t rslt = BMI160_OK; -uint8_t reg_addr = BMI160_INT_MOTION_1_ADDR; -uint8_t data = 20; -uint16_t len = 1; -rslt = bmi160_set_regs(reg_addr, &data, len, &sensor); -``` - -### Resetting the device using soft-reset -#### Example for writing soft-reset command to command register -``` c - -int8_t rslt = BMI160_OK; -rslt = bmi160_soft_reset(&sensor); -``` - - -### Configuring interrupts for sensors -To configure the sensor interrupts, you will first need to create an interrupt -structure. You can do this by creating an instance of the structure bmi160_int_settg. -Then go on to fill in the various parameters as shown below - - -### Configuring Any-motion Interrupt -#### Example for configuring Any-motion Interrupt -Note:- User can check the currently active interrupt(any-motion or sig-motion) by checking the **any_sig_sel** of bmi160_dev structure. -``` c - -struct bmi160_int_settg int_config; - -/* Select the Interrupt channel/pin */ -int_config.int_channel = BMI160_INT_CHANNEL_1;// Interrupt channel/pin 1 - -/* Select the Interrupt type */ -int_config.int_type = BMI160_ACC_ANY_MOTION_INT;// Choosing Any motion interrupt -/* Select the interrupt channel/pin settings */ -int_config.int_pin_settg.output_en = BMI160_ENABLE;// Enabling interrupt pins to act as output pin -int_config.int_pin_settg.output_mode = BMI160_DISABLE;// Choosing push-pull mode for interrupt pin -int_config.int_pin_settg.output_type = BMI160_DISABLE;// Choosing active low output -int_config.int_pin_settg.edge_ctrl = BMI160_ENABLE;// Choosing edge triggered output -int_config.int_pin_settg.input_en = BMI160_DISABLE;// Disabling interrupt pin to act as input -int_config.int_pin_settg.latch_dur = BMI160_LATCH_DUR_NONE;// non-latched output - -/* Select the Any-motion interrupt parameters */ -int_config.int_type_cfg.acc_any_motion_int.anymotion_en = BMI160_ENABLE;// 1- Enable the any-motion, 0- disable any-motion -int_config.int_type_cfg.acc_any_motion_int.anymotion_x = BMI160_ENABLE;// Enabling x-axis for any motion interrupt -int_config.int_type_cfg.acc_any_motion_int.anymotion_y = BMI160_ENABLE;// Enabling y-axis for any motion interrupt -int_config.int_type_cfg.acc_any_motion_int.anymotion_z = BMI160_ENABLE;// Enabling z-axis for any motion interrupt -int_config.int_type_cfg.acc_any_motion_int.anymotion_dur = 0;// any-motion duration -int_config.int_type_cfg.acc_any_motion_int.anymotion_thr = 20;// (2-g range) -> (slope_thr) * 3.91 mg, (4-g range) -> (slope_thr) * 7.81 mg, (8-g range) ->(slope_thr) * 15.63 mg, (16-g range) -> (slope_thr) * 31.25 mg - -/* Set the Any-motion interrupt */ -bmi160_set_int_config(&int_config, &sensor); /* sensor is an instance of the structure bmi160_dev */ - -``` -### Configuring Flat Interrupt -#### Example for configuring Flat Interrupt -``` c - -struct bmi160_int_settg int_config; - -/* Select the Interrupt channel/pin */ -int_config.int_channel = BMI160_INT_CHANNEL_1;// Interrupt channel/pin 1 - -/* Select the Interrupt type */ -int_config.int_type = BMI160_ACC_FLAT_INT;// Choosing flat interrupt -/* Select the interrupt channel/pin settings */ -int_config.int_pin_settg.output_en = BMI160_ENABLE;// Enabling interrupt pins to act as output pin -int_config.int_pin_settg.output_mode = BMI160_DISABLE;// Choosing push-pull mode for interrupt pin -int_config.int_pin_settg.output_type = BMI160_DISABLE;// Choosing active low output -int_config.int_pin_settg.edge_ctrl = BMI160_ENABLE;// Choosing edge triggered output -int_config.int_pin_settg.input_en = BMI160_DISABLE;// Disabling interrupt pin to act as input -int_config.int_pin_settg.latch_dur = BMI160_LATCH_DUR_NONE;// non-latched output - -/* Select the Flat interrupt parameters */ -int_config.int_type_cfg.acc_flat_int.flat_en = BMI160_ENABLE;// 1-enable, 0-disable the flat interrupt -int_config.int_type_cfg.acc_flat_int.flat_theta = 8;// threshold for detection of flat position in range from 0° to 44.8°. -int_config.int_type_cfg.acc_flat_int.flat_hy = 1;// Flat hysteresis -int_config.int_type_cfg.acc_flat_int.flat_hold_time = 1;// Flat hold time (0 -> 0 ms, 1 -> 640 ms, 2 -> 1280 ms, 3 -> 2560 ms) - -/* Set the Flat interrupt */ -bmi160_set_int_config(&int_config, &sensor); /* sensor is an instance of the structure bmi160_dev */ - -``` - - -### Configuring Step Detector Interrupt -#### Example for configuring Step Detector Interrupt -``` c - -struct bmi160_int_settg int_config; - -/* Select the Interrupt channel/pin */ -int_config.int_channel = BMI160_INT_CHANNEL_1;// Interrupt channel/pin 1 - -/* Select the Interrupt type */ -int_config.int_type = BMI160_STEP_DETECT_INT;// Choosing Step Detector interrupt -/* Select the interrupt channel/pin settings */ -int_config.int_pin_settg.output_en = BMI160_ENABLE;// Enabling interrupt pins to act as output pin -int_config.int_pin_settg.output_mode = BMI160_DISABLE;// Choosing push-pull mode for interrupt pin -int_config.int_pin_settg.output_type = BMI160_ENABLE;// Choosing active High output -int_config.int_pin_settg.edge_ctrl = BMI160_ENABLE;// Choosing edge triggered output -int_config.int_pin_settg.input_en = BMI160_DISABLE;// Disabling interrupt pin to act as input -int_config.int_pin_settg.latch_dur =BMI160_LATCH_DUR_NONE;// non-latched output - -/* Select the Step Detector interrupt parameters, Kindly use the recommended settings for step detector */ -int_config.int_type_cfg.acc_step_detect_int.step_detector_mode = BMI160_STEP_DETECT_NORMAL; -int_config.int_type_cfg.acc_step_detect_int.step_detector_en = BMI160_ENABLE;// 1-enable, 0-disable the step detector - -/* Set the Step Detector interrupt */ -bmi160_set_int_config(&int_config, &sensor); /* sensor is an instance of the structure bmi160_dev */ - -``` - -### Configuring Step counter -To configure the step counter, user need to configure the step detector interrupt as described in above section. -After configuring step detector, see the below code snippet for user space & ISR - -### User space -``` c -int8_t rslt = BMI160_OK; -uint8_t step_enable = 1;//enable the step counter - -rslt = bmi160_set_step_counter(step_enable, &sensor); -``` - -### ISR -``` c -int8_t rslt = BMI160_OK; -uint16_t step_count = 0;//stores the step counter value - -rslt = bmi160_read_step_counter(&step_count, &sensor); -``` - -### Unmapping Interrupt -#### Example for unmapping Step Detector Interrupt -``` c -struct bmi160_int_settg int_config; - -/* Deselect the Interrupt channel/pin */ -int_config.int_channel = BMI160_INT_CHANNEL_NONE; -/* Select the Interrupt type */ -int_config.int_type = BMI160_STEP_DETECT_INT;// Choosing Step Detector interrupt -/* Set the Step Detector interrupt */ -bmi160_set_int_config(&int_config, &sensor); /* sensor is an instance of the structure bmi160_dev */ -``` - -### Reading interrupt status -#### Example for reading interrupt status for step detector -``` c -union bmi160_int_status interrupt; -enum bmi160_int_status_sel int_status_sel; - -/* Interrupt status selection to read all interrupts */ -int_status_sel = BMI160_INT_STATUS_ALL; -rslt = bmi160_get_int_status(int_status_sel, &interrupt, &sensor); -if (interrupt.bit.step) - printf("Step detector interrupt occured\n"); -``` - -### Configuring the auxiliary sensor BMM150 -It is assumed that secondary interface of bmi160 has external pull-up resistor in order to access the auxiliary sensor bmm150. - -### Accessing auxiliary BMM150 with BMM150 APIs via BMI160 secondary interface. - -## Integration details -* Integrate the souce codes of BMM150 and BMI160 in project. -* Include the bmi160.h and bmm150.h file in your code like below. -* It is mandatory to initialize the bmi160 device structure for primary interface and auxiliary sensor settings. -* Create two wrapper functions , user_aux_read and user_aux_write in order to match the signature as mentioned below. -* Invoke the "bmi160_aux_init" API to initialise the secondary interface in BMI160. -* Invoke the "bmm150_init" API to initialise the BMM150 sensor. -* Now we can use the BMM150 sensor APIs to access the BMM150 via BMI160. - -``` c -/* main.c file */ -#include "bmi160.h" -#include "bmm150.h" -``` -### Initialization of auxiliary sensor BMM150 -``` - -/* main.c file */ -struct bmm150_dev bmm150; - -/* function declaration */ -int8_t user_aux_read(uint8_t id, uint8_t reg_addr, uint8_t *aux_data, uint16_t len); -int8_t user_aux_write(uint8_t id, uint8_t reg_addr, uint8_t *aux_data, uint16_t len); - -/* Configure device structure for auxiliary sensor parameter */ -sensor.aux_cfg.aux_sensor_enable = 1; // auxiliary sensor enable -sensor.aux_cfg.aux_i2c_addr = BMI160_AUX_BMM150_I2C_ADDR; // auxiliary sensor address -sensor.aux_cfg.manual_enable = 1; // setup mode enable -sensor.aux_cfg.aux_rd_burst_len = 2;// burst read of 2 byte - -/* Configure the BMM150 device structure by -mapping user_aux_read and user_aux_write */ -bmm150.read = user_aux_read; -bmm150.write = user_aux_write; -bmm150.id = BMM150_DEFAULT_I2C_ADDRESS; -/* Ensure that sensor.aux_cfg.aux_i2c_addr = bmm150.id - for proper sensor operation */ -bmm150.delay_ms = delay_ms; -bmm150.interface = BMM150_I2C_INTF; - -/* Initialize the auxiliary sensor interface */ -rslt = bmi160_aux_init(&sensor); - -/* Auxiliary sensor is enabled and can be accessed from this point */ - -/* Configure the desired settings in auxiliary BMM150 sensor - * using the bmm150 APIs */ - -/* Initialising the bmm150 sensor */ -rslt = bmm150_init(&bmm150); - -/* Set the power mode and preset mode to enable Mag data sampling */ -bmm150.settings.pwr_mode = BMM150_NORMAL_MODE; -rslt = bmm150_set_op_mode(&bmm150); - -bmm150.settings.preset_mode= BMM150_PRESETMODE_LOWPOWER; -rslt = bmm150_set_presetmode(&bmm150); - -``` -### Wrapper functions -``` - -/*wrapper function to match the signature of bmm150.read */ -int8_t user_aux_read(uint8_t id, uint8_t reg_addr, uint8_t *aux_data, uint16_t len) -{ - int8_t rslt; - - /* Discarding the parameter id as it is redundant*/ - rslt = bmi160_aux_read(reg_addr, aux_data, len, &bmi160); - - return rslt; -} - -/*wrapper function to match the signature of bmm150.write */ -int8_t user_aux_write(uint8_t id, uint8_t reg_addr, uint8_t *aux_data, uint16_t len) -{ - int8_t rslt; - - /* Discarding the parameter id as it is redundant */ - rslt = bmi160_aux_write(reg_addr, aux_data, len, &bmi160); - - return rslt; -} - -``` - -### Initialization of auxiliary BMM150 in auto mode -Any sensor whose data bytes are less than or equal to 8 bytes can be synchronized with the BMI160 -and read out of Accelerometer + Gyroscope + Auxiliary sensor data of that instance is possible -which helps in creating less latency fusion data - -``` -/* Initialize the Auxiliary BMM150 following the above code - * until setting the power mode (Set the power mode as forced mode) - * and preset mode */ - - /* In BMM150 Mag data starts from register address 0x42 */ - uint8_t aux_addr = 0x42; - /* Buffer to store the Mag data from 0x42 to 0x48 */ - uint8_t mag_data[8] = {0}; - - uint8_t index; - - /* Configure the Auxiliary sensor either in auto/manual modes and set the - polling frequency for the Auxiliary interface */ - sensor.aux_cfg.aux_odr = 8; /* Represents polling rate in 100 Hz*/ - rslt = bmi160_config_aux_mode(&sensor) - - /* Set the auxiliary sensor to auto mode */ - rslt = bmi160_set_aux_auto_mode(&aux_addr, &sensor); - - /* Reading data from BMI160 data registers */ - rslt = bmi160_read_aux_data_auto_mode(mag_data, &sensor); - - printf("\n RAW DATA "); - for(index = 0 ; index < 8 ; index++) - { - printf("\n MAG DATA[%d] : %d ", index, mag_data[index]); - } - - /* Compensating the raw mag data available from the BMM150 API */ - rslt = bmm150_aux_mag_data(mag_data, &bmm150); - - printf("\n COMPENSATED DATA "); - printf("\n MAG DATA X : %d Y : %d Z : %d", bmm150.data.x, bmm150.data.y, bmm150.data.z); - - -``` - -### Auxiliary FIFO data parsing -The Auxiliary sensor data can be stored in FIFO , Here we demonstrate an example for -using the Bosch Magnetometer sensor BMM150 and storing its data in FIFO - -``` -/* Initialize the Aux BMM150 following the above - * code and by creating the Wrapper functions */ - - int8_t rslt = 0; - uint8_t aux_instance = 0; - uint16_t fifo_cnt = 0; - uint8_t auto_mode_addr; - uint8_t i; - - /* Setup and configure the FIFO buffer */ - /* Declare memory to store the raw FIFO buffer information */ - uint8_t fifo_buff[1000] = {0}; - - /* Modify the FIFO buffer instance and link to the device instance */ - struct bmi160_fifo_frame fifo_frame; - fifo_frame.data = fifo_buff; - fifo_frame.length = 1000; - dev->fifo = &fifo_frame; - - /* Declare instances of the sensor data structure to store the parsed FIFO data */ - struct bmi160_aux_data aux_data[112]; //1000 / 9 bytes per frame ~ 111 data frames - - rslt = bmi160_init(dev); - printf("\n BMI160 chip ID is : %d ",dev->chip_id); - - rslt = bmi160_aux_init(dev); - - rslt = bmm150_init(&bmm150); - printf("\n BMM150 CHIP ID : %d",bmm150.chip_id); - - bmm150.settings.preset_mode = BMM150_PRESETMODE_LOWPOWER; - rslt = bmm150_set_presetmode(&bmm150); - - bmm150.settings.pwr_mode = BMM150_FORCED_MODE; - rslt = bmm150_set_op_mode(&bmm150); - - /* Enter the data register of BMM150 to "auto_mode_addr" here it is 0x42 */ - auto_mode_addr = 0x42; - printf("\n ENTERING AUX. AUTO MODE "); - dev->aux_cfg.aux_odr = BMI160_AUX_ODR_25HZ; - rslt = bmi160_set_aux_auto_mode(&auto_mode_addr, dev); - - - /* Disable other FIFO settings */ - rslt = bmi160_set_fifo_config(BMI160_FIFO_CONFIG_1_MASK , BMI160_DISABLE, dev); - - /* Enable the required FIFO settings */ - rslt = bmi160_set_fifo_config(BMI160_FIFO_AUX | BMI160_FIFO_HEADER, BMI160_ENABLE, dev); - - /* Delay for the FIFO to get filled */ - dev->delay_ms(400); - - - printf("\n FIFO DATA REQUESTED (in bytes): %d",dev->fifo->length); - rslt = bmi160_get_fifo_data(dev); - printf("\n FIFO DATA AVAILABLE (in bytes): %d",dev->fifo->length); - - /* Print the raw FIFO data obtained */ - for(fifo_cnt = 0; fifo_cnt < dev->fifo->length ; fifo_cnt++) { - printf("\n FIFO DATA [%d] IS : %x ",fifo_cnt ,dev->fifo->data[fifo_cnt]); - } - - printf("\n\n----------------------------------------------------\n"); - - /* Set the number of required sensor data instances */ - aux_instance = 150; - - /* Extract the aux data , 1frame = 8 data bytes */ - printf("\n AUX DATA REQUESTED TO BE EXTRACTED (in frames): %d",aux_instance); - rslt = bmi160_extract_aux(aux_data, &aux_instance, dev); - printf("\n AUX DATA ACTUALLY EXTRACTED (in frames): %d",aux_instance); - - /* Printing the raw aux data */ - for (i = 0; i < aux_instance; i++) { - printf("\n Aux data[%d] : %x",i,aux_data[i].data[0]); - printf("\n Aux data[%d] : %x",i,aux_data[i].data[1]); - printf("\n Aux data[%d] : %x",i,aux_data[i].data[2]); - printf("\n Aux data[%d] : %x",i,aux_data[i].data[3]); - printf("\n Aux data[%d] : %x",i,aux_data[i].data[4]); - printf("\n Aux data[%d] : %x",i,aux_data[i].data[5]); - printf("\n Aux data[%d] : %x",i,aux_data[i].data[6]); - printf("\n Aux data[%d] : %x",i,aux_data[i].data[7]); - } - - printf("\n\n----------------------------------------------------\n"); - - /* Compensate the raw mag data using BMM150 API */ - for (i = 0; i < aux_instance; i++) { - printf("\n----------------------------------------------------"); - printf("\n Aux data[%d] : %x , %x , %x , %x , %x , %x , %x , %x",i - ,aux_data[i].data[0],aux_data[i].data[1] - ,aux_data[i].data[2],aux_data[i].data[3] - ,aux_data[i].data[4],aux_data[i].data[5] - ,aux_data[i].data[6],aux_data[i].data[7]); - - /* Compensated mag data using BMM150 API */ - rslt = bmm150_aux_mag_data(&aux_data[i].data[0], &bmm150); - - /* Printing the Compensated mag data */ - if (rslt == BMM150_OK) { - printf("\n MAG DATA COMPENSATION USING BMM150 APIs"); - printf("\n COMPENSATED DATA "); - printf("\n MAG DATA X : %d Y : %d Z : %d" - , bmm150.data.x, bmm150.data.y, bmm150.data.z); - - } else { - printf("\n MAG DATA COMPENSATION IN BMM150 API is FAILED "); - } - printf("\n----------------------------------------------------\n"); - } - -``` - -## Self-test -#### Example for performing accel self test -``` -/* Call the "bmi160_init" API as a prerequisite before performing self test - * since invoking self-test will reset the sensor */ - - rslt = bmi160_perform_self_test(BMI160_ACCEL_ONLY, sen); - /* Utilize the enum BMI160_GYRO_ONLY instead of BMI160_ACCEL_ONLY - to perform self test for gyro */ - if (rslt == BMI160_OK) { - printf("\n ACCEL SELF TEST RESULT SUCCESS); - } else { - printf("\n ACCEL SELF TEST RESULT FAIL); - } -``` - - -## FIFO -#### Example for reading FIFO and extracting Gyro data in Header mode -``` -/* An example to read the Gyro data in header mode along with sensor time (if available) - * Configure the gyro sensor as prerequisite and follow the below example to read and - * obtain the gyro data from FIFO */ -int8_t fifo_gyro_header_time_data(struct bmi160_dev *dev) -{ - int8_t rslt = 0; - - /* Declare memory to store the raw FIFO buffer information */ - uint8_t fifo_buff[300]; - - /* Modify the FIFO buffer instance and link to the device instance */ - struct bmi160_fifo_frame fifo_frame; - fifo_frame.data = fifo_buff; - fifo_frame.length = 300; - dev->fifo = &fifo_frame; - uint16_t index = 0; - - /* Declare instances of the sensor data structure to store the parsed FIFO data */ - struct bmi160_sensor_data gyro_data[42]; // 300 bytes / ~7bytes per frame ~ 42 data frames - uint8_t gyro_frames_req = 42; - uint8_t gyro_index; - - /* Configure the sensor's FIFO settings */ - rslt = bmi160_set_fifo_config(BMI160_FIFO_GYRO | BMI160_FIFO_HEADER | BMI160_FIFO_TIME, - BMI160_ENABLE, dev); - - if (rslt == BMI160_OK) { - /* At ODR of 100 Hz ,1 frame gets updated in 1/100 = 0.01s - i.e. for 42 frames we need 42 * 0.01 = 0.42s = 420ms delay */ - dev->delay_ms(420); - - /* Read data from the sensor's FIFO and store it the FIFO buffer,"fifo_buff" */ - printf("\n USER REQUESTED FIFO LENGTH : %d\n",dev->fifo->length); - rslt = bmi160_get_fifo_data(dev); - - if (rslt == BMI160_OK) { - printf("\n AVAILABLE FIFO LENGTH : %d\n",dev->fifo->length); - /* Print the raw FIFO data */ - for (index = 0; index < dev->fifo->length; index++) { - printf("\n FIFO DATA INDEX[%d] = %d", index, - dev->fifo->data[index]); - } - /* Parse the FIFO data to extract gyro data from the FIFO buffer */ - printf("\n REQUESTED GYRO DATA FRAMES : %d\n ",gyro_frames_req); - rslt = bmi160_extract_gyro(gyro_data, &gyro_frames_req, dev); - - if (rslt == BMI160_OK) { - printf("\n AVAILABLE GYRO DATA FRAMES : %d\n ",gyro_frames_req); - - /* Print the parsed gyro data from the FIFO buffer */ - for (gyro_index = 0; gyro_index < gyro_frames_req; gyro_index++) { - printf("\nFIFO GYRO FRAME[%d]",gyro_index); - printf("\nGYRO X-DATA : %d \t Y-DATA : %d \t Z-DATA : %d" - ,gyro_data[gyro_index].x ,gyro_data[gyro_index].y - ,gyro_data[gyro_index].z); - } - /* Print the special FIFO frame data like sensortime */ - printf("\n SENSOR TIME DATA : %d \n",dev->fifo->sensor_time); - printf("SKIPPED FRAME COUNT : %d",dev->fifo->skipped_frame_count); - } else { - printf("\n Gyro data extraction failed"); - } - } else { - printf("\n Reading FIFO data failed"); - } - } else { - printf("\n Setting FIFO configuration failed"); - } - - return rslt; -} -``` - -## FOC and offset compensation -> FOC shouldnot be used in Low-power mode -#### Example for configuring FOC for accel and gyro -``` -/* An example for configuring FOC for accel and gyro data */ -int8_t start_foc(struct bmi160_dev *dev) -{ - int8_t rslt = 0; - /* FOC configuration structure */ - struct bmi160_foc_conf foc_conf; - /* Structure to store the offsets */ - struct bmi160_offsets offsets; - - /* Enable FOC for accel with target values of z = 1g ; x,y as 0g */ - foc_conf.acc_off_en = BMI160_ENABLE; - foc_conf.foc_acc_x = BMI160_FOC_ACCEL_0G; - foc_conf.foc_acc_y = BMI160_FOC_ACCEL_0G; - foc_conf.foc_acc_z = BMI160_FOC_ACCEL_POSITIVE_G; - - /* Enable FOC for gyro */ - foc_conf.foc_gyr_en = BMI160_ENABLE; - foc_conf.gyro_off_en = BMI160_ENABLE; - - rslt = bmi160_start_foc(&foc_conf, &offsets, sen); - - if (rslt == BMI160_OK) { - printf("\n FOC DONE SUCCESSFULLY "); - printf("\n OFFSET VALUES AFTER FOC : "); - printf("\n OFFSET VALUES ACCEL X : %d ",offsets.off_acc_x); - printf("\n OFFSET VALUES ACCEL Y : %d ",offsets.off_acc_y); - printf("\n OFFSET VALUES ACCEL Z : %d ",offsets.off_acc_z); - printf("\n OFFSET VALUES GYRO X : %d ",offsets.off_gyro_x); - printf("\n OFFSET VALUES GYRO Y : %d ",offsets.off_gyro_y); - printf("\n OFFSET VALUES GYRO Z : %d ",offsets.off_gyro_z); - } - - /* After start of FOC offsets will be updated automatically and - * the data will be very much close to the target values of measurement */ - - return rslt; -} -``` - -#### Example for updating the offsets manually -> The offsets set by this method will be reset on soft-reset/POR -``` -/* An example for updating manual offsets to sensor */ -int8_t write_offsets(struct bmi160_dev *dev) -{ - int8_t rslt = 0; - /* FOC configuration structure */ - struct bmi160_foc_conf foc_conf; - /* Structure to store the offsets */ - struct bmi160_offsets offsets; - - /* Enable offset update for accel */ - foc_conf.acc_off_en = BMI160_ENABLE; - - /* Enable offset update for gyro */ - foc_conf.gyro_off_en = BMI160_ENABLE; - - /* offset values set by user */ - offsets.off_acc_x = 0x10; - offsets.off_acc_y = 0x10; - offsets.off_acc_z = 0x10; - offsets.off_gyro_x = 0x10; - offsets.off_gyro_y = 0x10; - offsets.off_gyro_z = 0x10; - - rslt = bmi160_set_offsets(&foc_conf, &offsets, sen); - - /* After offset setting the data read from the - * sensor will have the corresponding offset */ - - return rslt; -} -``` - -#### Example for updating the offsets into NVM -> The offsets set by this method will be present in NVM and will be -> restored on POR/soft-reset -``` -/* An example for updating manual offsets to sensor */ -int8_t write_offsets_nvm(struct bmi160_dev *dev) -{ - int8_t rslt = 0; - /* FOC configuration structure */ - struct bmi160_foc_conf foc_conf; - /* Structure to store the offsets */ - struct bmi160_offsets offsets; - - /* Enable offset update for accel */ - foc_conf.acc_off_en = BMI160_ENABLE; - - /* Enable offset update for gyro */ - foc_conf.gyro_off_en = BMI160_ENABLE; - - /* offset values set by user as per their reference - * Resolution of accel = 3.9mg/LSB - * Resolution of gyro = (0.061degrees/second)/LSB */ - offsets.off_acc_x = 10; - offsets.off_acc_y = -15; - offsets.off_acc_z = 20; - offsets.off_gyro_x = 30; - offsets.off_gyro_y = -35; - offsets.off_gyro_z = -40; - - rslt = bmi160_set_offsets(&foc_conf, &offsets, sen); - - if (rslt == BMI160_OK) { - /* Update the NVM */ - rslt = bmi160_update_nvm(dev); - } - - /* After this procedure the offsets are written to - * NVM and restored on POR/soft-reset - * The set values can be removed to ideal case by - * invoking the following APIs - * - bmi160_start_foc() - * - bmi160_update_nvm() - */ - - return rslt; -} -``` - - - -## Copyright (C) 2019 Bosch Sensortec GmbH \ No newline at end of file +# BMI160 sensor API +## Introduction +This package contains the Bosch Sensortec's BMI160 sensor driver (sensor API) + +The sensor driver package includes bmi160.h, bmi160.c and bmi160_defs.h files + +## Version +File | Version | Date +--------------|---------|--------------- +bmi160.c | 3.7.5 | 11 Jan 2018 +bmi160.h | 3.7.5 | 11 Jan 2018 +bmi160_defs.h | 3.7.5 | 11 Jan 2018 + +## Integration details +* Integrate bmi160.h, bmi160_defs.h and bmi160.c file in to your project. +* Include the bmi160.h file in your code like below. +``` c +#include "bmi160.h" +``` + +## File information +* bmi160_defs.h : This header file has the constants, macros and datatype declarations. +* bmi160.h : This header file contains the declarations of the sensor driver APIs. +* bmi160.c : This source file contains the definitions of the sensor driver APIs. + +## Supported sensor interface +* SPI 4-wire +* I2C + +## Usage guide +### Initializing the sensor +To initialize the sensor, you will first need to create a device structure. You +can do this by creating an instance of the structure bmi160_dev. Then go on to +fill in the various parameters as shown below. + +#### Example for SPI 4-Wire +``` c +struct bmi160_dev sensor; + +/* You may assign a chip select identifier to be handled later */ +sensor.id = 0; +sensor.interface = BMI160_SPI_INTF; +sensor.read = user_spi_read; +sensor.write = user_spi_write; +sensor.delay_ms = user_delay_ms; + + +int8_t rslt = BMI160_OK; +rslt = bmi160_init(&sensor); +/* After the above function call, accel_cfg and gyro_cfg parameters in the device +structure are set with default values, found in the datasheet of the sensor */ +``` + +#### Example for I2C +``` c +struct bmi160_dev sensor; + +sensor.id = BMI160_I2C_ADDR; +sensor.interface = BMI160_I2C_INTF; +sensor.read = user_i2c_read; +sensor.write = user_i2c_write; +sensor.delay_ms = user_delay_ms; + +int8_t rslt = BMI160_OK; +rslt = bmi160_init(&sensor); +/* After the above function call, accel and gyro parameters in the device structure +are set with default values, found in the datasheet of the sensor */ +``` + +### Configuring accel and gyro sensor +#### Example for configuring accel and gyro sensors in normal mode +``` c + +int8_t rslt = BMI160_OK; + +/* Select the Output data rate, range of accelerometer sensor */ +sensor.accel_cfg.odr = BMI160_ACCEL_ODR_1600HZ; +sensor.accel_cfg.range = BMI160_ACCEL_RANGE_2G; +sensor.accel_cfg.bw = BMI160_ACCEL_BW_NORMAL_AVG4; + +/* Select the power mode of accelerometer sensor */ +sensor.accel_cfg.power = BMI160_ACCEL_NORMAL_MODE; + +/* Select the Output data rate, range of Gyroscope sensor */ +sensor.gyro_cfg.odr = BMI160_GYRO_ODR_3200HZ; +sensor.gyro_cfg.range = BMI160_GYRO_RANGE_2000_DPS; +sensor.gyro_cfg.bw = BMI160_GYRO_BW_NORMAL_MODE; + +/* Select the power mode of Gyroscope sensor */ +sensor.gyro_cfg.power = BMI160_GYRO_NORMAL_MODE; + +/* Set the sensor configuration */ +rslt = bmi160_set_sens_conf(&sensor); +``` + +### Reading sensor data +#### Example for reading sensor data +``` c + +int8_t rslt = BMI160_OK; +struct bmi160_sensor_data accel; +struct bmi160_sensor_data gyro; + +/* To read only Accel data */ +rslt = bmi160_get_sensor_data(BMI160_ACCEL_SEL, &accel, NULL, &sensor); + +/* To read only Gyro data */ +rslt = bmi160_get_sensor_data(BMI160_GYRO_SEL, NULL, &gyro, &sensor); + +/* To read both Accel and Gyro data */ +bmi160_get_sensor_data((BMI160_ACCEL_SEL | BMI160_GYRO_SEL), &accel, &gyro, &sensor); + +/* To read Accel data along with time */ +rslt = bmi160_get_sensor_data((BMI160_ACCEL_SEL | BMI160_TIME_SEL) , &accel, NULL, &sensor); + +/* To read Gyro data along with time */ +rslt = bmi160_get_sensor_data((BMI160_GYRO_SEL | BMI160_TIME_SEL), NULL, &gyro, &sensor); + +/* To read both Accel and Gyro data along with time*/ +bmi160_get_sensor_data((BMI160_ACCEL_SEL | BMI160_GYRO_SEL | BMI160_TIME_SEL), &accel, &gyro, &sensor); +``` + +### Setting the power mode of sensors +#### Example for setting power mode of accel and gyro +``` c + +int8_t rslt = BMI160_OK; + +/* Select the power mode */ +sensor.accel_cfg.power = BMI160_ACCEL_SUSPEND_MODE; +sensor.gyro_cfg.power = BMI160_GYRO_FASTSTARTUP_MODE; + +/* Set the Power mode */ +rslt = bmi160_set_power_mode(&sensor); + +/* Select the power mode */ +sensor.accel_cfg.power = BMI160_ACCEL_NORMAL_MODE; +sensor.gyro_cfg.power = BMI160_GYRO_NORMAL_MODE; + +/* Set the Power mode */ +rslt = bmi160_set_power_mode(&sensor); + +``` + +### Reading sensor data register +#### Example for reading Chip Address +``` c + +int8_t rslt = BMI160_OK; +uint8_t reg_addr = BMI160_CHIP_ID_ADDR; +uint8_t data; +uint16_t len = 1; +rslt = bmi160_get_regs(reg_addr, &data, len, &sensor); +``` + + +### Writing to sensor data register +#### Example for writing data to any motion threshold register +``` c + +int8_t rslt = BMI160_OK; +uint8_t reg_addr = BMI160_INT_MOTION_1_ADDR; +uint8_t data = 20; +uint16_t len = 1; +rslt = bmi160_set_regs(reg_addr, &data, len, &sensor); +``` + +### Resetting the device using soft-reset +#### Example for writing soft-reset command to command register +``` c + +int8_t rslt = BMI160_OK; +rslt = bmi160_soft_reset(&sensor); +``` + + +### Configuring interrupts for sensors +To configure the sensor interrupts, you will first need to create an interrupt +structure. You can do this by creating an instance of the structure bmi160_int_settg. +Then go on to fill in the various parameters as shown below + + +### Configuring Any-motion Interrupt +#### Example for configuring Any-motion Interrupt +Note:- User can check the currently active interrupt(any-motion or sig-motion) by checking the **any_sig_sel** of bmi160_dev structure. +``` c + +struct bmi160_int_settg int_config; + +/* Select the Interrupt channel/pin */ +int_config.int_channel = BMI160_INT_CHANNEL_1;// Interrupt channel/pin 1 + +/* Select the Interrupt type */ +int_config.int_type = BMI160_ACC_ANY_MOTION_INT;// Choosing Any motion interrupt +/* Select the interrupt channel/pin settings */ +int_config.int_pin_settg.output_en = BMI160_ENABLE;// Enabling interrupt pins to act as output pin +int_config.int_pin_settg.output_mode = BMI160_DISABLE;// Choosing push-pull mode for interrupt pin +int_config.int_pin_settg.output_type = BMI160_DISABLE;// Choosing active low output +int_config.int_pin_settg.edge_ctrl = BMI160_ENABLE;// Choosing edge triggered output +int_config.int_pin_settg.input_en = BMI160_DISABLE;// Disabling interrupt pin to act as input +int_config.int_pin_settg.latch_dur = BMI160_LATCH_DUR_NONE;// non-latched output + +/* Select the Any-motion interrupt parameters */ +int_config.int_type_cfg.acc_any_motion_int.anymotion_en = BMI160_ENABLE;// 1- Enable the any-motion, 0- disable any-motion +int_config.int_type_cfg.acc_any_motion_int.anymotion_x = BMI160_ENABLE;// Enabling x-axis for any motion interrupt +int_config.int_type_cfg.acc_any_motion_int.anymotion_y = BMI160_ENABLE;// Enabling y-axis for any motion interrupt +int_config.int_type_cfg.acc_any_motion_int.anymotion_z = BMI160_ENABLE;// Enabling z-axis for any motion interrupt +int_config.int_type_cfg.acc_any_motion_int.anymotion_dur = 0;// any-motion duration +int_config.int_type_cfg.acc_any_motion_int.anymotion_thr = 20;// (2-g range) -> (slope_thr) * 3.91 mg, (4-g range) -> (slope_thr) * 7.81 mg, (8-g range) ->(slope_thr) * 15.63 mg, (16-g range) -> (slope_thr) * 31.25 mg + +/* Set the Any-motion interrupt */ +bmi160_set_int_config(&int_config, &sensor); /* sensor is an instance of the structure bmi160_dev */ + +``` +### Configuring Flat Interrupt +#### Example for configuring Flat Interrupt +``` c + +struct bmi160_int_settg int_config; + +/* Select the Interrupt channel/pin */ +int_config.int_channel = BMI160_INT_CHANNEL_1;// Interrupt channel/pin 1 + +/* Select the Interrupt type */ +int_config.int_type = BMI160_ACC_FLAT_INT;// Choosing flat interrupt +/* Select the interrupt channel/pin settings */ +int_config.int_pin_settg.output_en = BMI160_ENABLE;// Enabling interrupt pins to act as output pin +int_config.int_pin_settg.output_mode = BMI160_DISABLE;// Choosing push-pull mode for interrupt pin +int_config.int_pin_settg.output_type = BMI160_DISABLE;// Choosing active low output +int_config.int_pin_settg.edge_ctrl = BMI160_ENABLE;// Choosing edge triggered output +int_config.int_pin_settg.input_en = BMI160_DISABLE;// Disabling interrupt pin to act as input +int_config.int_pin_settg.latch_dur = BMI160_LATCH_DUR_NONE;// non-latched output + +/* Select the Flat interrupt parameters */ +int_config.int_type_cfg.acc_flat_int.flat_en = BMI160_ENABLE;// 1-enable, 0-disable the flat interrupt +int_config.int_type_cfg.acc_flat_int.flat_theta = 8;// threshold for detection of flat position in range from 0° to 44.8°. +int_config.int_type_cfg.acc_flat_int.flat_hy = 1;// Flat hysteresis +int_config.int_type_cfg.acc_flat_int.flat_hold_time = 1;// Flat hold time (0 -> 0 ms, 1 -> 640 ms, 2 -> 1280 ms, 3 -> 2560 ms) + +/* Set the Flat interrupt */ +bmi160_set_int_config(&int_config, &sensor); /* sensor is an instance of the structure bmi160_dev */ + +``` + + +### Configuring Step Detector Interrupt +#### Example for configuring Step Detector Interrupt +``` c + +struct bmi160_int_settg int_config; + +/* Select the Interrupt channel/pin */ +int_config.int_channel = BMI160_INT_CHANNEL_1;// Interrupt channel/pin 1 + +/* Select the Interrupt type */ +int_config.int_type = BMI160_STEP_DETECT_INT;// Choosing Step Detector interrupt +/* Select the interrupt channel/pin settings */ +int_config.int_pin_settg.output_en = BMI160_ENABLE;// Enabling interrupt pins to act as output pin +int_config.int_pin_settg.output_mode = BMI160_DISABLE;// Choosing push-pull mode for interrupt pin +int_config.int_pin_settg.output_type = BMI160_ENABLE;// Choosing active High output +int_config.int_pin_settg.edge_ctrl = BMI160_ENABLE;// Choosing edge triggered output +int_config.int_pin_settg.input_en = BMI160_DISABLE;// Disabling interrupt pin to act as input +int_config.int_pin_settg.latch_dur =BMI160_LATCH_DUR_NONE;// non-latched output + +/* Select the Step Detector interrupt parameters, Kindly use the recommended settings for step detector */ +int_config.int_type_cfg.acc_step_detect_int.step_detector_mode = BMI160_STEP_DETECT_NORMAL; +int_config.int_type_cfg.acc_step_detect_int.step_detector_en = BMI160_ENABLE;// 1-enable, 0-disable the step detector + +/* Set the Step Detector interrupt */ +bmi160_set_int_config(&int_config, &sensor); /* sensor is an instance of the structure bmi160_dev */ + +``` + +### Configuring Step counter +To configure the step counter, user need to configure the step detector interrupt as described in above section. +After configuring step detector, see the below code snippet for user space & ISR + +### User space +``` c +int8_t rslt = BMI160_OK; +uint8_t step_enable = 1;//enable the step counter + +rslt = bmi160_set_step_counter(step_enable, &sensor); +``` + +### ISR +``` c +int8_t rslt = BMI160_OK; +uint16_t step_count = 0;//stores the step counter value + +rslt = bmi160_read_step_counter(&step_count, &sensor); +``` + +### Unmapping Interrupt +#### Example for unmapping Step Detector Interrupt +``` c +struct bmi160_int_settg int_config; + +/* Deselect the Interrupt channel/pin */ +int_config.int_channel = BMI160_INT_CHANNEL_NONE; +/* Select the Interrupt type */ +int_config.int_type = BMI160_STEP_DETECT_INT;// Choosing Step Detector interrupt +/* Set the Step Detector interrupt */ +bmi160_set_int_config(&int_config, &sensor); /* sensor is an instance of the structure bmi160_dev */ +``` + +### Reading interrupt status +#### Example for reading interrupt status for step detector +``` c +union bmi160_int_status interrupt; +enum bmi160_int_status_sel int_status_sel; + +/* Interrupt status selection to read all interrupts */ +int_status_sel = BMI160_INT_STATUS_ALL; +rslt = bmi160_get_int_status(int_status_sel, &interrupt, &sensor); +if (interrupt.bit.step) + printf("Step detector interrupt occured\n"); +``` + +### Configuring the auxiliary sensor BMM150 +It is assumed that secondary interface of bmi160 has external pull-up resistor in order to access the auxiliary sensor bmm150. + +### Accessing auxiliary BMM150 with BMM150 APIs via BMI160 secondary interface. + +## Integration details +* Integrate the souce codes of BMM150 and BMI160 in project. +* Include the bmi160.h and bmm150.h file in your code like below. +* It is mandatory to initialize the bmi160 device structure for primary interface and auxiliary sensor settings. +* Create two wrapper functions , user_aux_read and user_aux_write in order to match the signature as mentioned below. +* Invoke the "bmi160_aux_init" API to initialise the secondary interface in BMI160. +* Invoke the "bmm150_init" API to initialise the BMM150 sensor. +* Now we can use the BMM150 sensor APIs to access the BMM150 via BMI160. + +``` c +/* main.c file */ +#include "bmi160.h" +#include "bmm150.h" +``` +### Initialization of auxiliary sensor BMM150 +``` + +/* main.c file */ +struct bmm150_dev bmm150; + +/* function declaration */ +int8_t user_aux_read(uint8_t id, uint8_t reg_addr, uint8_t *aux_data, uint16_t len); +int8_t user_aux_write(uint8_t id, uint8_t reg_addr, uint8_t *aux_data, uint16_t len); + +/* Configure device structure for auxiliary sensor parameter */ +sensor.aux_cfg.aux_sensor_enable = 1; // auxiliary sensor enable +sensor.aux_cfg.aux_i2c_addr = BMI160_AUX_BMM150_I2C_ADDR; // auxiliary sensor address +sensor.aux_cfg.manual_enable = 1; // setup mode enable +sensor.aux_cfg.aux_rd_burst_len = 2;// burst read of 2 byte + +/* Configure the BMM150 device structure by +mapping user_aux_read and user_aux_write */ +bmm150.read = user_aux_read; +bmm150.write = user_aux_write; +bmm150.id = BMM150_DEFAULT_I2C_ADDRESS; +/* Ensure that sensor.aux_cfg.aux_i2c_addr = bmm150.id + for proper sensor operation */ +bmm150.delay_ms = delay_ms; +bmm150.interface = BMM150_I2C_INTF; + +/* Initialize the auxiliary sensor interface */ +rslt = bmi160_aux_init(&sensor); + +/* Auxiliary sensor is enabled and can be accessed from this point */ + +/* Configure the desired settings in auxiliary BMM150 sensor + * using the bmm150 APIs */ + +/* Initialising the bmm150 sensor */ +rslt = bmm150_init(&bmm150); + +/* Set the power mode and preset mode to enable Mag data sampling */ +bmm150.settings.pwr_mode = BMM150_NORMAL_MODE; +rslt = bmm150_set_op_mode(&bmm150); + +bmm150.settings.preset_mode= BMM150_PRESETMODE_LOWPOWER; +rslt = bmm150_set_presetmode(&bmm150); + +``` +### Wrapper functions +``` + +/*wrapper function to match the signature of bmm150.read */ +int8_t user_aux_read(uint8_t id, uint8_t reg_addr, uint8_t *aux_data, uint16_t len) +{ + int8_t rslt; + + /* Discarding the parameter id as it is redundant*/ + rslt = bmi160_aux_read(reg_addr, aux_data, len, &bmi160); + + return rslt; +} + +/*wrapper function to match the signature of bmm150.write */ +int8_t user_aux_write(uint8_t id, uint8_t reg_addr, uint8_t *aux_data, uint16_t len) +{ + int8_t rslt; + + /* Discarding the parameter id as it is redundant */ + rslt = bmi160_aux_write(reg_addr, aux_data, len, &bmi160); + + return rslt; +} + +``` + +### Initialization of auxiliary BMM150 in auto mode +Any sensor whose data bytes are less than or equal to 8 bytes can be synchronized with the BMI160 +and read out of Accelerometer + Gyroscope + Auxiliary sensor data of that instance is possible +which helps in creating less latency fusion data + +``` +/* Initialize the Auxiliary BMM150 following the above code + * until setting the power mode (Set the power mode as forced mode) + * and preset mode */ + + /* In BMM150 Mag data starts from register address 0x42 */ + uint8_t aux_addr = 0x42; + /* Buffer to store the Mag data from 0x42 to 0x48 */ + uint8_t mag_data[8] = {0}; + + uint8_t index; + + /* Configure the Auxiliary sensor either in auto/manual modes and set the + polling frequency for the Auxiliary interface */ + sensor.aux_cfg.aux_odr = 8; /* Represents polling rate in 100 Hz*/ + rslt = bmi160_config_aux_mode(&sensor) + + /* Set the auxiliary sensor to auto mode */ + rslt = bmi160_set_aux_auto_mode(&aux_addr, &sensor); + + /* Reading data from BMI160 data registers */ + rslt = bmi160_read_aux_data_auto_mode(mag_data, &sensor); + + printf("\n RAW DATA "); + for(index = 0 ; index < 8 ; index++) + { + printf("\n MAG DATA[%d] : %d ", index, mag_data[index]); + } + + /* Compensating the raw mag data available from the BMM150 API */ + rslt = bmm150_aux_mag_data(mag_data, &bmm150); + + printf("\n COMPENSATED DATA "); + printf("\n MAG DATA X : %d Y : %d Z : %d", bmm150.data.x, bmm150.data.y, bmm150.data.z); + + +``` + +### Auxiliary FIFO data parsing +The Auxiliary sensor data can be stored in FIFO , Here we demonstrate an example for +using the Bosch Magnetometer sensor BMM150 and storing its data in FIFO + +``` +/* Initialize the Aux BMM150 following the above + * code and by creating the Wrapper functions */ + + int8_t rslt = 0; + uint8_t aux_instance = 0; + uint16_t fifo_cnt = 0; + uint8_t auto_mode_addr; + uint8_t i; + + /* Setup and configure the FIFO buffer */ + /* Declare memory to store the raw FIFO buffer information */ + uint8_t fifo_buff[1000] = {0}; + + /* Modify the FIFO buffer instance and link to the device instance */ + struct bmi160_fifo_frame fifo_frame; + fifo_frame.data = fifo_buff; + fifo_frame.length = 1000; + dev->fifo = &fifo_frame; + + /* Declare instances of the sensor data structure to store the parsed FIFO data */ + struct bmi160_aux_data aux_data[112]; //1000 / 9 bytes per frame ~ 111 data frames + + rslt = bmi160_init(dev); + printf("\n BMI160 chip ID is : %d ",dev->chip_id); + + rslt = bmi160_aux_init(dev); + + rslt = bmm150_init(&bmm150); + printf("\n BMM150 CHIP ID : %d",bmm150.chip_id); + + bmm150.settings.preset_mode = BMM150_PRESETMODE_LOWPOWER; + rslt = bmm150_set_presetmode(&bmm150); + + bmm150.settings.pwr_mode = BMM150_FORCED_MODE; + rslt = bmm150_set_op_mode(&bmm150); + + /* Enter the data register of BMM150 to "auto_mode_addr" here it is 0x42 */ + auto_mode_addr = 0x42; + printf("\n ENTERING AUX. AUTO MODE "); + dev->aux_cfg.aux_odr = BMI160_AUX_ODR_25HZ; + rslt = bmi160_set_aux_auto_mode(&auto_mode_addr, dev); + + + /* Disable other FIFO settings */ + rslt = bmi160_set_fifo_config(BMI160_FIFO_CONFIG_1_MASK , BMI160_DISABLE, dev); + + /* Enable the required FIFO settings */ + rslt = bmi160_set_fifo_config(BMI160_FIFO_AUX | BMI160_FIFO_HEADER, BMI160_ENABLE, dev); + + /* Delay for the FIFO to get filled */ + dev->delay_ms(400); + + + printf("\n FIFO DATA REQUESTED (in bytes): %d",dev->fifo->length); + rslt = bmi160_get_fifo_data(dev); + printf("\n FIFO DATA AVAILABLE (in bytes): %d",dev->fifo->length); + + /* Print the raw FIFO data obtained */ + for(fifo_cnt = 0; fifo_cnt < dev->fifo->length ; fifo_cnt++) { + printf("\n FIFO DATA [%d] IS : %x ",fifo_cnt ,dev->fifo->data[fifo_cnt]); + } + + printf("\n\n----------------------------------------------------\n"); + + /* Set the number of required sensor data instances */ + aux_instance = 150; + + /* Extract the aux data , 1frame = 8 data bytes */ + printf("\n AUX DATA REQUESTED TO BE EXTRACTED (in frames): %d",aux_instance); + rslt = bmi160_extract_aux(aux_data, &aux_instance, dev); + printf("\n AUX DATA ACTUALLY EXTRACTED (in frames): %d",aux_instance); + + /* Printing the raw aux data */ + for (i = 0; i < aux_instance; i++) { + printf("\n Aux data[%d] : %x",i,aux_data[i].data[0]); + printf("\n Aux data[%d] : %x",i,aux_data[i].data[1]); + printf("\n Aux data[%d] : %x",i,aux_data[i].data[2]); + printf("\n Aux data[%d] : %x",i,aux_data[i].data[3]); + printf("\n Aux data[%d] : %x",i,aux_data[i].data[4]); + printf("\n Aux data[%d] : %x",i,aux_data[i].data[5]); + printf("\n Aux data[%d] : %x",i,aux_data[i].data[6]); + printf("\n Aux data[%d] : %x",i,aux_data[i].data[7]); + } + + printf("\n\n----------------------------------------------------\n"); + + /* Compensate the raw mag data using BMM150 API */ + for (i = 0; i < aux_instance; i++) { + printf("\n----------------------------------------------------"); + printf("\n Aux data[%d] : %x , %x , %x , %x , %x , %x , %x , %x",i + ,aux_data[i].data[0],aux_data[i].data[1] + ,aux_data[i].data[2],aux_data[i].data[3] + ,aux_data[i].data[4],aux_data[i].data[5] + ,aux_data[i].data[6],aux_data[i].data[7]); + + /* Compensated mag data using BMM150 API */ + rslt = bmm150_aux_mag_data(&aux_data[i].data[0], &bmm150); + + /* Printing the Compensated mag data */ + if (rslt == BMM150_OK) { + printf("\n MAG DATA COMPENSATION USING BMM150 APIs"); + printf("\n COMPENSATED DATA "); + printf("\n MAG DATA X : %d Y : %d Z : %d" + , bmm150.data.x, bmm150.data.y, bmm150.data.z); + + } else { + printf("\n MAG DATA COMPENSATION IN BMM150 API is FAILED "); + } + printf("\n----------------------------------------------------\n"); + } + +``` + +## Self-test +#### Example for performing accel self test +``` +/* Call the "bmi160_init" API as a prerequisite before performing self test + * since invoking self-test will reset the sensor */ + + rslt = bmi160_perform_self_test(BMI160_ACCEL_ONLY, sen); + /* Utilize the enum BMI160_GYRO_ONLY instead of BMI160_ACCEL_ONLY + to perform self test for gyro */ + if (rslt == BMI160_OK) { + printf("\n ACCEL SELF TEST RESULT SUCCESS); + } else { + printf("\n ACCEL SELF TEST RESULT FAIL); + } +``` + + +## FIFO +#### Example for reading FIFO and extracting Gyro data in Header mode +``` +/* An example to read the Gyro data in header mode along with sensor time (if available) + * Configure the gyro sensor as prerequisite and follow the below example to read and + * obtain the gyro data from FIFO */ +int8_t fifo_gyro_header_time_data(struct bmi160_dev *dev) +{ + int8_t rslt = 0; + + /* Declare memory to store the raw FIFO buffer information */ + uint8_t fifo_buff[300]; + + /* Modify the FIFO buffer instance and link to the device instance */ + struct bmi160_fifo_frame fifo_frame; + fifo_frame.data = fifo_buff; + fifo_frame.length = 300; + dev->fifo = &fifo_frame; + uint16_t index = 0; + + /* Declare instances of the sensor data structure to store the parsed FIFO data */ + struct bmi160_sensor_data gyro_data[42]; // 300 bytes / ~7bytes per frame ~ 42 data frames + uint8_t gyro_frames_req = 42; + uint8_t gyro_index; + + /* Configure the sensor's FIFO settings */ + rslt = bmi160_set_fifo_config(BMI160_FIFO_GYRO | BMI160_FIFO_HEADER | BMI160_FIFO_TIME, + BMI160_ENABLE, dev); + + if (rslt == BMI160_OK) { + /* At ODR of 100 Hz ,1 frame gets updated in 1/100 = 0.01s + i.e. for 42 frames we need 42 * 0.01 = 0.42s = 420ms delay */ + dev->delay_ms(420); + + /* Read data from the sensor's FIFO and store it the FIFO buffer,"fifo_buff" */ + printf("\n USER REQUESTED FIFO LENGTH : %d\n",dev->fifo->length); + rslt = bmi160_get_fifo_data(dev); + + if (rslt == BMI160_OK) { + printf("\n AVAILABLE FIFO LENGTH : %d\n",dev->fifo->length); + /* Print the raw FIFO data */ + for (index = 0; index < dev->fifo->length; index++) { + printf("\n FIFO DATA INDEX[%d] = %d", index, + dev->fifo->data[index]); + } + /* Parse the FIFO data to extract gyro data from the FIFO buffer */ + printf("\n REQUESTED GYRO DATA FRAMES : %d\n ",gyro_frames_req); + rslt = bmi160_extract_gyro(gyro_data, &gyro_frames_req, dev); + + if (rslt == BMI160_OK) { + printf("\n AVAILABLE GYRO DATA FRAMES : %d\n ",gyro_frames_req); + + /* Print the parsed gyro data from the FIFO buffer */ + for (gyro_index = 0; gyro_index < gyro_frames_req; gyro_index++) { + printf("\nFIFO GYRO FRAME[%d]",gyro_index); + printf("\nGYRO X-DATA : %d \t Y-DATA : %d \t Z-DATA : %d" + ,gyro_data[gyro_index].x ,gyro_data[gyro_index].y + ,gyro_data[gyro_index].z); + } + /* Print the special FIFO frame data like sensortime */ + printf("\n SENSOR TIME DATA : %d \n",dev->fifo->sensor_time); + printf("SKIPPED FRAME COUNT : %d",dev->fifo->skipped_frame_count); + } else { + printf("\n Gyro data extraction failed"); + } + } else { + printf("\n Reading FIFO data failed"); + } + } else { + printf("\n Setting FIFO configuration failed"); + } + + return rslt; +} +``` + +## FOC and offset compensation +> FOC shouldnot be used in Low-power mode +#### Example for configuring FOC for accel and gyro +``` +/* An example for configuring FOC for accel and gyro data */ +int8_t start_foc(struct bmi160_dev *dev) +{ + int8_t rslt = 0; + /* FOC configuration structure */ + struct bmi160_foc_conf foc_conf; + /* Structure to store the offsets */ + struct bmi160_offsets offsets; + + /* Enable FOC for accel with target values of z = 1g ; x,y as 0g */ + foc_conf.acc_off_en = BMI160_ENABLE; + foc_conf.foc_acc_x = BMI160_FOC_ACCEL_0G; + foc_conf.foc_acc_y = BMI160_FOC_ACCEL_0G; + foc_conf.foc_acc_z = BMI160_FOC_ACCEL_POSITIVE_G; + + /* Enable FOC for gyro */ + foc_conf.foc_gyr_en = BMI160_ENABLE; + foc_conf.gyro_off_en = BMI160_ENABLE; + + rslt = bmi160_start_foc(&foc_conf, &offsets, sen); + + if (rslt == BMI160_OK) { + printf("\n FOC DONE SUCCESSFULLY "); + printf("\n OFFSET VALUES AFTER FOC : "); + printf("\n OFFSET VALUES ACCEL X : %d ",offsets.off_acc_x); + printf("\n OFFSET VALUES ACCEL Y : %d ",offsets.off_acc_y); + printf("\n OFFSET VALUES ACCEL Z : %d ",offsets.off_acc_z); + printf("\n OFFSET VALUES GYRO X : %d ",offsets.off_gyro_x); + printf("\n OFFSET VALUES GYRO Y : %d ",offsets.off_gyro_y); + printf("\n OFFSET VALUES GYRO Z : %d ",offsets.off_gyro_z); + } + + /* After start of FOC offsets will be updated automatically and + * the data will be very much close to the target values of measurement */ + + return rslt; +} +``` + +#### Example for updating the offsets manually +> The offsets set by this method will be reset on soft-reset/POR +``` +/* An example for updating manual offsets to sensor */ +int8_t write_offsets(struct bmi160_dev *dev) +{ + int8_t rslt = 0; + /* FOC configuration structure */ + struct bmi160_foc_conf foc_conf; + /* Structure to store the offsets */ + struct bmi160_offsets offsets; + + /* Enable offset update for accel */ + foc_conf.acc_off_en = BMI160_ENABLE; + + /* Enable offset update for gyro */ + foc_conf.gyro_off_en = BMI160_ENABLE; + + /* offset values set by user */ + offsets.off_acc_x = 0x10; + offsets.off_acc_y = 0x10; + offsets.off_acc_z = 0x10; + offsets.off_gyro_x = 0x10; + offsets.off_gyro_y = 0x10; + offsets.off_gyro_z = 0x10; + + rslt = bmi160_set_offsets(&foc_conf, &offsets, sen); + + /* After offset setting the data read from the + * sensor will have the corresponding offset */ + + return rslt; +} +``` + +#### Example for updating the offsets into NVM +> The offsets set by this method will be present in NVM and will be +> restored on POR/soft-reset +``` +/* An example for updating manual offsets to sensor */ +int8_t write_offsets_nvm(struct bmi160_dev *dev) +{ + int8_t rslt = 0; + /* FOC configuration structure */ + struct bmi160_foc_conf foc_conf; + /* Structure to store the offsets */ + struct bmi160_offsets offsets; + + /* Enable offset update for accel */ + foc_conf.acc_off_en = BMI160_ENABLE; + + /* Enable offset update for gyro */ + foc_conf.gyro_off_en = BMI160_ENABLE; + + /* offset values set by user as per their reference + * Resolution of accel = 3.9mg/LSB + * Resolution of gyro = (0.061degrees/second)/LSB */ + offsets.off_acc_x = 10; + offsets.off_acc_y = -15; + offsets.off_acc_z = 20; + offsets.off_gyro_x = 30; + offsets.off_gyro_y = -35; + offsets.off_gyro_z = -40; + + rslt = bmi160_set_offsets(&foc_conf, &offsets, sen); + + if (rslt == BMI160_OK) { + /* Update the NVM */ + rslt = bmi160_update_nvm(dev); + } + + /* After this procedure the offsets are written to + * NVM and restored on POR/soft-reset + * The set values can be removed to ideal case by + * invoking the following APIs + * - bmi160_start_foc() + * - bmi160_update_nvm() + */ + + return rslt; +} +``` + + + +## Copyright (C) 2016 - 2017 Bosch Sensortec GmbH \ No newline at end of file diff --git a/bmi160.c b/bmi160.c index c9ae861..9782d41 100644 --- a/bmi160.c +++ b/bmi160.c @@ -1,6336 +1,6360 @@ -/** - * Copyright (C) 2018 - 2019 Bosch Sensortec GmbH - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * - * 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. - * - * Neither the name of the copyright holder nor the names of the - * 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 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 - * - * The information provided is believed to be accurate and reliable. - * The copyright holder assumes no responsibility - * for the consequences of use - * of such information nor for any infringement of patents or - * other rights of third parties which may result from its use. - * No license is granted by implication or otherwise under any patent or - * patent rights of the copyright holder. - * - * @file bmi160.c - * @date 13 Mar 2019 - * @version 3.7.7 - * @brief - * - */ - -/*! - * @defgroup bmi160 - * @brief - * @{*/ - -#include "bmi160.h" - -/* Below look up table follows the enum bmi160_int_types. - * Hence any change should match to the enum bmi160_int_types - */ -const uint8_t int_mask_lookup_table[13] = { - BMI160_INT1_SLOPE_MASK, BMI160_INT1_SLOPE_MASK, BMI160_INT2_LOW_STEP_DETECT_MASK, BMI160_INT1_DOUBLE_TAP_MASK, - BMI160_INT1_SINGLE_TAP_MASK, BMI160_INT1_ORIENT_MASK, BMI160_INT1_FLAT_MASK, BMI160_INT1_HIGH_G_MASK, - BMI160_INT1_LOW_G_MASK, BMI160_INT1_NO_MOTION_MASK, BMI160_INT2_DATA_READY_MASK, BMI160_INT2_FIFO_FULL_MASK, - BMI160_INT2_FIFO_WM_MASK -}; - -/*********************************************************************/ -/* Static function declarations */ - -/*! - * @brief This API configures the pins to fire the - * interrupt signal when it occurs - * - * @param[in] int_config : Structure instance of bmi160_int_settg. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error. - */ -static int8_t set_intr_pin_config(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); - -/*! - * @brief This API sets the any-motion interrupt of the sensor. - * This interrupt occurs when accel values exceeds preset threshold - * for a certain period of time. - * - * @param[in] int_config : Structure instance of bmi160_int_settg. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error. - */ -static int8_t set_accel_any_motion_int(struct bmi160_int_settg *int_config, struct bmi160_dev *dev); - -/*! - * @brief This API sets tap interrupts.Interrupt is fired when - * tap movements happen. - * - * @param[in] int_config : Structure instance of bmi160_int_settg. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error. - */ -static int8_t set_accel_tap_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); - -/*! - * @brief This API sets the data ready interrupt for both accel and gyro. - * This interrupt occurs when new accel and gyro data come. - * - * @param[in] int_config : Structure instance of bmi160_int_settg. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error. - */ -static int8_t set_accel_gyro_data_ready_int(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); - -/*! - * @brief This API sets the significant motion interrupt of the sensor.This - * interrupt occurs when there is change in user location. - * - * @param[in] int_config : Structure instance of bmi160_int_settg. - * @param[in] dev : Structure instance of bmi160_dev. - * - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error. - */ -static int8_t set_accel_sig_motion_int(struct bmi160_int_settg *int_config, struct bmi160_dev *dev); - -/*! - * @brief This API sets the no motion/slow motion interrupt of the sensor. - * Slow motion is similar to any motion interrupt.No motion interrupt - * occurs when slope bet. two accel values falls below preset threshold - * for preset duration. - * - * @param[in] int_config : Structure instance of bmi160_int_settg. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error. - */ -static int8_t set_accel_no_motion_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); - -/*! - * @brief This API sets the step detection interrupt.This interrupt - * occurs when the single step causes accel values to go above - * preset threshold. - * - * @param[in] int_config : Structure instance of bmi160_int_settg. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error. - */ -static int8_t set_accel_step_detect_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); - -/*! - * @brief This API sets the orientation interrupt of the sensor.This - * interrupt occurs when there is orientation change in the sensor - * with respect to gravitational field vector g. - * - * @param[in] int_config : Structure instance of bmi160_int_settg. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error. - */ -static int8_t set_accel_orientation_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); - -/*! - * @brief This API sets the flat interrupt of the sensor.This interrupt - * occurs in case of flat orientation - * - * @param[in] int_config : Structure instance of bmi160_int_settg. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error. - */ -static int8_t set_accel_flat_detect_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); - -/*! - * @brief This API sets the low-g interrupt of the sensor.This interrupt - * occurs during free-fall. - * - * @param[in] int_config : Structure instance of bmi160_int_settg. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error. - */ -static int8_t set_accel_low_g_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); - -/*! - * @brief This API sets the high-g interrupt of the sensor.The interrupt - * occurs if the absolute value of acceleration data of any enabled axis - * exceeds the programmed threshold and the sign of the value does not - * change for a preset duration. - * - * @param[in] int_config : Structure instance of bmi160_int_settg. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error. - */ -static int8_t set_accel_high_g_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); - -/*! - * @brief This API sets the default configuration parameters of accel & gyro. - * Also maintain the previous state of configurations. - * - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error. - */ -static void default_param_settg(struct bmi160_dev *dev); - -/*! - * @brief This API is used to validate the device structure pointer for - * null conditions. - * - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error. - */ -static int8_t null_ptr_check(const struct bmi160_dev *dev); - -/*! - * @brief This API set the accel configuration. - * - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error. - */ -static int8_t set_accel_conf(struct bmi160_dev *dev); - -/*! - * @brief This API check the accel configuration. - * - * @param[in] data : Pointer to store the updated accel config. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error. - */ -static int8_t check_accel_config(uint8_t *data, const struct bmi160_dev *dev); - -/*! - * @brief This API process the accel odr. - * - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error. - */ -static int8_t process_accel_odr(uint8_t *data, const struct bmi160_dev *dev); - -/*! - * @brief This API process the accel bandwidth. - * - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error. - */ -static int8_t process_accel_bw(uint8_t *data, const struct bmi160_dev *dev); - -/*! - * @brief This API process the accel range. - * - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error. - */ -static int8_t process_accel_range(uint8_t *data, const struct bmi160_dev *dev); - -/*! - * @brief This API checks the invalid settings for ODR & Bw for Accel and Gyro. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error. - */ -static int8_t check_invalid_settg(const struct bmi160_dev *dev); - -/*! - * @brief This API set the gyro configuration. - * - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error. - */ -static int8_t set_gyro_conf(struct bmi160_dev *dev); - -/*! - * @brief This API check the gyro configuration. - * - * @param[in] data : Pointer to store the updated gyro config. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error. - */ -static int8_t check_gyro_config(uint8_t *data, const struct bmi160_dev *dev); - -/*! - * @brief This API process the gyro odr. - * - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error. - */ -static int8_t process_gyro_odr(uint8_t *data, const struct bmi160_dev *dev); - -/*! - * @brief This API process the gyro bandwidth. - * - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error. - */ -static int8_t process_gyro_bw(uint8_t *data, const struct bmi160_dev *dev); - -/*! - * @brief This API process the gyro range. - * - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error. - */ -static int8_t process_gyro_range(uint8_t *data, const struct bmi160_dev *dev); - -/*! - * @brief This API sets the accel power mode. - * - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error. - */ -static int8_t set_accel_pwr(struct bmi160_dev *dev); - -/*! - * @brief This API process the undersampling setting of Accel. - * - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error. - */ -static int8_t process_under_sampling(uint8_t *data, const struct bmi160_dev *dev); - -/*! - * @brief This API sets the gyro power mode. - * - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error. - */ -static int8_t set_gyro_pwr(struct bmi160_dev *dev); - -/*! - * @brief This API reads accel data along with sensor time if time is requested - * by user. Kindly refer the user guide(README.md) for more info. - * - * @param[in] len : len to read no of bytes - * @param[out] accel : Structure pointer to store accel data - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t get_accel_data(uint8_t len, struct bmi160_sensor_data *accel, const struct bmi160_dev *dev); - -/*! - * @brief This API reads accel data along with sensor time if time is requested - * by user. Kindly refer the user guide(README.md) for more info. - * - * @param[in] len : len to read no of bytes - * @param[out] gyro : Structure pointer to store accel data - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t get_gyro_data(uint8_t len, struct bmi160_sensor_data *gyro, const struct bmi160_dev *dev); - -/*! - * @brief This API reads accel and gyro data along with sensor time - * if time is requested by user. - * Kindly refer the user guide(README.md) for more info. - * - * @param[in] len : len to read no of bytes - * @param[out] accel : Structure pointer to store accel data - * @param[out] gyro : Structure pointer to store accel data - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t get_accel_gyro_data(uint8_t len, - struct bmi160_sensor_data *accel, - struct bmi160_sensor_data *gyro, - const struct bmi160_dev *dev); - -/*! - * @brief This API enables the any-motion interrupt for accel. - * - * @param[in] any_motion_int_cfg : Structure instance of - * bmi160_acc_any_mot_int_cfg. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t enable_accel_any_motion_int(const struct bmi160_acc_any_mot_int_cfg *any_motion_int_cfg, - struct bmi160_dev *dev); - -/*! - * @brief This API disable the sig-motion interrupt. - * - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t disable_sig_motion_int(const struct bmi160_dev *dev); - -/*! - * @brief This API configure the source of data(filter & pre-filter) - * for any-motion interrupt. - * - * @param[in] any_motion_int_cfg : Structure instance of - * bmi160_acc_any_mot_int_cfg. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t config_any_motion_src(const struct bmi160_acc_any_mot_int_cfg *any_motion_int_cfg, - const struct bmi160_dev *dev); - -/*! - * @brief This API configure the duration and threshold of - * any-motion interrupt. - * - * @param[in] any_motion_int_cfg : Structure instance of - * bmi160_acc_any_mot_int_cfg. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t config_any_dur_threshold(const struct bmi160_acc_any_mot_int_cfg *any_motion_int_cfg, - const struct bmi160_dev *dev); - -/*! - * @brief This API configure necessary setting of any-motion interrupt. - * - * @param[in] int_config : Structure instance of bmi160_int_settg. - * @param[in] any_motion_int_cfg : Structure instance of - * bmi160_acc_any_mot_int_cfg. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t config_any_motion_int_settg(const struct bmi160_int_settg *int_config, - const struct bmi160_acc_any_mot_int_cfg *any_motion_int_cfg, - const struct bmi160_dev *dev); - -/*! - * @brief This API enable the data ready interrupt. - * - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t enable_data_ready_int(const struct bmi160_dev *dev); - -/*! - * @brief This API enables the no motion/slow motion interrupt. - * - * @param[in] no_mot_int_cfg : Structure instance of - * bmi160_acc_no_motion_int_cfg. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t enable_no_motion_int(const struct bmi160_acc_no_motion_int_cfg *no_mot_int_cfg, - const struct bmi160_dev *dev); - -/*! - * @brief This API configure the interrupt PIN setting for - * no motion/slow motion interrupt. - * - * @param[in] int_config : structure instance of bmi160_int_settg. - * @param[in] no_mot_int_cfg : Structure instance of - * bmi160_acc_no_motion_int_cfg. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t config_no_motion_int_settg(const struct bmi160_int_settg *int_config, - const struct bmi160_acc_no_motion_int_cfg *no_mot_int_cfg, - const struct bmi160_dev *dev); - -/*! - * @brief This API configure the source of interrupt for no motion. - * - * @param[in] no_mot_int_cfg : Structure instance of - * bmi160_acc_no_motion_int_cfg. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t config_no_motion_data_src(const struct bmi160_acc_no_motion_int_cfg *no_mot_int_cfg, - const struct bmi160_dev *dev); - -/*! - * @brief This API configure the duration and threshold of - * no motion/slow motion interrupt along with selection of no/slow motion. - * - * @param[in] no_mot_int_cfg : Structure instance of - * bmi160_acc_no_motion_int_cfg. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t config_no_motion_dur_thr(const struct bmi160_acc_no_motion_int_cfg *no_mot_int_cfg, - const struct bmi160_dev *dev); - -/*! - * @brief This API enables the sig-motion motion interrupt. - * - * @param[in] sig_mot_int_cfg : Structure instance of - * bmi160_acc_sig_mot_int_cfg. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t enable_sig_motion_int(const struct bmi160_acc_sig_mot_int_cfg *sig_mot_int_cfg, struct bmi160_dev *dev); - -/*! - * @brief This API configure the interrupt PIN setting for - * significant motion interrupt. - * - * @param[in] int_config : Structure instance of bmi160_int_settg. - * @param[in] sig_mot_int_cfg : Structure instance of - * bmi160_acc_sig_mot_int_cfg. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t config_sig_motion_int_settg(const struct bmi160_int_settg *int_config, - const struct bmi160_acc_sig_mot_int_cfg *sig_mot_int_cfg, - const struct bmi160_dev *dev); - -/*! - * @brief This API configure the source of data(filter & pre-filter) - * for sig motion interrupt. - * - * @param[in] sig_mot_int_cfg : Structure instance of - * bmi160_acc_sig_mot_int_cfg. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t config_sig_motion_data_src(const struct bmi160_acc_sig_mot_int_cfg *sig_mot_int_cfg, - const struct bmi160_dev *dev); - -/*! - * @brief This API configure the threshold, skip and proof time of - * sig motion interrupt. - * - * @param[in] sig_mot_int_cfg : Structure instance of - * bmi160_acc_sig_mot_int_cfg. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t config_sig_dur_threshold(const struct bmi160_acc_sig_mot_int_cfg *sig_mot_int_cfg, - const struct bmi160_dev *dev); - -/*! - * @brief This API enables the step detector interrupt. - * - * @param[in] step_detect_int_cfg : Structure instance of - * bmi160_acc_step_detect_int_cfg. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t enable_step_detect_int(const struct bmi160_acc_step_detect_int_cfg *step_detect_int_cfg, - const struct bmi160_dev *dev); - -/*! - * @brief This API configure the step detector parameter. - * - * @param[in] step_detect_int_cfg : Structure instance of - * bmi160_acc_step_detect_int_cfg. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t config_step_detect(const struct bmi160_acc_step_detect_int_cfg *step_detect_int_cfg, - const struct bmi160_dev *dev); - -/*! - * @brief This API enables the single/double tap interrupt. - * - * @param[in] int_config : Structure instance of bmi160_int_settg. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t enable_tap_int(const struct bmi160_int_settg *int_config, - const struct bmi160_acc_tap_int_cfg *tap_int_cfg, - const struct bmi160_dev *dev); - -/*! - * @brief This API configure the interrupt PIN setting for - * tap interrupt. - * - * @param[in] int_config : Structure instance of bmi160_int_settg. - * @param[in] tap_int_cfg : Structure instance of bmi160_acc_tap_int_cfg. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t config_tap_int_settg(const struct bmi160_int_settg *int_config, - const struct bmi160_acc_tap_int_cfg *tap_int_cfg, - const struct bmi160_dev *dev); - -/*! - * @brief This API configure the source of data(filter & pre-filter) - * for tap interrupt. - * - * @param[in] tap_int_cfg : Structure instance of bmi160_acc_tap_int_cfg. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t config_tap_data_src(const struct bmi160_acc_tap_int_cfg *tap_int_cfg, const struct bmi160_dev *dev); - -/*! - * @brief This API configure the parameters of tap interrupt. - * Threshold, quite, shock, and duration. - * - * @param[in] int_config : Structure instance of bmi160_int_settg. - * @param[in] tap_int_cfg : Structure instance of bmi160_acc_tap_int_cfg. - * @param[in] dev : structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t config_tap_param(const struct bmi160_int_settg *int_config, - const struct bmi160_acc_tap_int_cfg *tap_int_cfg, - const struct bmi160_dev *dev); - -/*! - * @brief This API enable the external mode configuration. - * - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t config_sec_if(const struct bmi160_dev *dev); - -/*! - * @brief This API configure the ODR of the auxiliary sensor. - * - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t config_aux_odr(const struct bmi160_dev *dev); - -/*! - * @brief This API maps the actual burst read length set by user. - * - * @param[in] len : Pointer to store the read length. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t map_read_len(uint16_t *len, const struct bmi160_dev *dev); - -/*! - * @brief This API configure the settings of auxiliary sensor. - * - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t config_aux_settg(const struct bmi160_dev *dev); - -/*! - * @brief This API extract the read data from auxiliary sensor. - * - * @param[in] map_len : burst read value. - * @param[in] reg_addr : Address of register to read. - * @param[in] aux_data : Pointer to store the read data. - * @param[in] len : length to read the data. - * @param[in] dev : Structure instance of bmi160_dev. - * @note : Refer user guide for detailed info. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t extract_aux_read(uint16_t map_len, - uint8_t reg_addr, - uint8_t *aux_data, - uint16_t len, - const struct bmi160_dev *dev); - -/*! - * @brief This API enables the orient interrupt. - * - * @param[in] orient_int_cfg : Structure instance of bmi160_acc_orient_int_cfg. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t enable_orient_int(const struct bmi160_acc_orient_int_cfg *orient_int_cfg, const struct bmi160_dev *dev); - -/*! - * @brief This API configure the necessary setting of orientation interrupt. - * - * @param[in] orient_int_cfg : Structure instance of bmi160_acc_orient_int_cfg. - * @param[in] dev : structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t config_orient_int_settg(const struct bmi160_acc_orient_int_cfg *orient_int_cfg, - const struct bmi160_dev *dev); - -/*! - * @brief This API enables the flat interrupt. - * - * @param[in] flat_int : Structure instance of bmi160_acc_flat_detect_int_cfg. - * @param[in] dev : structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t enable_flat_int(const struct bmi160_acc_flat_detect_int_cfg *flat_int, const struct bmi160_dev *dev); - -/*! - * @brief This API configure the necessary setting of flat interrupt. - * - * @param[in] flat_int : Structure instance of bmi160_acc_flat_detect_int_cfg. - * @param[in] dev : structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t config_flat_int_settg(const struct bmi160_acc_flat_detect_int_cfg *flat_int, - const struct bmi160_dev *dev); - -/*! - * @brief This API enables the Low-g interrupt. - * - * @param[in] low_g_int : Structure instance of bmi160_acc_low_g_int_cfg. - * @param[in] dev : structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t enable_low_g_int(const struct bmi160_acc_low_g_int_cfg *low_g_int, const struct bmi160_dev *dev); - -/*! - * @brief This API configure the source of data(filter & pre-filter) for low-g interrupt. - * - * @param[in] low_g_int : Structure instance of bmi160_acc_low_g_int_cfg. - * @param[in] dev : structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t config_low_g_data_src(const struct bmi160_acc_low_g_int_cfg *low_g_int, const struct bmi160_dev *dev); - -/*! - * @brief This API configure the necessary setting of low-g interrupt. - * - * @param[in] low_g_int : Structure instance of bmi160_acc_low_g_int_cfg. - * @param[in] dev : structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t config_low_g_int_settg(const struct bmi160_acc_low_g_int_cfg *low_g_int, const struct bmi160_dev *dev); - -/*! - * @brief This API enables the high-g interrupt. - * - * @param[in] high_g_int_cfg : Structure instance of bmi160_acc_high_g_int_cfg. - * @param[in] dev : structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t enable_high_g_int(const struct bmi160_acc_high_g_int_cfg *high_g_int_cfg, const struct bmi160_dev *dev); - -/*! - * @brief This API configure the source of data(filter & pre-filter) - * for high-g interrupt. - * - * @param[in] high_g_int_cfg : Structure instance of bmi160_acc_high_g_int_cfg. - * @param[in] dev : structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t config_high_g_data_src(const struct bmi160_acc_high_g_int_cfg *high_g_int_cfg, - const struct bmi160_dev *dev); - -/*! - * @brief This API configure the necessary setting of high-g interrupt. - * - * @param[in] high_g_int_cfg : Structure instance of bmi160_acc_high_g_int_cfg. - * @param[in] dev : structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t config_high_g_int_settg(const struct bmi160_acc_high_g_int_cfg *high_g_int_cfg, - const struct bmi160_dev *dev); - -/*! - * @brief This API configure the behavioural setting of interrupt pin. - * - * @param[in] int_config : Structure instance of bmi160_int_settg. - * @param[in] dev : structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t config_int_out_ctrl(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); - -/*! - * @brief This API configure the mode(input enable, latch or non-latch) of interrupt pin. - * - * @param[in] int_config : Structure instance of bmi160_int_settg. - * @param[in] dev : structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t config_int_latch(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); - -/*! - * @brief This API performs the self test for accelerometer of BMI160 - * - * @param[in] dev : structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t perform_accel_self_test(struct bmi160_dev *dev); - -/*! - * @brief This API enables to perform the accel self test by setting proper - * configurations to facilitate accel self test - * - * @param[in] dev : structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t enable_accel_self_test(struct bmi160_dev *dev); - -/*! - * @brief This API performs accel self test with positive excitation - * - * @param[in] accel_pos : Structure pointer to store accel data - * for positive excitation - * @param[in] dev : structure instance of bmi160_dev - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t accel_self_test_positive_excitation(struct bmi160_sensor_data *accel_pos, const struct bmi160_dev *dev); - -/*! - * @brief This API performs accel self test with negative excitation - * - * @param[in] accel_neg : Structure pointer to store accel data - * for negative excitation - * @param[in] dev : structure instance of bmi160_dev - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t accel_self_test_negative_excitation(struct bmi160_sensor_data *accel_neg, const struct bmi160_dev *dev); - -/*! - * @brief This API validates the accel self test results - * - * @param[in] accel_pos : Structure pointer to store accel data - * for positive excitation - * @param[in] accel_neg : Structure pointer to store accel data - * for negative excitation - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error / +ve value -> Self test fail - */ -static int8_t validate_accel_self_test(const struct bmi160_sensor_data *accel_pos, - const struct bmi160_sensor_data *accel_neg); - -/*! - * @brief This API performs the self test for gyroscope of BMI160 - * - * @param[in] dev : structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t perform_gyro_self_test(const struct bmi160_dev *dev); - -/*! - * @brief This API enables the self test bit to trigger self test for gyro - * - * @param[in] dev : structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t enable_gyro_self_test(const struct bmi160_dev *dev); - -/*! - * @brief This API validates the self test results of gyro - * - * @param[in] dev : structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t validate_gyro_self_test(const struct bmi160_dev *dev); - -/*! - * @brief This API sets FIFO full interrupt of the sensor.This interrupt - * occurs when the FIFO is full and the next full data sample would cause - * a FIFO overflow, which may delete the old samples. - * - * @param[in] int_config : Structure instance of bmi160_int_settg. - * @param[in] dev : structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t set_fifo_full_int(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); - -/*! - * @brief This enable the FIFO full interrupt engine. - * - * @param[in] int_config : Structure instance of bmi160_int_settg. - * @param[in] dev : structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t enable_fifo_full_int(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); - -/*! - * @brief This API sets FIFO watermark interrupt of the sensor.The FIFO - * watermark interrupt is fired, when the FIFO fill level is above a fifo - * watermark. - * - * @param[in] int_config : Structure instance of bmi160_int_settg. - * @param[in] dev : structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t set_fifo_watermark_int(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); - -/*! - * @brief This enable the FIFO watermark interrupt engine. - * - * @param[in] int_config : Structure instance of bmi160_int_settg. - * @param[in] dev : structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t enable_fifo_wtm_int(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); - -/*! - * @brief This API is used to reset the FIFO related configurations - * in the fifo_frame structure. - * - * @param[in] dev : structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static void reset_fifo_data_structure(const struct bmi160_dev *dev); - -/*! - * @brief This API is used to read number of bytes filled - * currently in FIFO buffer. - * - * @param[in] bytes_to_read : Number of bytes available in FIFO at the - * instant which is obtained from FIFO counter. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error. - * @retval Any non zero value -> Fail - * - */ -static int8_t get_fifo_byte_counter(uint16_t *bytes_to_read, struct bmi160_dev const *dev); - -/*! - * @brief This API is used to compute the number of bytes of accel FIFO data - * which is to be parsed in header-less mode - * - * @param[out] data_index : The start index for parsing data - * @param[out] data_read_length : Number of bytes to be parsed - * @param[in] acc_frame_count : Number of accelerometer frames to be read - * @param[in] dev : Structure instance of bmi160_dev. - * - */ -static void get_accel_len_to_parse(uint16_t *data_index, - uint16_t *data_read_length, - const uint8_t *acc_frame_count, - const struct bmi160_dev *dev); - -/*! - * @brief This API is used to parse the accelerometer data from the - * FIFO data in both header mode and header-less mode. - * It updates the idx value which is used to store the index of - * the current data byte which is parsed. - * - * @param[in,out] acc : structure instance of sensor data - * @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) frames parsed - * @param[in] frame_info : It consists of either fifo_data_enable - * parameter in header-less mode or - * frame header data in header mode - * @param[in] dev : structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static void unpack_accel_frame(struct bmi160_sensor_data *acc, - uint16_t *idx, - uint8_t *acc_idx, - uint8_t frame_info, - const struct bmi160_dev *dev); - -/*! - * @brief This API is used to parse the accelerometer data from the - * FIFO data and store it in the instance of the structure bmi160_sensor_data. - * - * @param[in,out] accel_data : structure instance of sensor data - * @param[in,out] data_start_index : Index value of number of bytes parsed - * @param[in] dev : structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static void unpack_accel_data(struct bmi160_sensor_data *accel_data, - uint16_t data_start_index, - const struct bmi160_dev *dev); - -/*! - * @brief This API is used to parse the accelerometer data from the - * FIFO data in header mode. - * - * @param[in,out] accel_data : Structure instance of sensor data - * @param[in,out] accel_length : Number of accelerometer frames - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static void extract_accel_header_mode(struct bmi160_sensor_data *accel_data, - uint8_t *accel_length, - const struct bmi160_dev *dev); - -/*! - * @brief This API computes the number of bytes of gyro FIFO data - * which is to be parsed in header-less mode - * - * @param[out] data_index : The start index for parsing data - * @param[out] data_read_length : No of bytes to be parsed from FIFO buffer - * @param[in] gyro_frame_count : Number of Gyro data frames to be read - * @param[in] dev : Structure instance of bmi160_dev. - */ -static void get_gyro_len_to_parse(uint16_t *data_index, - uint16_t *data_read_length, - const uint8_t *gyro_frame_count, - const struct bmi160_dev *dev); - -/*! - * @brief This API is used to parse the gyroscope's data from the - * FIFO data in both header mode and header-less mode. - * It updates the idx value which is used to store the index of - * the current data byte which is parsed. - * - * @param[in,out] gyro : structure instance of sensor data - * @param[in,out] idx : Index value of number of bytes parsed - * @param[in,out] gyro_idx : Index value of gyro data - * (x,y,z axes) frames parsed - * @param[in] frame_info : It consists of either fifo_data_enable - * parameter in header-less mode or - * frame header data in header mode - * @param[in] dev : structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static void unpack_gyro_frame(struct bmi160_sensor_data *gyro, - uint16_t *idx, - uint8_t *gyro_idx, - uint8_t frame_info, - const struct bmi160_dev *dev); - -/*! - * @brief This API is used to parse the gyro data from the - * FIFO data and store it in the instance of the structure bmi160_sensor_data. - * - * @param[in,out] gyro_data : structure instance of sensor data - * @param[in,out] data_start_index : Index value of number of bytes parsed - * @param[in] dev : structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static void unpack_gyro_data(struct bmi160_sensor_data *gyro_data, - uint16_t data_start_index, - const struct bmi160_dev *dev); - -/*! - * @brief This API is used to parse the gyro data from the - * FIFO data in header mode. - * - * @param[in,out] gyro_data : Structure instance of sensor data - * @param[in,out] gyro_length : Number of gyro frames - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static void extract_gyro_header_mode(struct bmi160_sensor_data *gyro_data, - uint8_t *gyro_length, - const struct bmi160_dev *dev); - -/*! - * @brief This API computes the number of bytes of aux FIFO data - * which is to be parsed in header-less mode - * - * @param[out] data_index : The start index for parsing data - * @param[out] data_read_length : No of bytes to be parsed from FIFO buffer - * @param[in] aux_frame_count : Number of Aux data frames to be read - * @param[in] dev : Structure instance of bmi160_dev. - */ -static void get_aux_len_to_parse(uint16_t *data_index, - uint16_t *data_read_length, - const uint8_t *aux_frame_count, - const struct bmi160_dev *dev); - -/*! - * @brief This API is used to parse the aux's data from the - * FIFO data in both header mode and header-less mode. - * It updates the idx value which is used to store the index of - * the current data byte which is parsed - * - * @param[in,out] aux_data : structure instance of sensor data - * @param[in,out] idx : Index value of number of bytes parsed - * @param[in,out] aux_index : Index value of gyro data - * (x,y,z axes) frames parsed - * @param[in] frame_info : It consists of either fifo_data_enable - * parameter in header-less mode or - * frame header data in header mode - * @param[in] dev : structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static void unpack_aux_frame(struct bmi160_aux_data *aux_data, - uint16_t *idx, - uint8_t *aux_index, - uint8_t frame_info, - const struct bmi160_dev *dev); - -/*! - * @brief This API is used to parse the aux data from the - * FIFO data and store it in the instance of the structure bmi160_aux_data. - * - * @param[in,out] aux_data : structure instance of sensor data - * @param[in,out] data_start_index : Index value of number of bytes parsed - * @param[in] dev : structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static void unpack_aux_data(struct bmi160_aux_data *aux_data, uint16_t data_start_index, const struct bmi160_dev *dev); - -/*! - * @brief This API is used to parse the aux data from the - * FIFO data in header mode. - * - * @param[in,out] aux_data : Structure instance of sensor data - * @param[in,out] aux_length : Number of aux frames - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static void extract_aux_header_mode(struct bmi160_aux_data *aux_data, uint8_t *aux_length, - const struct bmi160_dev *dev); - -/*! - * @brief This API checks the presence of non-valid frames in the read fifo data. - * - * @param[in,out] data_index : The index of the current data to - * be parsed from fifo data - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static void check_frame_validity(uint16_t *data_index, const struct bmi160_dev *dev); - -/*! - * @brief This 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 a particular frame - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static void move_next_frame(uint16_t *data_index, uint8_t current_frame_length, const struct bmi160_dev *dev); - -/*! - * @brief This API is used to parse and store the sensor time from the - * FIFO data in the structure instance dev. - * - * @param[in,out] data_index : Index of the FIFO data which - * has the sensor time. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static void unpack_sensortime_frame(uint16_t *data_index, const struct bmi160_dev *dev); - -/*! - * @brief This API is used to parse and store the skipped_frame_count from - * the FIFO data in the structure instance dev. - * - * @param[in,out] data_index : Index of the FIFO data which - * has the skipped frame count. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static void unpack_skipped_frame(uint16_t *data_index, const struct bmi160_dev *dev); - -/*! - * @brief This API is used to get the FOC status from the sensor - * - * @param[in,out] foc_status : Result of FOC status. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t get_foc_status(uint8_t *foc_status, struct bmi160_dev const *dev); - -/*! - * @brief This API is used to configure the offset enable bits in the sensor - * - * @param[in,out] foc_conf : Structure instance of bmi160_foc_conf which - * has the FOC and offset configurations - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t configure_offset_enable(const struct bmi160_foc_conf *foc_conf, struct bmi160_dev const *dev); - -/*! - * @brief This API is used to trigger the FOC in the sensor - * - * @param[in,out] offset : Structure instance of bmi160_offsets which - * reads and stores the offset values after FOC - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t trigger_foc(struct bmi160_offsets *offset, struct bmi160_dev const *dev); - -/*! - * @brief This API is used to map/unmap the Dataready(Accel & Gyro), FIFO full - * and FIFO watermark interrupt - * - * @param[in] int_config : Structure instance of bmi160_int_settg which - * stores the interrupt type and interrupt channel - * configurations to map/unmap the interrupt pins - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t map_hardware_interrupt(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); - -/*! - * @brief This API is used to map/unmap the Any/Sig motion, Step det/Low-g, - * Double tap, Single tap, Orientation, Flat, High-G, Nomotion interrupt pins. - * - * @param[in] int_config : Structure instance of bmi160_int_settg which - * stores the interrupt type and interrupt channel - * configurations to map/unmap the interrupt pins - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -static int8_t map_feature_interrupt(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); - -/*********************** User function definitions ****************************/ - -/*! - * @brief This API reads the data from the given register address - * of sensor. - */ -int8_t bmi160_get_regs(uint8_t reg_addr, uint8_t *data, uint16_t len, const struct bmi160_dev *dev) -{ - int8_t rslt = BMI160_OK; - - /* Null-pointer check */ - if ((dev == NULL) || (dev->read == NULL)) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - /* Configuring reg_addr for SPI Interface */ - if (dev->interface == BMI160_SPI_INTF) - { - reg_addr = (reg_addr | BMI160_SPI_RD_MASK); - } - rslt = dev->read(dev->id, reg_addr, data, len); - - if (rslt != BMI160_OK) - { - rslt = BMI160_E_COM_FAIL; - } - } - - return rslt; -} - -/*! - * @brief This API writes the given data to the register address - * of sensor. - */ -int8_t bmi160_set_regs(uint8_t reg_addr, uint8_t *data, uint16_t len, const struct bmi160_dev *dev) -{ - int8_t rslt = BMI160_OK; - uint8_t count = 0; - - /* Null-pointer check */ - if ((dev == NULL) || (dev->write == NULL)) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - /* Configuring reg_addr for SPI Interface */ - if (dev->interface == BMI160_SPI_INTF) - { - reg_addr = (reg_addr & BMI160_SPI_WR_MASK); - } - if ((dev->prev_accel_cfg.power == BMI160_ACCEL_NORMAL_MODE) || - (dev->prev_gyro_cfg.power == BMI160_GYRO_NORMAL_MODE)) - { - rslt = dev->write(dev->id, reg_addr, data, len); - - /* Kindly refer bmi160 data sheet section 3.2.4 */ - dev->delay_ms(1); - - } - else - { - /*Burst write is not allowed in - * suspend & low power mode */ - for (; count < len; count++) - { - rslt = dev->write(dev->id, reg_addr, &data[count], 1); - reg_addr++; - - /* Kindly refer bmi160 data sheet section 3.2.4 */ - dev->delay_ms(1); - - } - } - if (rslt != BMI160_OK) - { - rslt = BMI160_E_COM_FAIL; - } - } - - return rslt; -} - -/*! - * @brief This API is the entry point for sensor.It performs - * the selection of I2C/SPI read mechanism according to the - * selected interface and reads the chip-id of bmi160 sensor. - */ -int8_t bmi160_init(struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t data; - uint8_t try = 3; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - - /* Dummy read of 0x7F register to enable SPI Interface - * if SPI is used */ - if ((rslt == BMI160_OK) && (dev->interface == BMI160_SPI_INTF)) - { - rslt = bmi160_get_regs(BMI160_SPI_COMM_TEST_ADDR, &data, 1, dev); - } - if (rslt == BMI160_OK) - { - /* Assign chip id as zero */ - dev->chip_id = 0; - while ((try--) && (dev->chip_id != BMI160_CHIP_ID)) - { - /* Read chip_id */ - rslt = bmi160_get_regs(BMI160_CHIP_ID_ADDR, &dev->chip_id, 1, dev); - } - if ((rslt == BMI160_OK) && (dev->chip_id == BMI160_CHIP_ID)) - { - dev->any_sig_sel = BMI160_BOTH_ANY_SIG_MOTION_DISABLED; - - /* Soft reset */ - rslt = bmi160_soft_reset(dev); - } - else - { - rslt = BMI160_E_DEV_NOT_FOUND; - } - } - - return rslt; -} - -/*! - * @brief This API resets and restarts the device. - * All register values are overwritten with default parameters. - */ -int8_t bmi160_soft_reset(struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t data = BMI160_SOFT_RESET_CMD; - - /* Null-pointer check */ - if ((dev == NULL) || (dev->delay_ms == NULL)) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - /* Reset the device */ - rslt = bmi160_set_regs(BMI160_COMMAND_REG_ADDR, &data, 1, dev); - dev->delay_ms(BMI160_SOFT_RESET_DELAY_MS); - if ((rslt == BMI160_OK) && (dev->interface == BMI160_SPI_INTF)) - { - /* Dummy read of 0x7F register to enable SPI Interface - * if SPI is used */ - rslt = bmi160_get_regs(BMI160_SPI_COMM_TEST_ADDR, &data, 1, dev); - } - if (rslt == BMI160_OK) - { - /* Update the default parameters */ - default_param_settg(dev); - } - } - - return rslt; -} - -/*! - * @brief This API configures the power mode, range and bandwidth - * of sensor. - */ -int8_t bmi160_set_sens_conf(struct bmi160_dev *dev) -{ - int8_t rslt = BMI160_OK; - - /* Null-pointer check */ - if ((dev == NULL) || (dev->delay_ms == NULL)) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - rslt = set_accel_conf(dev); - if (rslt == BMI160_OK) - { - rslt = set_gyro_conf(dev); - if (rslt == BMI160_OK) - { - /* write power mode for accel and gyro */ - rslt = bmi160_set_power_mode(dev); - if (rslt == BMI160_OK) - { - rslt = check_invalid_settg(dev); - } - } - } - } - - return rslt; -} - -/*! - * @brief This API sets the power mode of the sensor. - */ -int8_t bmi160_set_power_mode(struct bmi160_dev *dev) -{ - int8_t rslt = 0; - - /* Null-pointer check */ - if ((dev == NULL) || (dev->delay_ms == NULL)) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - rslt = set_accel_pwr(dev); - if (rslt == BMI160_OK) - { - rslt = set_gyro_pwr(dev); - } - } - - return rslt; -} - -/*! - * @brief This API gets the power mode of the sensor. - */ -int8_t bmi160_get_power_mode(struct bmi160_pmu_status *pmu_status, const struct bmi160_dev *dev) -{ - int8_t rslt = 0; - uint8_t power_mode = 0; - - /* Null-pointer check */ - if ((dev == NULL) || (dev->delay_ms == NULL)) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - rslt = bmi160_get_regs(BMI160_PMU_STATUS_ADDR, &power_mode, 1, dev); - if (rslt == BMI160_OK) - { - /* Power mode of the accel,gyro,aux sensor is obtained */ - pmu_status->aux_pmu_status = BMI160_GET_BITS_POS_0(power_mode, BMI160_MAG_POWER_MODE); - pmu_status->gyro_pmu_status = BMI160_GET_BITS(power_mode, BMI160_GYRO_POWER_MODE); - pmu_status->accel_pmu_status = BMI160_GET_BITS(power_mode, BMI160_ACCEL_POWER_MODE); - } - } - - return rslt; -} - -/*! - * @brief This API reads sensor data, stores it in - * the bmi160_sensor_data structure pointer passed by the user. - */ -int8_t bmi160_get_sensor_data(uint8_t select_sensor, - struct bmi160_sensor_data *accel, - struct bmi160_sensor_data *gyro, - const struct bmi160_dev *dev) -{ - int8_t rslt = BMI160_OK; - uint8_t time_sel; - uint8_t sen_sel; - uint8_t len = 0; - - /*Extract the sensor and time select information*/ - sen_sel = select_sensor & BMI160_SEN_SEL_MASK; - time_sel = ((sen_sel & BMI160_TIME_SEL) >> 2); - sen_sel = sen_sel & (BMI160_ACCEL_SEL | BMI160_GYRO_SEL); - if (time_sel == 1) - { - len = 3; - } - - /* Null-pointer check */ - if (dev != NULL) - { - switch (sen_sel) - { - case BMI160_ACCEL_ONLY: - - /* Null-pointer check */ - if (accel == NULL) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - rslt = get_accel_data(len, accel, dev); - } - break; - case BMI160_GYRO_ONLY: - - /* Null-pointer check */ - if (gyro == NULL) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - rslt = get_gyro_data(len, gyro, dev); - } - break; - case BMI160_BOTH_ACCEL_AND_GYRO: - - /* Null-pointer check */ - if ((gyro == NULL) || (accel == NULL)) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - rslt = get_accel_gyro_data(len, accel, gyro, dev); - } - break; - default: - rslt = BMI160_E_INVALID_INPUT; - break; - } - } - else - { - rslt = BMI160_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API configures the necessary interrupt based on - * the user settings in the bmi160_int_settg structure instance. - */ -int8_t bmi160_set_int_config(struct bmi160_int_settg *int_config, struct bmi160_dev *dev) -{ - int8_t rslt = BMI160_OK; - - switch (int_config->int_type) - { - case BMI160_ACC_ANY_MOTION_INT: - - /*Any-motion interrupt*/ - rslt = set_accel_any_motion_int(int_config, dev); - break; - case BMI160_ACC_SIG_MOTION_INT: - - /* Significant motion interrupt */ - rslt = set_accel_sig_motion_int(int_config, dev); - break; - case BMI160_ACC_SLOW_NO_MOTION_INT: - - /* Slow or no motion interrupt */ - rslt = set_accel_no_motion_int(int_config, dev); - break; - case BMI160_ACC_DOUBLE_TAP_INT: - case BMI160_ACC_SINGLE_TAP_INT: - - /* Double tap and single tap Interrupt */ - rslt = set_accel_tap_int(int_config, dev); - break; - case BMI160_STEP_DETECT_INT: - - /* Step detector interrupt */ - rslt = set_accel_step_detect_int(int_config, dev); - break; - case BMI160_ACC_ORIENT_INT: - - /* Orientation interrupt */ - rslt = set_accel_orientation_int(int_config, dev); - break; - case BMI160_ACC_FLAT_INT: - - /* Flat detection interrupt */ - rslt = set_accel_flat_detect_int(int_config, dev); - break; - case BMI160_ACC_LOW_G_INT: - - /* Low-g interrupt */ - rslt = set_accel_low_g_int(int_config, dev); - break; - case BMI160_ACC_HIGH_G_INT: - - /* High-g interrupt */ - rslt = set_accel_high_g_int(int_config, dev); - break; - case BMI160_ACC_GYRO_DATA_RDY_INT: - - /* Data ready interrupt */ - rslt = set_accel_gyro_data_ready_int(int_config, dev); - break; - case BMI160_ACC_GYRO_FIFO_FULL_INT: - - /* Fifo full interrupt */ - rslt = set_fifo_full_int(int_config, dev); - break; - case BMI160_ACC_GYRO_FIFO_WATERMARK_INT: - - /* Fifo water-mark interrupt */ - rslt = set_fifo_watermark_int(int_config, dev); - break; - case BMI160_FIFO_TAG_INT_PIN: - - /* Fifo tagging feature support */ - /* Configure Interrupt pins */ - rslt = set_intr_pin_config(int_config, dev); - break; - default: - break; - } - - return rslt; -} - -/*! - * @brief This API enables or disable the step counter feature. - * 1 - enable step counter (0 - disable) - */ -int8_t bmi160_set_step_counter(uint8_t step_cnt_enable, const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t data = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if (rslt != BMI160_OK) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - rslt = bmi160_get_regs(BMI160_INT_STEP_CONFIG_1_ADDR, &data, 1, dev); - if (rslt == BMI160_OK) - { - if (step_cnt_enable == BMI160_ENABLE) - { - data |= (uint8_t)(step_cnt_enable << 3); - } - else - { - data &= ~BMI160_STEP_COUNT_EN_BIT_MASK; - } - rslt = bmi160_set_regs(BMI160_INT_STEP_CONFIG_1_ADDR, &data, 1, dev); - } - } - - return rslt; -} - -/*! - * @brief This API reads the step counter value. - */ -int8_t bmi160_read_step_counter(uint16_t *step_val, const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t data[2] = { 0, 0 }; - uint16_t msb = 0; - uint8_t lsb = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if (rslt != BMI160_OK) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - rslt = bmi160_get_regs(BMI160_INT_STEP_CNT_0_ADDR, data, 2, dev); - if (rslt == BMI160_OK) - { - lsb = data[0]; - msb = data[1] << 8; - *step_val = msb | lsb; - } - } - - return rslt; -} - -/*! - * @brief This API reads the mention no of byte of data from the given - * register address of auxiliary sensor. - */ -int8_t bmi160_aux_read(uint8_t reg_addr, uint8_t *aux_data, uint16_t len, const struct bmi160_dev *dev) -{ - int8_t rslt = BMI160_OK; - uint16_t map_len = 0; - - /* Null-pointer check */ - if ((dev == NULL) || (dev->read == NULL)) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - if (dev->aux_cfg.aux_sensor_enable == BMI160_ENABLE) - { - rslt = map_read_len(&map_len, dev); - if (rslt == BMI160_OK) - { - rslt = extract_aux_read(map_len, reg_addr, aux_data, len, dev); - } - } - else - { - rslt = BMI160_E_INVALID_INPUT; - } - } - - return rslt; -} - -/*! - * @brief This API writes the mention no of byte of data to the given - * register address of auxiliary sensor. - */ -int8_t bmi160_aux_write(uint8_t reg_addr, uint8_t *aux_data, uint16_t len, const struct bmi160_dev *dev) -{ - int8_t rslt = BMI160_OK; - uint8_t count = 0; - - /* Null-pointer check */ - if ((dev == NULL) || (dev->write == NULL)) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - for (; count < len; count++) - { - /* set data to write */ - rslt = bmi160_set_regs(BMI160_AUX_IF_4_ADDR, aux_data, 1, dev); - dev->delay_ms(BMI160_AUX_COM_DELAY); - if (rslt == BMI160_OK) - { - /* set address to write */ - rslt = bmi160_set_regs(BMI160_AUX_IF_3_ADDR, ®_addr, 1, dev); - dev->delay_ms(BMI160_AUX_COM_DELAY); - if (rslt == BMI160_OK && (count < len - 1)) - { - aux_data++; - reg_addr++; - } - } - } - } - - return rslt; -} - -/*! - * @brief This API initialize the auxiliary sensor - * in order to access it. - */ -int8_t bmi160_aux_init(const struct bmi160_dev *dev) -{ - int8_t rslt; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if (rslt != BMI160_OK) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - if (dev->aux_cfg.aux_sensor_enable == BMI160_ENABLE) - { - /* Configures the auxiliary sensor interface settings */ - rslt = config_aux_settg(dev); - } - else - { - rslt = BMI160_E_INVALID_INPUT; - } - } - - return rslt; -} - -/*! - * @brief This API is used to setup the auxiliary sensor of bmi160 in auto mode - * Thus enabling the auto update of 8 bytes of data from auxiliary sensor - * to BMI160 register address 0x04 to 0x0B - */ -int8_t bmi160_set_aux_auto_mode(uint8_t *data_addr, struct bmi160_dev *dev) -{ - int8_t rslt; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if (rslt != BMI160_OK) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - if (dev->aux_cfg.aux_sensor_enable == BMI160_ENABLE) - { - /* Write the aux. address to read in 0x4D of BMI160*/ - rslt = bmi160_set_regs(BMI160_AUX_IF_2_ADDR, data_addr, 1, dev); - dev->delay_ms(BMI160_AUX_COM_DELAY); - if (rslt == BMI160_OK) - { - /* Configure the polling ODR for - * auxiliary sensor */ - rslt = config_aux_odr(dev); - if (rslt == BMI160_OK) - { - /* Disable the aux. manual mode, i.e aux. - * sensor is in auto-mode (data-mode) */ - dev->aux_cfg.manual_enable = BMI160_DISABLE; - rslt = bmi160_config_aux_mode(dev); - - /* Auxiliary sensor data is obtained - * in auto mode from this point */ - } - } - } - else - { - rslt = BMI160_E_INVALID_INPUT; - } - } - - return rslt; -} - -/*! - * @brief This API configures the 0x4C register and settings like - * Auxiliary sensor manual enable/ disable and aux burst read length. - */ -int8_t bmi160_config_aux_mode(const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t aux_if[2] = { (uint8_t)(dev->aux_cfg.aux_i2c_addr * 2), 0 }; - - rslt = bmi160_get_regs(BMI160_AUX_IF_1_ADDR, &aux_if[1], 1, dev); - if (rslt == BMI160_OK) - { - /* update the Auxiliary interface to manual/auto mode */ - aux_if[1] = BMI160_SET_BITS(aux_if[1], BMI160_MANUAL_MODE_EN, dev->aux_cfg.manual_enable); - - /* update the burst read length defined by user */ - aux_if[1] = BMI160_SET_BITS_POS_0(aux_if[1], BMI160_AUX_READ_BURST, dev->aux_cfg.aux_rd_burst_len); - - /* Set the secondary interface address and manual mode - * along with burst read length */ - rslt = bmi160_set_regs(BMI160_AUX_IF_0_ADDR, &aux_if[0], 2, dev); - dev->delay_ms(BMI160_AUX_COM_DELAY); - } - - return rslt; -} - -/*! - * @brief This API is used to read the raw uncompensated auxiliary sensor - * data of 8 bytes from BMI160 register address 0x04 to 0x0B - */ -int8_t bmi160_read_aux_data_auto_mode(uint8_t *aux_data, const struct bmi160_dev *dev) -{ - int8_t rslt; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if (rslt != BMI160_OK) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - if ((dev->aux_cfg.aux_sensor_enable == BMI160_ENABLE) && (dev->aux_cfg.manual_enable == BMI160_DISABLE)) - { - /* Read the aux. sensor's raw data */ - rslt = bmi160_get_regs(BMI160_AUX_DATA_ADDR, aux_data, 8, dev); - } - else - { - rslt = BMI160_E_INVALID_INPUT; - } - } - - return rslt; -} - -/*! - * @brief This is used to perform self test of accel/gyro of the BMI160 sensor - */ -int8_t bmi160_perform_self_test(uint8_t select_sensor, struct bmi160_dev *dev) -{ - int8_t rslt; - int8_t self_test_rslt = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if (rslt != BMI160_OK) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - /* Proceed if null check is fine */ - switch (select_sensor) - { - case BMI160_ACCEL_ONLY: - rslt = perform_accel_self_test(dev); - break; - case BMI160_GYRO_ONLY: - - /* Set the power mode as normal mode */ - dev->gyro_cfg.power = BMI160_GYRO_NORMAL_MODE; - rslt = bmi160_set_power_mode(dev); - - /* Perform gyro self test */ - if (rslt == BMI160_OK) - { - /* Perform gyro self test */ - rslt = perform_gyro_self_test(dev); - } - break; - default: - rslt = BMI160_E_INVALID_INPUT; - break; - } - - /* Check to ensure bus error does not occur */ - if (rslt >= BMI160_OK) - { - /* Store the status of self test result */ - self_test_rslt = rslt; - - /* Perform soft reset */ - rslt = bmi160_soft_reset(dev); - } - - /* Check to ensure bus operations are success */ - if (rslt == BMI160_OK) - { - /* Restore self_test_rslt as return value */ - rslt = self_test_rslt; - } - } - - return rslt; -} - -/*! - * @brief This API reads the data from fifo buffer. - */ -int8_t bmi160_get_fifo_data(struct bmi160_dev const *dev) -{ - int8_t rslt = 0; - uint16_t bytes_to_read = 0; - uint16_t user_fifo_len = 0; - uint8_t addr = BMI160_FIFO_DATA_ADDR; - - /* check the bmi160 structure as NULL*/ - if ((dev == NULL) || (dev->fifo->data == NULL)) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - reset_fifo_data_structure(dev); - - /* get current FIFO fill-level*/ - rslt = get_fifo_byte_counter(&bytes_to_read, dev); - if (rslt == BMI160_OK) - { - user_fifo_len = dev->fifo->length; - if (dev->fifo->length > bytes_to_read) - { - /* Handling the case where user requests - * more data than available in FIFO */ - dev->fifo->length = bytes_to_read; - } - if ((dev->fifo->fifo_time_enable == BMI160_FIFO_TIME_ENABLE) && - (bytes_to_read + BMI160_FIFO_BYTES_OVERREAD <= user_fifo_len)) - { - /* Handling case of sensor time availability*/ - dev->fifo->length = dev->fifo->length + BMI160_FIFO_BYTES_OVERREAD; - } - if (dev->interface == BMI160_SPI_INTF) - { - /* SPI read mask */ - addr = addr | BMI160_SPI_RD_MASK; - } - - /* read only the filled bytes in the FIFO Buffer */ - rslt = dev->read(dev->id, addr, dev->fifo->data, dev->fifo->length); - } - } - - return rslt; -} - -/*! - * @brief This API writes fifo_flush command to command register.This - * action clears all data in the Fifo without changing fifo configuration - * settings - */ -int8_t bmi160_set_fifo_flush(const struct bmi160_dev *dev) -{ - int8_t rslt = 0; - uint8_t data = BMI160_FIFO_FLUSH_VALUE; - uint8_t reg_addr = BMI160_COMMAND_REG_ADDR; - - /* Check the bmi160_dev structure for NULL address*/ - if (dev == NULL) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - rslt = bmi160_set_regs(reg_addr, &data, BMI160_ONE, dev); - } - - return rslt; -} - -/*! - * @brief This API sets the FIFO configuration in the sensor. - */ -int8_t bmi160_set_fifo_config(uint8_t config, uint8_t enable, struct bmi160_dev const *dev) -{ - int8_t rslt = 0; - uint8_t data = 0; - uint8_t reg_addr = BMI160_FIFO_CONFIG_1_ADDR; - uint8_t fifo_config = config & BMI160_FIFO_CONFIG_1_MASK; - - /* Check the bmi160_dev structure for NULL address*/ - if (dev == NULL) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - rslt = bmi160_get_regs(reg_addr, &data, BMI160_ONE, dev); - if (rslt == BMI160_OK) - { - if (fifo_config > 0) - { - if (enable == BMI160_ENABLE) - { - data = data | fifo_config; - } - else - { - data = data & (~fifo_config); - } - } - - /* write fifo frame content configuration*/ - rslt = bmi160_set_regs(reg_addr, &data, BMI160_ONE, dev); - if (rslt == BMI160_OK) - { - /* read fifo frame content configuration*/ - rslt = bmi160_get_regs(reg_addr, &data, BMI160_ONE, dev); - if (rslt == BMI160_OK) - { - /* extract fifo header enabled status */ - dev->fifo->fifo_header_enable = data & BMI160_FIFO_HEAD_ENABLE; - - /* extract accel/gyr/aux. data enabled status */ - dev->fifo->fifo_data_enable = data & BMI160_FIFO_M_G_A_ENABLE; - - /* extract fifo sensor time enabled status */ - dev->fifo->fifo_time_enable = data & BMI160_FIFO_TIME_ENABLE; - } - } - } - } - - return rslt; -} - -/*! @brief This API is used to configure the down sampling ratios of - * the accel and gyro data for FIFO.Also, it configures filtered or - * pre-filtered data for accel and gyro. - * - */ -int8_t bmi160_set_fifo_down(uint8_t fifo_down, const struct bmi160_dev *dev) -{ - int8_t rslt = 0; - uint8_t data = 0; - uint8_t reg_addr = BMI160_FIFO_DOWN_ADDR; - - /* Check the bmi160_dev structure for NULL address*/ - if (dev == NULL) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - rslt = bmi160_get_regs(reg_addr, &data, BMI160_ONE, dev); - if (rslt == BMI160_OK) - { - data = data | fifo_down; - rslt = bmi160_set_regs(reg_addr, &data, BMI160_ONE, dev); - } - } - - return rslt; -} - -/*! - * @brief This API sets the FIFO watermark level in the sensor. - * - */ -int8_t bmi160_set_fifo_wm(uint8_t fifo_wm, const struct bmi160_dev *dev) -{ - int8_t rslt = 0; - uint8_t data = fifo_wm; - uint8_t reg_addr = BMI160_FIFO_CONFIG_0_ADDR; - - /* Check the bmi160_dev structure for NULL address*/ - if (dev == NULL) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - rslt = bmi160_set_regs(reg_addr, &data, BMI160_ONE, dev); - } - - return rslt; -} - -/*! - * @brief This API parses and extracts the accelerometer frames from - * FIFO data read by the "bmi160_get_fifo_data" API and stores it in - * the "accel_data" structure instance. - */ -int8_t bmi160_extract_accel(struct bmi160_sensor_data *accel_data, uint8_t *accel_length, struct bmi160_dev const *dev) -{ - int8_t rslt = 0; - uint16_t data_index = 0; - uint16_t data_read_length = 0; - uint8_t accel_index = 0; - uint8_t fifo_data_enable = 0; - - if (dev == NULL || dev->fifo == NULL || dev->fifo->data == NULL) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - /* Parsing the FIFO data in header-less mode */ - if (dev->fifo->fifo_header_enable == 0) - { - /* Number of bytes to be parsed from FIFO */ - get_accel_len_to_parse(&data_index, &data_read_length, accel_length, dev); - for (; data_index < data_read_length;) - { - /*Check for the availability of next two bytes of FIFO data */ - check_frame_validity(&data_index, dev); - fifo_data_enable = dev->fifo->fifo_data_enable; - unpack_accel_frame(accel_data, &data_index, &accel_index, fifo_data_enable, dev); - } - - /* update number of accel data read*/ - *accel_length = accel_index; - - /*update the accel byte index*/ - dev->fifo->accel_byte_start_idx = data_index; - } - else - { - /* Parsing the FIFO data in header mode */ - extract_accel_header_mode(accel_data, accel_length, dev); - } - } - - return rslt; -} - -/*! - * @brief This API parses and extracts the gyro frames from - * FIFO data read by the "bmi160_get_fifo_data" API and stores it in - * the "gyro_data" structure instance. - */ -int8_t bmi160_extract_gyro(struct bmi160_sensor_data *gyro_data, uint8_t *gyro_length, struct bmi160_dev const *dev) -{ - int8_t rslt = 0; - uint16_t data_index = 0; - uint16_t data_read_length = 0; - uint8_t gyro_index = 0; - uint8_t fifo_data_enable = 0; - - if (dev == NULL || dev->fifo->data == NULL) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - /* Parsing the FIFO data in header-less mode */ - if (dev->fifo->fifo_header_enable == 0) - { - /* Number of bytes to be parsed from FIFO */ - get_gyro_len_to_parse(&data_index, &data_read_length, gyro_length, dev); - for (; data_index < data_read_length;) - { - /*Check for the availability of next two bytes of FIFO data */ - check_frame_validity(&data_index, dev); - fifo_data_enable = dev->fifo->fifo_data_enable; - unpack_gyro_frame(gyro_data, &data_index, &gyro_index, fifo_data_enable, dev); - } - - /* update number of gyro data read */ - *gyro_length = gyro_index; - - /* update the gyro byte index */ - dev->fifo->gyro_byte_start_idx = data_index; - } - else - { - /* Parsing the FIFO data in header mode */ - extract_gyro_header_mode(gyro_data, gyro_length, dev); - } - } - - return rslt; -} - -/*! - * @brief This API parses and extracts the aux frames from - * FIFO data read by the "bmi160_get_fifo_data" API and stores it in - * the "aux_data" structure instance. - */ -int8_t bmi160_extract_aux(struct bmi160_aux_data *aux_data, uint8_t *aux_len, struct bmi160_dev const *dev) -{ - int8_t rslt = 0; - uint16_t data_index = 0; - uint16_t data_read_length = 0; - uint8_t aux_index = 0; - uint8_t fifo_data_enable = 0; - - if ((dev == NULL) || (dev->fifo->data == NULL) || (aux_data == NULL)) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - /* Parsing the FIFO data in header-less mode */ - if (dev->fifo->fifo_header_enable == 0) - { - /* Number of bytes to be parsed from FIFO */ - get_aux_len_to_parse(&data_index, &data_read_length, aux_len, dev); - for (; data_index < data_read_length;) - { - /* Check for the availability of next two - * bytes of FIFO data */ - check_frame_validity(&data_index, dev); - fifo_data_enable = dev->fifo->fifo_data_enable; - unpack_aux_frame(aux_data, &data_index, &aux_index, fifo_data_enable, dev); - } - - /* update number of aux data read */ - *aux_len = aux_index; - - /* update the aux byte index */ - dev->fifo->aux_byte_start_idx = data_index; - } - else - { - /* Parsing the FIFO data in header mode */ - extract_aux_header_mode(aux_data, aux_len, dev); - } - } - - return rslt; -} - -/*! - * @brief This API starts the FOC of accel and gyro - * - * @note FOC should not be used in low-power mode of sensor - * - * @note Accel FOC targets values of +1g , 0g , -1g - * Gyro FOC always targets value of 0 dps - */ -int8_t bmi160_start_foc(const struct bmi160_foc_conf *foc_conf, - struct bmi160_offsets *offset, - struct bmi160_dev const *dev) -{ - int8_t rslt; - uint8_t data; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if (rslt != BMI160_OK) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - /* Set the offset enable bits */ - rslt = configure_offset_enable(foc_conf, dev); - if (rslt == BMI160_OK) - { - /* Read the FOC config from the sensor */ - rslt = bmi160_get_regs(BMI160_FOC_CONF_ADDR, &data, 1, dev); - - /* Set the FOC config for gyro */ - data = BMI160_SET_BITS(data, BMI160_GYRO_FOC_EN, foc_conf->foc_gyr_en); - - /* Set the FOC config for accel xyz axes */ - data = BMI160_SET_BITS(data, BMI160_ACCEL_FOC_X_CONF, foc_conf->foc_acc_x); - data = BMI160_SET_BITS(data, BMI160_ACCEL_FOC_Y_CONF, foc_conf->foc_acc_y); - data = BMI160_SET_BITS_POS_0(data, BMI160_ACCEL_FOC_Z_CONF, foc_conf->foc_acc_z); - if (rslt == BMI160_OK) - { - /* Set the FOC config in the sensor */ - rslt = bmi160_set_regs(BMI160_FOC_CONF_ADDR, &data, 1, dev); - if (rslt == BMI160_OK) - { - /* Procedure to trigger - * FOC and check status */ - rslt = trigger_foc(offset, dev); - } - } - } - } - - return rslt; -} - -/*! - * @brief This API reads and stores the offset values of accel and gyro - */ -int8_t bmi160_get_offsets(struct bmi160_offsets *offset, const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t data[7]; - uint8_t lsb, msb; - int16_t offset_msb, offset_lsb; - int16_t offset_data; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if (rslt != BMI160_OK) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - /* Read the FOC config from the sensor */ - rslt = bmi160_get_regs(BMI160_OFFSET_ADDR, data, 7, dev); - - /* Accel offsets */ - offset->off_acc_x = (int8_t)data[0]; - offset->off_acc_y = (int8_t)data[1]; - offset->off_acc_z = (int8_t)data[2]; - - /* Gyro x-axis offset */ - lsb = data[3]; - msb = BMI160_GET_BITS_POS_0(data[6], BMI160_GYRO_OFFSET_X); - offset_msb = (int16_t)(msb << 14); - offset_lsb = lsb << 6; - offset_data = offset_msb | offset_lsb; - - /* Divide by 64 to get the Right shift by 6 value */ - offset->off_gyro_x = (int16_t)(offset_data / 64); - - /* Gyro y-axis offset */ - lsb = data[4]; - msb = BMI160_GET_BITS(data[6], BMI160_GYRO_OFFSET_Y); - offset_msb = (int16_t)(msb << 14); - offset_lsb = lsb << 6; - offset_data = offset_msb | offset_lsb; - - /* Divide by 64 to get the Right shift by 6 value */ - offset->off_gyro_y = (int16_t)(offset_data / 64); - - /* Gyro z-axis offset */ - lsb = data[5]; - msb = BMI160_GET_BITS(data[6], BMI160_GYRO_OFFSET_Z); - offset_msb = (int16_t)(msb << 14); - offset_lsb = lsb << 6; - offset_data = offset_msb | offset_lsb; - - /* Divide by 64 to get the Right shift by 6 value */ - offset->off_gyro_z = (int16_t)(offset_data / 64); - } - - return rslt; -} - -/*! - * @brief This API writes the offset values of accel and gyro to - * the sensor but these values will be reset on POR or soft reset. - */ -int8_t bmi160_set_offsets(const struct bmi160_foc_conf *foc_conf, - const struct bmi160_offsets *offset, - struct bmi160_dev const *dev) -{ - int8_t rslt; - uint8_t data[7]; - uint8_t x_msb, y_msb, z_msb; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if (rslt != BMI160_OK) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - /* Update the accel offset */ - data[0] = (uint8_t)offset->off_acc_x; - data[1] = (uint8_t)offset->off_acc_y; - data[2] = (uint8_t)offset->off_acc_z; - - /* Update the LSB of gyro offset */ - data[3] = BMI160_GET_LSB(offset->off_gyro_x); - data[4] = BMI160_GET_LSB(offset->off_gyro_y); - data[5] = BMI160_GET_LSB(offset->off_gyro_z); - - /* Update the MSB of gyro offset */ - x_msb = BMI160_GET_BITS(offset->off_gyro_x, BMI160_GYRO_OFFSET); - y_msb = BMI160_GET_BITS(offset->off_gyro_y, BMI160_GYRO_OFFSET); - z_msb = BMI160_GET_BITS(offset->off_gyro_z, BMI160_GYRO_OFFSET); - data[6] = (uint8_t)(z_msb << 4 | y_msb << 2 | x_msb); - - /* Set the offset enable/disable for gyro and accel */ - data[6] = BMI160_SET_BITS(data[6], BMI160_GYRO_OFFSET_EN, foc_conf->gyro_off_en); - data[6] = BMI160_SET_BITS(data[6], BMI160_ACCEL_OFFSET_EN, foc_conf->acc_off_en); - - /* Set the offset config and values in the sensor */ - rslt = bmi160_set_regs(BMI160_OFFSET_ADDR, data, 7, dev); - } - - return rslt; -} - -/*! - * @brief This API writes the image registers values to NVM which is - * stored even after POR or soft reset - */ -int8_t bmi160_update_nvm(struct bmi160_dev const *dev) -{ - int8_t rslt; - uint8_t data; - uint8_t cmd = BMI160_NVM_BACKUP_EN; - - /* Read the nvm_prog_en configuration */ - rslt = bmi160_get_regs(BMI160_CONF_ADDR, &data, 1, dev); - if (rslt == BMI160_OK) - { - data = BMI160_SET_BITS(data, BMI160_NVM_UPDATE, 1); - - /* Set the nvm_prog_en bit in the sensor */ - rslt = bmi160_set_regs(BMI160_CONF_ADDR, &data, 1, dev); - if (rslt == BMI160_OK) - { - /* Update NVM */ - rslt = bmi160_set_regs(BMI160_COMMAND_REG_ADDR, &cmd, 1, dev); - if (rslt == BMI160_OK) - { - /* Check for NVM ready status */ - rslt = bmi160_get_regs(BMI160_STATUS_ADDR, &data, 1, dev); - if (rslt == BMI160_OK) - { - data = BMI160_GET_BITS(data, BMI160_NVM_STATUS); - if (data != BMI160_ENABLE) - { - /* Delay to update NVM */ - dev->delay_ms(25); - } - } - } - } - } - - return rslt; -} - -/*! - * @brief This API gets the interrupt status from the sensor. - */ -int8_t bmi160_get_int_status(enum bmi160_int_status_sel int_status_sel, - union bmi160_int_status *int_status, - struct bmi160_dev const *dev) -{ - int8_t rslt = 0; - - /* To get the status of all interrupts */ - if (int_status_sel == BMI160_INT_STATUS_ALL) - { - rslt = bmi160_get_regs(BMI160_INT_STATUS_ADDR, &int_status->data[0], 4, dev); - } - else - { - if (int_status_sel & BMI160_INT_STATUS_0) - { - rslt = bmi160_get_regs(BMI160_INT_STATUS_ADDR, &int_status->data[0], 1, dev); - } - if (int_status_sel & BMI160_INT_STATUS_1) - { - rslt = bmi160_get_regs(BMI160_INT_STATUS_ADDR + 1, &int_status->data[1], 1, dev); - } - if (int_status_sel & BMI160_INT_STATUS_2) - { - rslt = bmi160_get_regs(BMI160_INT_STATUS_ADDR + 2, &int_status->data[2], 1, dev); - } - if (int_status_sel & BMI160_INT_STATUS_3) - { - rslt = bmi160_get_regs(BMI160_INT_STATUS_ADDR + 3, &int_status->data[3], 1, dev); - } - } - - return rslt; -} - -/*********************** Local function definitions ***************************/ - -/*! - * @brief This API sets the any-motion interrupt of the sensor. - * This interrupt occurs when accel values exceeds preset threshold - * for a certain period of time. - */ -static int8_t set_accel_any_motion_int(struct bmi160_int_settg *int_config, struct bmi160_dev *dev) -{ - int8_t rslt; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt != BMI160_OK) || (int_config == NULL)) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - /* updating the interrupt structure to local structure */ - struct bmi160_acc_any_mot_int_cfg *any_motion_int_cfg = &(int_config->int_type_cfg.acc_any_motion_int); - rslt = enable_accel_any_motion_int(any_motion_int_cfg, dev); - if (rslt == BMI160_OK) - { - rslt = config_any_motion_int_settg(int_config, any_motion_int_cfg, dev); - } - } - - return rslt; -} - -/*! - * @brief This API sets tap interrupts.Interrupt is fired when - * tap movements happen. - */ -static int8_t set_accel_tap_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) -{ - int8_t rslt; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt != BMI160_OK) || (int_config == NULL)) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - /* updating the interrupt structure to local structure */ - struct bmi160_acc_tap_int_cfg *tap_int_cfg = &(int_config->int_type_cfg.acc_tap_int); - rslt = enable_tap_int(int_config, tap_int_cfg, dev); - if (rslt == BMI160_OK) - { - /* Configure Interrupt pins */ - rslt = set_intr_pin_config(int_config, dev); - if (rslt == BMI160_OK) - { - rslt = config_tap_int_settg(int_config, tap_int_cfg, dev); - } - } - } - - return rslt; -} - -/*! - * @brief This API sets the data ready interrupt for both accel and gyro. - * This interrupt occurs when new accel and gyro data comes. - */ -static int8_t set_accel_gyro_data_ready_int(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) -{ - int8_t rslt; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt != BMI160_OK) || (int_config == NULL)) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - rslt = enable_data_ready_int(dev); - if (rslt == BMI160_OK) - { - /* Configure Interrupt pins */ - rslt = set_intr_pin_config(int_config, dev); - if (rslt == BMI160_OK) - { - rslt = map_hardware_interrupt(int_config, dev); - } - } - } - - return rslt; -} - -/*! - * @brief This API sets the significant motion interrupt of the sensor.This - * interrupt occurs when there is change in user location. - */ -static int8_t set_accel_sig_motion_int(struct bmi160_int_settg *int_config, struct bmi160_dev *dev) -{ - int8_t rslt; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt != BMI160_OK) || (int_config == NULL)) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - /* updating the interrupt structure to local structure */ - struct bmi160_acc_sig_mot_int_cfg *sig_mot_int_cfg = &(int_config->int_type_cfg.acc_sig_motion_int); - rslt = enable_sig_motion_int(sig_mot_int_cfg, dev); - if (rslt == BMI160_OK) - { - rslt = config_sig_motion_int_settg(int_config, sig_mot_int_cfg, dev); - } - } - - return rslt; -} - -/*! - * @brief This API sets the no motion/slow motion interrupt of the sensor. - * Slow motion is similar to any motion interrupt.No motion interrupt - * occurs when slope bet. two accel values falls below preset threshold - * for preset duration. - */ -static int8_t set_accel_no_motion_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) -{ - int8_t rslt; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt != BMI160_OK) || (int_config == NULL)) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - /* updating the interrupt structure to local structure */ - struct bmi160_acc_no_motion_int_cfg *no_mot_int_cfg = &(int_config->int_type_cfg.acc_no_motion_int); - rslt = enable_no_motion_int(no_mot_int_cfg, dev); - if (rslt == BMI160_OK) - { - /* Configure the INT PIN settings*/ - rslt = config_no_motion_int_settg(int_config, no_mot_int_cfg, dev); - } - } - - return rslt; -} - -/*! - * @brief This API sets the step detection interrupt.This interrupt - * occurs when the single step causes accel values to go above - * preset threshold. - */ -static int8_t set_accel_step_detect_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) -{ - int8_t rslt; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt != BMI160_OK) || (int_config == NULL)) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - /* updating the interrupt structure to local structure */ - struct bmi160_acc_step_detect_int_cfg *step_detect_int_cfg = &(int_config->int_type_cfg.acc_step_detect_int); - rslt = enable_step_detect_int(step_detect_int_cfg, dev); - if (rslt == BMI160_OK) - { - /* Configure Interrupt pins */ - rslt = set_intr_pin_config(int_config, dev); - if (rslt == BMI160_OK) - { - rslt = map_feature_interrupt(int_config, dev); - if (rslt == BMI160_OK) - { - rslt = config_step_detect(step_detect_int_cfg, dev); - } - } - } - } - - return rslt; -} - -/*! - * @brief This API sets the orientation interrupt of the sensor.This - * interrupt occurs when there is orientation change in the sensor - * with respect to gravitational field vector g. - */ -static int8_t set_accel_orientation_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) -{ - int8_t rslt; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt != BMI160_OK) || (int_config == NULL)) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - /* updating the interrupt structure to local structure */ - struct bmi160_acc_orient_int_cfg *orient_int_cfg = &(int_config->int_type_cfg.acc_orient_int); - rslt = enable_orient_int(orient_int_cfg, dev); - if (rslt == BMI160_OK) - { - /* Configure Interrupt pins */ - rslt = set_intr_pin_config(int_config, dev); - if (rslt == BMI160_OK) - { - /* map INT pin to orient interrupt */ - rslt = map_feature_interrupt(int_config, dev); - if (rslt == BMI160_OK) - { - /* configure the - * orientation setting*/ - rslt = config_orient_int_settg(orient_int_cfg, dev); - } - } - } - } - - return rslt; -} - -/*! - * @brief This API sets the flat interrupt of the sensor.This interrupt - * occurs in case of flat orientation - */ -static int8_t set_accel_flat_detect_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) -{ - int8_t rslt; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt != BMI160_OK) || (int_config == NULL)) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - /* updating the interrupt structure to local structure */ - struct bmi160_acc_flat_detect_int_cfg *flat_detect_int = &(int_config->int_type_cfg.acc_flat_int); - - /* enable the flat interrupt */ - rslt = enable_flat_int(flat_detect_int, dev); - if (rslt == BMI160_OK) - { - /* Configure Interrupt pins */ - rslt = set_intr_pin_config(int_config, dev); - if (rslt == BMI160_OK) - { - /* map INT pin to flat interrupt */ - rslt = map_feature_interrupt(int_config, dev); - if (rslt == BMI160_OK) - { - /* configure the flat setting*/ - rslt = config_flat_int_settg(flat_detect_int, dev); - } - } - } - } - - return rslt; -} - -/*! - * @brief This API sets the low-g interrupt of the sensor.This interrupt - * occurs during free-fall. - */ -static int8_t set_accel_low_g_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) -{ - int8_t rslt; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt != BMI160_OK) || (int_config == NULL)) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - /* updating the interrupt structure to local structure */ - struct bmi160_acc_low_g_int_cfg *low_g_int = &(int_config->int_type_cfg.acc_low_g_int); - - /* Enable the low-g interrupt*/ - rslt = enable_low_g_int(low_g_int, dev); - if (rslt == BMI160_OK) - { - /* Configure Interrupt pins */ - rslt = set_intr_pin_config(int_config, dev); - if (rslt == BMI160_OK) - { - /* Map INT pin to low-g interrupt */ - rslt = map_feature_interrupt(int_config, dev); - if (rslt == BMI160_OK) - { - /* configure the data source - * for low-g interrupt*/ - rslt = config_low_g_data_src(low_g_int, dev); - if (rslt == BMI160_OK) - { - rslt = config_low_g_int_settg(low_g_int, dev); - } - } - } - } - } - - return rslt; -} - -/*! - * @brief This API sets the high-g interrupt of the sensor.The interrupt - * occurs if the absolute value of acceleration data of any enabled axis - * exceeds the programmed threshold and the sign of the value does not - * change for a preset duration. - */ -static int8_t set_accel_high_g_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) -{ - int8_t rslt; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt != BMI160_OK) || (int_config == NULL)) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - /* updating the interrupt structure to local structure */ - struct bmi160_acc_high_g_int_cfg *high_g_int_cfg = &(int_config->int_type_cfg.acc_high_g_int); - - /* Enable the high-g interrupt */ - rslt = enable_high_g_int(high_g_int_cfg, dev); - if (rslt == BMI160_OK) - { - /* Configure Interrupt pins */ - rslt = set_intr_pin_config(int_config, dev); - if (rslt == BMI160_OK) - { - /* Map INT pin to high-g interrupt */ - rslt = map_feature_interrupt(int_config, dev); - if (rslt == BMI160_OK) - { - /* configure the data source - * for high-g interrupt*/ - rslt = config_high_g_data_src(high_g_int_cfg, dev); - if (rslt == BMI160_OK) - { - rslt = config_high_g_int_settg(high_g_int_cfg, dev); - } - } - } - } - } - - return rslt; -} - -/*! - * @brief This API configures the pins to fire the - * interrupt signal when it occurs. - */ -static int8_t set_intr_pin_config(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) -{ - int8_t rslt; - - /* configure the behavioural settings of interrupt pin */ - rslt = config_int_out_ctrl(int_config, dev); - if (rslt == BMI160_OK) - { - rslt = config_int_latch(int_config, dev); - } - - 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 bmi160_dev *dev) -{ - int8_t rslt; - - if ((dev == NULL) || (dev->read == NULL) || (dev->write == NULL) || (dev->delay_ms == NULL)) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - /* Device structure is fine */ - rslt = BMI160_OK; - } - - return rslt; -} - -/*! - * @brief This API sets the default configuration parameters of accel & gyro. - * Also maintain the previous state of configurations. - */ -static void default_param_settg(struct bmi160_dev *dev) -{ - /* Initializing accel and gyro params with - * default values */ - dev->accel_cfg.bw = BMI160_ACCEL_BW_NORMAL_AVG4; - dev->accel_cfg.odr = BMI160_ACCEL_ODR_100HZ; - dev->accel_cfg.power = BMI160_ACCEL_SUSPEND_MODE; - dev->accel_cfg.range = BMI160_ACCEL_RANGE_2G; - dev->gyro_cfg.bw = BMI160_GYRO_BW_NORMAL_MODE; - dev->gyro_cfg.odr = BMI160_GYRO_ODR_100HZ; - dev->gyro_cfg.power = BMI160_GYRO_SUSPEND_MODE; - dev->gyro_cfg.range = BMI160_GYRO_RANGE_2000_DPS; - - /* To maintain the previous state of accel configuration */ - dev->prev_accel_cfg = dev->accel_cfg; - - /* To maintain the previous state of gyro configuration */ - dev->prev_gyro_cfg = dev->gyro_cfg; -} - -/*! - * @brief This API set the accel configuration. - */ -static int8_t set_accel_conf(struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t data[2] = { 0 }; - - rslt = check_accel_config(data, dev); - if (rslt == BMI160_OK) - { - /* Write output data rate and bandwidth */ - rslt = bmi160_set_regs(BMI160_ACCEL_CONFIG_ADDR, &data[0], 1, dev); - if (rslt == BMI160_OK) - { - dev->prev_accel_cfg.odr = dev->accel_cfg.odr; - dev->prev_accel_cfg.bw = dev->accel_cfg.bw; - - /* write accel range */ - rslt = bmi160_set_regs(BMI160_ACCEL_RANGE_ADDR, &data[1], 1, dev); - if (rslt == BMI160_OK) - { - dev->prev_accel_cfg.range = dev->accel_cfg.range; - } - } - } - - return rslt; -} - -/*! - * @brief This API check the accel configuration. - */ -static int8_t check_accel_config(uint8_t *data, const struct bmi160_dev *dev) -{ - int8_t rslt; - - /* read accel Output data rate and bandwidth */ - rslt = bmi160_get_regs(BMI160_ACCEL_CONFIG_ADDR, data, 2, dev); - if (rslt == BMI160_OK) - { - rslt = process_accel_odr(&data[0], dev); - if (rslt == BMI160_OK) - { - rslt = process_accel_bw(&data[0], dev); - if (rslt == BMI160_OK) - { - rslt = process_accel_range(&data[1], dev); - } - } - } - - return rslt; -} - -/*! - * @brief This API process the accel odr. - */ -static int8_t process_accel_odr(uint8_t *data, const struct bmi160_dev *dev) -{ - int8_t rslt = 0; - uint8_t temp = 0; - uint8_t odr = 0; - - if (dev->accel_cfg.odr <= BMI160_ACCEL_ODR_MAX) - { - if (dev->accel_cfg.odr != dev->prev_accel_cfg.odr) - { - odr = (uint8_t)dev->accel_cfg.odr; - temp = *data & ~BMI160_ACCEL_ODR_MASK; - - /* Adding output data rate */ - *data = temp | (odr & BMI160_ACCEL_ODR_MASK); - } - } - else - { - rslt = BMI160_E_OUT_OF_RANGE; - } - - return rslt; -} - -/*! - * @brief This API process the accel bandwidth. - */ -static int8_t process_accel_bw(uint8_t *data, const struct bmi160_dev *dev) -{ - int8_t rslt = 0; - uint8_t temp = 0; - uint8_t bw = 0; - - if (dev->accel_cfg.bw <= BMI160_ACCEL_BW_MAX) - { - if (dev->accel_cfg.bw != dev->prev_accel_cfg.bw) - { - bw = (uint8_t)dev->accel_cfg.bw; - temp = *data & ~BMI160_ACCEL_BW_MASK; - - /* Adding bandwidth */ - *data = temp | ((bw << 4) & BMI160_ACCEL_ODR_MASK); - } - } - else - { - rslt = BMI160_E_OUT_OF_RANGE; - } - - return rslt; -} - -/*! - * @brief This API process the accel range. - */ -static int8_t process_accel_range(uint8_t *data, const struct bmi160_dev *dev) -{ - int8_t rslt = 0; - uint8_t temp = 0; - uint8_t range = 0; - - if (dev->accel_cfg.range <= BMI160_ACCEL_RANGE_MAX) - { - if (dev->accel_cfg.range != dev->prev_accel_cfg.range) - { - range = (uint8_t)dev->accel_cfg.range; - temp = *data & ~BMI160_ACCEL_RANGE_MASK; - - /* Adding range */ - *data = temp | (range & BMI160_ACCEL_RANGE_MASK); - } - } - else - { - rslt = BMI160_E_OUT_OF_RANGE; - } - - return rslt; -} - -/*! - * @brief This API checks the invalid settings for ODR & Bw for - * Accel and Gyro. - */ -static int8_t check_invalid_settg(const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t data = 0; - - /* read the error reg */ - rslt = bmi160_get_regs(BMI160_ERROR_REG_ADDR, &data, 1, dev); - data = data >> 1; - data = data & BMI160_ERR_REG_MASK; - if (data == 1) - { - rslt = BMI160_E_ACCEL_ODR_BW_INVALID; - } - else if (data == 2) - { - rslt = BMI160_E_GYRO_ODR_BW_INVALID; - } - else if (data == 3) - { - rslt = BMI160_E_LWP_PRE_FLTR_INT_INVALID; - } - else if (data == 7) - { - rslt = BMI160_E_LWP_PRE_FLTR_INVALID; - } - - return rslt; -} -static int8_t set_gyro_conf(struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t data[2] = { 0 }; - - rslt = check_gyro_config(data, dev); - if (rslt == BMI160_OK) - { - /* Write output data rate and bandwidth */ - rslt = bmi160_set_regs(BMI160_GYRO_CONFIG_ADDR, &data[0], 1, dev); - if (rslt == BMI160_OK) - { - dev->prev_gyro_cfg.odr = dev->gyro_cfg.odr; - dev->prev_gyro_cfg.bw = dev->gyro_cfg.bw; - - /* Write gyro range */ - rslt = bmi160_set_regs(BMI160_GYRO_RANGE_ADDR, &data[1], 1, dev); - if (rslt == BMI160_OK) - { - dev->prev_gyro_cfg.range = dev->gyro_cfg.range; - } - } - } - - return rslt; -} - -/*! - * @brief This API check the gyro configuration. - */ -static int8_t check_gyro_config(uint8_t *data, const struct bmi160_dev *dev) -{ - int8_t rslt; - - /* read gyro Output data rate and bandwidth */ - rslt = bmi160_get_regs(BMI160_GYRO_CONFIG_ADDR, data, 2, dev); - if (rslt == BMI160_OK) - { - rslt = process_gyro_odr(&data[0], dev); - if (rslt == BMI160_OK) - { - rslt = process_gyro_bw(&data[0], dev); - if (rslt == BMI160_OK) - { - rslt = process_gyro_range(&data[1], dev); - } - } - } - - return rslt; -} - -/*! - * @brief This API process the gyro odr. - */ -static int8_t process_gyro_odr(uint8_t *data, const struct bmi160_dev *dev) -{ - int8_t rslt = 0; - uint8_t temp = 0; - uint8_t odr = 0; - - if (dev->gyro_cfg.odr <= BMI160_GYRO_ODR_MAX) - { - if (dev->gyro_cfg.odr != dev->prev_gyro_cfg.odr) - { - odr = (uint8_t)dev->gyro_cfg.odr; - temp = (*data & ~BMI160_GYRO_ODR_MASK); - - /* Adding output data rate */ - *data = temp | (odr & BMI160_GYRO_ODR_MASK); - } - } - else - { - rslt = BMI160_E_OUT_OF_RANGE; - } - - return rslt; -} - -/*! - * @brief This API process the gyro bandwidth. - */ -static int8_t process_gyro_bw(uint8_t *data, const struct bmi160_dev *dev) -{ - int8_t rslt = 0; - uint8_t temp = 0; - uint8_t bw = 0; - - if (dev->gyro_cfg.bw <= BMI160_GYRO_BW_MAX) - { - bw = (uint8_t)dev->gyro_cfg.bw; - temp = *data & ~BMI160_GYRO_BW_MASK; - - /* Adding bandwidth */ - *data = temp | ((bw << 4) & BMI160_GYRO_BW_MASK); - } - else - { - rslt = BMI160_E_OUT_OF_RANGE; - } - - return rslt; -} - -/*! - * @brief This API process the gyro range. - */ -static int8_t process_gyro_range(uint8_t *data, const struct bmi160_dev *dev) -{ - int8_t rslt = 0; - uint8_t temp = 0; - uint8_t range = 0; - - if (dev->gyro_cfg.range <= BMI160_GYRO_RANGE_MAX) - { - if (dev->gyro_cfg.range != dev->prev_gyro_cfg.range) - { - range = (uint8_t)dev->gyro_cfg.range; - temp = *data & ~BMI160_GYRO_RANGE_MSK; - - /* Adding range */ - *data = temp | (range & BMI160_GYRO_RANGE_MSK); - } - } - else - { - rslt = BMI160_E_OUT_OF_RANGE; - } - - return rslt; -} - -/*! - * @brief This API sets the accel power. - */ -static int8_t set_accel_pwr(struct bmi160_dev *dev) -{ - int8_t rslt = 0; - uint8_t data = 0; - - if ((dev->accel_cfg.power >= BMI160_ACCEL_SUSPEND_MODE) && (dev->accel_cfg.power <= BMI160_ACCEL_LOWPOWER_MODE)) - { - if (dev->accel_cfg.power != dev->prev_accel_cfg.power) - { - rslt = process_under_sampling(&data, dev); - if (rslt == BMI160_OK) - { - /* Write accel power */ - rslt = bmi160_set_regs(BMI160_COMMAND_REG_ADDR, &dev->accel_cfg.power, 1, dev); - - /* Add delay of 3.8 ms - refer data sheet table 24*/ - if (dev->prev_accel_cfg.power == BMI160_ACCEL_SUSPEND_MODE) - { - dev->delay_ms(BMI160_ACCEL_DELAY_MS); - } - dev->prev_accel_cfg.power = dev->accel_cfg.power; - } - } - } - else - { - rslt = BMI160_E_OUT_OF_RANGE; - } - - return rslt; -} - -/*! - * @brief This API process the undersampling setting of Accel. - */ -static int8_t process_under_sampling(uint8_t *data, const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t temp = 0; - uint8_t pre_filter = 0; - - rslt = bmi160_get_regs(BMI160_ACCEL_CONFIG_ADDR, data, 1, dev); - if (rslt == BMI160_OK) - { - if (dev->accel_cfg.power == BMI160_ACCEL_LOWPOWER_MODE) - { - temp = *data & ~BMI160_ACCEL_UNDERSAMPLING_MASK; - - /* Set under-sampling parameter */ - *data = temp | ((1 << 7) & BMI160_ACCEL_UNDERSAMPLING_MASK); - - /* Write data */ - rslt = bmi160_set_regs(BMI160_ACCEL_CONFIG_ADDR, data, 1, dev); - - /* disable the pre-filter data in - * low power mode */ - if (rslt == BMI160_OK) - { - /* Disable the Pre-filter data*/ - rslt = bmi160_set_regs(BMI160_INT_DATA_0_ADDR, &pre_filter, 2, dev); - } - } - else - { - if (*data & BMI160_ACCEL_UNDERSAMPLING_MASK) - { - temp = *data & ~BMI160_ACCEL_UNDERSAMPLING_MASK; - - /* disable under-sampling parameter - * if already enabled */ - *data = temp; - - /* Write data */ - rslt = bmi160_set_regs(BMI160_ACCEL_CONFIG_ADDR, data, 1, dev); - } - } - } - - return rslt; -} - -/*! - * @brief This API sets the gyro power mode. - */ -static int8_t set_gyro_pwr(struct bmi160_dev *dev) -{ - int8_t rslt = 0; - - if ((dev->gyro_cfg.power == BMI160_GYRO_SUSPEND_MODE) || (dev->gyro_cfg.power == BMI160_GYRO_NORMAL_MODE) || - (dev->gyro_cfg.power == BMI160_GYRO_FASTSTARTUP_MODE)) - { - if (dev->gyro_cfg.power != dev->prev_gyro_cfg.power) - { - /* Write gyro power */ - rslt = bmi160_set_regs(BMI160_COMMAND_REG_ADDR, &dev->gyro_cfg.power, 1, dev); - if (dev->prev_gyro_cfg.power == BMI160_GYRO_SUSPEND_MODE) - { - /* Delay of 80 ms - datasheet Table 24 */ - dev->delay_ms(BMI160_GYRO_DELAY_MS); - } - else if ((dev->prev_gyro_cfg.power == BMI160_GYRO_FASTSTARTUP_MODE) && - (dev->gyro_cfg.power == BMI160_GYRO_NORMAL_MODE)) - { - /* This delay is required for transition from - * fast-startup mode to normal mode - datasheet Table 3 */ - dev->delay_ms(10); - } - else - { - /* do nothing */ - } - dev->prev_gyro_cfg.power = dev->gyro_cfg.power; - } - } - else - { - rslt = BMI160_E_OUT_OF_RANGE; - } - - return rslt; -} - -/*! - * @brief This API reads accel data along with sensor time if time is requested - * by user. Kindly refer the user guide(README.md) for more info. - */ -static int8_t get_accel_data(uint8_t len, struct bmi160_sensor_data *accel, const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t idx = 0; - uint8_t data_array[9] = { 0 }; - uint8_t time_0 = 0; - uint16_t time_1 = 0; - uint32_t time_2 = 0; - uint8_t lsb; - uint8_t msb; - int16_t msblsb; - - /* read accel sensor data along with time if requested */ - rslt = bmi160_get_regs(BMI160_ACCEL_DATA_ADDR, data_array, 6 + len, dev); - if (rslt == BMI160_OK) - { - /* Accel Data */ - lsb = data_array[idx++]; - msb = data_array[idx++]; - msblsb = (int16_t)((msb << 8) | lsb); - accel->x = msblsb; /* Data in X axis */ - lsb = data_array[idx++]; - msb = data_array[idx++]; - msblsb = (int16_t)((msb << 8) | lsb); - accel->y = msblsb; /* Data in Y axis */ - lsb = data_array[idx++]; - msb = data_array[idx++]; - msblsb = (int16_t)((msb << 8) | lsb); - accel->z = msblsb; /* Data in Z axis */ - if (len == 3) - { - time_0 = data_array[idx++]; - time_1 = (uint16_t)(data_array[idx++] << 8); - time_2 = (uint32_t)(data_array[idx++] << 16); - accel->sensortime = (uint32_t)(time_2 | time_1 | time_0); - } - else - { - accel->sensortime = 0; - } - } - else - { - rslt = BMI160_E_COM_FAIL; - } - - return rslt; -} - -/*! - * @brief This API reads accel data along with sensor time if time is requested - * by user. Kindly refer the user guide(README.md) for more info. - */ -static int8_t get_gyro_data(uint8_t len, struct bmi160_sensor_data *gyro, const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t idx = 0; - uint8_t data_array[15] = { 0 }; - uint8_t time_0 = 0; - uint16_t time_1 = 0; - uint32_t time_2 = 0; - uint8_t lsb; - uint8_t msb; - int16_t msblsb; - - if (len == 0) - { - /* read gyro data only */ - rslt = bmi160_get_regs(BMI160_GYRO_DATA_ADDR, data_array, 6, dev); - if (rslt == BMI160_OK) - { - /* Gyro Data */ - lsb = data_array[idx++]; - msb = data_array[idx++]; - msblsb = (int16_t)((msb << 8) | lsb); - gyro->x = msblsb; /* Data in X axis */ - lsb = data_array[idx++]; - msb = data_array[idx++]; - msblsb = (int16_t)((msb << 8) | lsb); - gyro->y = msblsb; /* Data in Y axis */ - lsb = data_array[idx++]; - msb = data_array[idx++]; - msblsb = (int16_t)((msb << 8) | lsb); - gyro->z = msblsb; /* Data in Z axis */ - gyro->sensortime = 0; - } - else - { - rslt = BMI160_E_COM_FAIL; - } - } - else - { - /* read gyro sensor data along with time */ - rslt = bmi160_get_regs(BMI160_GYRO_DATA_ADDR, data_array, 12 + len, dev); - if (rslt == BMI160_OK) - { - /* Gyro Data */ - lsb = data_array[idx++]; - msb = data_array[idx++]; - msblsb = (int16_t)((msb << 8) | lsb); - gyro->x = msblsb; /* gyro X axis data */ - lsb = data_array[idx++]; - msb = data_array[idx++]; - msblsb = (int16_t)((msb << 8) | lsb); - gyro->y = msblsb; /* gyro Y axis data */ - lsb = data_array[idx++]; - msb = data_array[idx++]; - msblsb = (int16_t)((msb << 8) | lsb); - gyro->z = msblsb; /* gyro Z axis data */ - idx = idx + 6; - time_0 = data_array[idx++]; - time_1 = (uint16_t)(data_array[idx++] << 8); - time_2 = (uint32_t)(data_array[idx++] << 16); - gyro->sensortime = (uint32_t)(time_2 | time_1 | time_0); - } - else - { - rslt = BMI160_E_COM_FAIL; - } - } - - return rslt; -} - -/*! - * @brief This API reads accel and gyro data along with sensor time - * if time is requested by user. - * Kindly refer the user guide(README.md) for more info. - */ -static int8_t get_accel_gyro_data(uint8_t len, - struct bmi160_sensor_data *accel, - struct bmi160_sensor_data *gyro, - const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t idx = 0; - uint8_t data_array[15] = { 0 }; - uint8_t time_0 = 0; - uint16_t time_1 = 0; - uint32_t time_2 = 0; - uint8_t lsb; - uint8_t msb; - int16_t msblsb; - - /* read both accel and gyro sensor data - * along with time if requested */ - rslt = bmi160_get_regs(BMI160_GYRO_DATA_ADDR, data_array, 12 + len, dev); - if (rslt == BMI160_OK) - { - /* Gyro Data */ - lsb = data_array[idx++]; - msb = data_array[idx++]; - msblsb = (int16_t)((msb << 8) | lsb); - gyro->x = msblsb; /* gyro X axis data */ - lsb = data_array[idx++]; - msb = data_array[idx++]; - msblsb = (int16_t)((msb << 8) | lsb); - gyro->y = msblsb; /* gyro Y axis data */ - lsb = data_array[idx++]; - msb = data_array[idx++]; - msblsb = (int16_t)((msb << 8) | lsb); - gyro->z = msblsb; /* gyro Z axis data */ - /* Accel Data */ - lsb = data_array[idx++]; - msb = data_array[idx++]; - msblsb = (int16_t)((msb << 8) | lsb); - accel->x = (int16_t)msblsb; /* accel X axis data */ - lsb = data_array[idx++]; - msb = data_array[idx++]; - msblsb = (int16_t)((msb << 8) | lsb); - accel->y = (int16_t)msblsb; /* accel Y axis data */ - lsb = data_array[idx++]; - msb = data_array[idx++]; - msblsb = (int16_t)((msb << 8) | lsb); - accel->z = (int16_t)msblsb; /* accel Z axis data */ - if (len == 3) - { - time_0 = data_array[idx++]; - time_1 = (uint16_t)(data_array[idx++] << 8); - time_2 = (uint32_t)(data_array[idx++] << 16); - accel->sensortime = (uint32_t)(time_2 | time_1 | time_0); - gyro->sensortime = (uint32_t)(time_2 | time_1 | time_0); - } - else - { - accel->sensortime = 0; - gyro->sensortime = 0; - } - } - else - { - rslt = BMI160_E_COM_FAIL; - } - - return rslt; -} - -/*! - * @brief This API enables the any-motion interrupt for accel. - */ -static int8_t enable_accel_any_motion_int(const struct bmi160_acc_any_mot_int_cfg *any_motion_int_cfg, - struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t data = 0; - uint8_t temp = 0; - - /* Enable any motion x, any motion y, any motion z - * in Int Enable 0 register */ - rslt = bmi160_get_regs(BMI160_INT_ENABLE_0_ADDR, &data, 1, dev); - if (rslt == BMI160_OK) - { - if (any_motion_int_cfg->anymotion_en == BMI160_ENABLE) - { - temp = data & ~BMI160_ANY_MOTION_X_INT_EN_MASK; - - /* Adding Any_motion x axis */ - data = temp | (any_motion_int_cfg->anymotion_x & BMI160_ANY_MOTION_X_INT_EN_MASK); - temp = data & ~BMI160_ANY_MOTION_Y_INT_EN_MASK; - - /* Adding Any_motion y axis */ - data = temp | ((any_motion_int_cfg->anymotion_y << 1) & BMI160_ANY_MOTION_Y_INT_EN_MASK); - temp = data & ~BMI160_ANY_MOTION_Z_INT_EN_MASK; - - /* Adding Any_motion z axis */ - data = temp | ((any_motion_int_cfg->anymotion_z << 2) & BMI160_ANY_MOTION_Z_INT_EN_MASK); - - /* any-motion feature selected*/ - dev->any_sig_sel = BMI160_ANY_MOTION_ENABLED; - } - else - { - data = data & ~BMI160_ANY_MOTION_ALL_INT_EN_MASK; - - /* neither any-motion feature nor sig-motion selected */ - dev->any_sig_sel = BMI160_BOTH_ANY_SIG_MOTION_DISABLED; - } - - /* write data to Int Enable 0 register */ - rslt = bmi160_set_regs(BMI160_INT_ENABLE_0_ADDR, &data, 1, dev); - } - - return rslt; -} - -/*! - * @brief This API disable the sig-motion interrupt. - */ -static int8_t disable_sig_motion_int(const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t data = 0; - uint8_t temp = 0; - - /* Disabling Significant motion interrupt if enabled */ - rslt = bmi160_get_regs(BMI160_INT_MOTION_3_ADDR, &data, 1, dev); - if (rslt == BMI160_OK) - { - temp = (data & BMI160_SIG_MOTION_SEL_MASK); - if (temp) - { - temp = data & ~BMI160_SIG_MOTION_SEL_MASK; - data = temp; - - /* Write data to register */ - rslt = bmi160_set_regs(BMI160_INT_MOTION_3_ADDR, &data, 1, dev); - } - } - - return rslt; -} - -/*! - * @brief This API is used to map/unmap the Any/Sig motion, Step det/Low-g, - * Double tap, Single tap, Orientation, Flat, High-G, Nomotion interrupt pins. - */ -static int8_t map_feature_interrupt(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t data[3] = { 0, 0, 0 }; - uint8_t temp[3] = { 0, 0, 0 }; - - rslt = bmi160_get_regs(BMI160_INT_MAP_0_ADDR, data, 3, dev); - if (rslt == BMI160_OK) - { - temp[0] = data[0] & ~int_mask_lookup_table[int_config->int_type]; - temp[2] = data[2] & ~int_mask_lookup_table[int_config->int_type]; - switch (int_config->int_channel) - { - case BMI160_INT_CHANNEL_NONE: - data[0] = temp[0]; - data[2] = temp[2]; - break; - case BMI160_INT_CHANNEL_1: - data[0] = temp[0] | int_mask_lookup_table[int_config->int_type]; - data[2] = temp[2]; - break; - case BMI160_INT_CHANNEL_2: - data[2] = temp[2] | int_mask_lookup_table[int_config->int_type]; - data[0] = temp[0]; - break; - case BMI160_INT_CHANNEL_BOTH: - data[0] = temp[0] | int_mask_lookup_table[int_config->int_type]; - data[2] = temp[2] | int_mask_lookup_table[int_config->int_type]; - break; - default: - rslt = BMI160_E_OUT_OF_RANGE; - } - if (rslt == BMI160_OK) - { - rslt = bmi160_set_regs(BMI160_INT_MAP_0_ADDR, data, 3, dev); - } - } - - return rslt; -} - -/*! - * @brief This API is used to map/unmap the Dataready(Accel & Gyro), FIFO full - * and FIFO watermark interrupt. - */ -static int8_t map_hardware_interrupt(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t data = 0; - uint8_t temp = 0; - - rslt = bmi160_get_regs(BMI160_INT_MAP_1_ADDR, &data, 1, dev); - if (rslt == BMI160_OK) - { - temp = data & ~int_mask_lookup_table[int_config->int_type]; - temp = temp & ~((uint8_t)(int_mask_lookup_table[int_config->int_type] << 4)); - switch (int_config->int_channel) - { - case BMI160_INT_CHANNEL_NONE: - data = temp; - break; - case BMI160_INT_CHANNEL_1: - data = temp | (uint8_t)((int_mask_lookup_table[int_config->int_type]) << 4); - break; - case BMI160_INT_CHANNEL_2: - data = temp | int_mask_lookup_table[int_config->int_type]; - break; - case BMI160_INT_CHANNEL_BOTH: - data = temp | int_mask_lookup_table[int_config->int_type]; - data = data | (uint8_t)((int_mask_lookup_table[int_config->int_type]) << 4); - break; - default: - rslt = BMI160_E_OUT_OF_RANGE; - } - if (rslt == BMI160_OK) - { - rslt = bmi160_set_regs(BMI160_INT_MAP_1_ADDR, &data, 1, dev); - } - } - - return rslt; -} - -/*! - * @brief This API configure the source of data(filter & pre-filter) - * for any-motion interrupt. - */ -static int8_t config_any_motion_src(const struct bmi160_acc_any_mot_int_cfg *any_motion_int_cfg, - const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t data = 0; - uint8_t temp = 0; - - /* Configure Int data 1 register to add source of interrupt */ - rslt = bmi160_get_regs(BMI160_INT_DATA_1_ADDR, &data, 1, dev); - if (rslt == BMI160_OK) - { - temp = data & ~BMI160_MOTION_SRC_INT_MASK; - data = temp | ((any_motion_int_cfg->anymotion_data_src << 7) & BMI160_MOTION_SRC_INT_MASK); - - /* Write data to DATA 1 address */ - rslt = bmi160_set_regs(BMI160_INT_DATA_1_ADDR, &data, 1, dev); - } - - return rslt; -} - -/*! - * @brief This API configure the duration and threshold of - * any-motion interrupt. - */ -static int8_t config_any_dur_threshold(const struct bmi160_acc_any_mot_int_cfg *any_motion_int_cfg, - const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t data = 0; - uint8_t temp = 0; - uint8_t data_array[2] = { 0 }; - uint8_t dur; - - /* Configure Int Motion 0 register */ - rslt = bmi160_get_regs(BMI160_INT_MOTION_0_ADDR, &data, 1, dev); - if (rslt == BMI160_OK) - { - /* slope duration */ - dur = (uint8_t)any_motion_int_cfg->anymotion_dur; - temp = data & ~BMI160_SLOPE_INT_DUR_MASK; - data = temp | (dur & BMI160_MOTION_SRC_INT_MASK); - data_array[0] = data; - - /* add slope threshold */ - data_array[1] = any_motion_int_cfg->anymotion_thr; - - /* INT MOTION 0 and INT MOTION 1 address lie consecutively, - * hence writing data to respective registers at one go */ - - /* Writing to Int_motion 0 and - * Int_motion 1 Address simultaneously */ - rslt = bmi160_set_regs(BMI160_INT_MOTION_0_ADDR, data_array, 2, dev); - } - - return rslt; -} - -/*! - * @brief This API configure necessary setting of any-motion interrupt. - */ -static int8_t config_any_motion_int_settg(const struct bmi160_int_settg *int_config, - const struct bmi160_acc_any_mot_int_cfg *any_motion_int_cfg, - const struct bmi160_dev *dev) -{ - int8_t rslt; - - /* Configure Interrupt pins */ - rslt = set_intr_pin_config(int_config, dev); - if (rslt == BMI160_OK) - { - rslt = disable_sig_motion_int(dev); - if (rslt == BMI160_OK) - { - rslt = map_feature_interrupt(int_config, dev); - if (rslt == BMI160_OK) - { - rslt = config_any_motion_src(any_motion_int_cfg, dev); - if (rslt == BMI160_OK) - { - rslt = config_any_dur_threshold(any_motion_int_cfg, dev); - } - } - } - } - - return rslt; -} - -/*! - * @brief This API enable the data ready interrupt. - */ -static int8_t enable_data_ready_int(const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t data = 0; - uint8_t temp = 0; - - /* Enable data ready interrupt in Int Enable 1 register */ - rslt = bmi160_get_regs(BMI160_INT_ENABLE_1_ADDR, &data, 1, dev); - if (rslt == BMI160_OK) - { - temp = data & ~BMI160_DATA_RDY_INT_EN_MASK; - data = temp | ((1 << 4) & BMI160_DATA_RDY_INT_EN_MASK); - - /* Writing data to INT ENABLE 1 Address */ - rslt = bmi160_set_regs(BMI160_INT_ENABLE_1_ADDR, &data, 1, dev); - } - - return rslt; -} - -/*! - * @brief This API enables the no motion/slow motion interrupt. - */ -static int8_t enable_no_motion_int(const struct bmi160_acc_no_motion_int_cfg *no_mot_int_cfg, - const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t data = 0; - uint8_t temp = 0; - - /* Enable no motion x, no motion y, no motion z - * in Int Enable 2 register */ - rslt = bmi160_get_regs(BMI160_INT_ENABLE_2_ADDR, &data, 1, dev); - if (rslt == BMI160_OK) - { - if (no_mot_int_cfg->no_motion_x == 1) - { - temp = data & ~BMI160_NO_MOTION_X_INT_EN_MASK; - - /* Adding No_motion x axis */ - data = temp | (1 & BMI160_NO_MOTION_X_INT_EN_MASK); - } - if (no_mot_int_cfg->no_motion_y == 1) - { - temp = data & ~BMI160_NO_MOTION_Y_INT_EN_MASK; - - /* Adding No_motion x axis */ - data = temp | ((1 << 1) & BMI160_NO_MOTION_Y_INT_EN_MASK); - } - if (no_mot_int_cfg->no_motion_z == 1) - { - temp = data & ~BMI160_NO_MOTION_Z_INT_EN_MASK; - - /* Adding No_motion x axis */ - data = temp | ((1 << 2) & BMI160_NO_MOTION_Z_INT_EN_MASK); - } - - /* write data to Int Enable 2 register */ - rslt = bmi160_set_regs(BMI160_INT_ENABLE_2_ADDR, &data, 1, dev); - } - - return rslt; -} - -/*! - * @brief This API configure the interrupt PIN setting for - * no motion/slow motion interrupt. - */ -static int8_t config_no_motion_int_settg(const struct bmi160_int_settg *int_config, - const struct bmi160_acc_no_motion_int_cfg *no_mot_int_cfg, - const struct bmi160_dev *dev) -{ - int8_t rslt; - - /* Configure Interrupt pins */ - rslt = set_intr_pin_config(int_config, dev); - if (rslt == BMI160_OK) - { - rslt = map_feature_interrupt(int_config, dev); - if (rslt == BMI160_OK) - { - rslt = config_no_motion_data_src(no_mot_int_cfg, dev); - if (rslt == BMI160_OK) - { - rslt = config_no_motion_dur_thr(no_mot_int_cfg, dev); - } - } - } - - return rslt; -} - -/*! - * @brief This API configure the source of interrupt for no motion. - */ -static int8_t config_no_motion_data_src(const struct bmi160_acc_no_motion_int_cfg *no_mot_int_cfg, - const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t data = 0; - uint8_t temp = 0; - - /* Configure Int data 1 register to add source of interrupt */ - rslt = bmi160_get_regs(BMI160_INT_DATA_1_ADDR, &data, 1, dev); - if (rslt == BMI160_OK) - { - temp = data & ~BMI160_MOTION_SRC_INT_MASK; - data = temp | ((no_mot_int_cfg->no_motion_src << 7) & BMI160_MOTION_SRC_INT_MASK); - - /* Write data to DATA 1 address */ - rslt = bmi160_set_regs(BMI160_INT_DATA_1_ADDR, &data, 1, dev); - } - - return rslt; -} - -/*! - * @brief This API configure the duration and threshold of - * no motion/slow motion interrupt along with selection of no/slow motion. - */ -static int8_t config_no_motion_dur_thr(const struct bmi160_acc_no_motion_int_cfg *no_mot_int_cfg, - const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t data = 0; - uint8_t temp = 0; - uint8_t temp_1 = 0; - uint8_t reg_addr; - uint8_t data_array[2] = { 0 }; - - /* Configuring INT_MOTION register */ - reg_addr = BMI160_INT_MOTION_0_ADDR; - rslt = bmi160_get_regs(reg_addr, &data, 1, dev); - if (rslt == BMI160_OK) - { - temp = data & ~BMI160_NO_MOTION_INT_DUR_MASK; - - /* Adding no_motion duration */ - data = temp | ((no_mot_int_cfg->no_motion_dur << 2) & BMI160_NO_MOTION_INT_DUR_MASK); - - /* Write data to NO_MOTION 0 address */ - rslt = bmi160_set_regs(reg_addr, &data, 1, dev); - if (rslt == BMI160_OK) - { - reg_addr = BMI160_INT_MOTION_3_ADDR; - rslt = bmi160_get_regs(reg_addr, &data, 1, dev); - if (rslt == BMI160_OK) - { - temp = data & ~BMI160_NO_MOTION_SEL_BIT_MASK; - - /* Adding no_motion_sel bit */ - temp_1 = (no_mot_int_cfg->no_motion_sel & BMI160_NO_MOTION_SEL_BIT_MASK); - data = (temp | temp_1); - data_array[1] = data; - - /* Adding no motion threshold */ - data_array[0] = no_mot_int_cfg->no_motion_thres; - reg_addr = BMI160_INT_MOTION_2_ADDR; - - /* writing data to INT_MOTION 2 and INT_MOTION 3 - * address simultaneously */ - rslt = bmi160_set_regs(reg_addr, data_array, 2, dev); - } - } - } - - return rslt; -} - -/*! - * @brief This API enables the sig-motion motion interrupt. - */ -static int8_t enable_sig_motion_int(const struct bmi160_acc_sig_mot_int_cfg *sig_mot_int_cfg, struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t data = 0; - uint8_t temp = 0; - - /* For significant motion,enable any motion x,any motion y, - * any motion z in Int Enable 0 register */ - rslt = bmi160_get_regs(BMI160_INT_ENABLE_0_ADDR, &data, 1, dev); - if (rslt == BMI160_OK) - { - if (sig_mot_int_cfg->sig_en == BMI160_ENABLE) - { - temp = data & ~BMI160_SIG_MOTION_INT_EN_MASK; - data = temp | (7 & BMI160_SIG_MOTION_INT_EN_MASK); - - /* sig-motion feature selected*/ - dev->any_sig_sel = BMI160_SIG_MOTION_ENABLED; - } - else - { - data = data & ~BMI160_SIG_MOTION_INT_EN_MASK; - - /* neither any-motion feature nor sig-motion selected */ - dev->any_sig_sel = BMI160_BOTH_ANY_SIG_MOTION_DISABLED; - } - - /* write data to Int Enable 0 register */ - rslt = bmi160_set_regs(BMI160_INT_ENABLE_0_ADDR, &data, 1, dev); - } - - return rslt; -} - -/*! - * @brief This API configure the interrupt PIN setting for - * significant motion interrupt. - */ -static int8_t config_sig_motion_int_settg(const struct bmi160_int_settg *int_config, - const struct bmi160_acc_sig_mot_int_cfg *sig_mot_int_cfg, - const struct bmi160_dev *dev) -{ - int8_t rslt; - - /* Configure Interrupt pins */ - rslt = set_intr_pin_config(int_config, dev); - if (rslt == BMI160_OK) - { - rslt = map_feature_interrupt(int_config, dev); - if (rslt == BMI160_OK) - { - rslt = config_sig_motion_data_src(sig_mot_int_cfg, dev); - if (rslt == BMI160_OK) - { - rslt = config_sig_dur_threshold(sig_mot_int_cfg, dev); - } - } - } - - return rslt; -} - -/*! - * @brief This API configure the source of data(filter & pre-filter) - * for sig motion interrupt. - */ -static int8_t config_sig_motion_data_src(const struct bmi160_acc_sig_mot_int_cfg *sig_mot_int_cfg, - const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t data = 0; - uint8_t temp = 0; - - /* Configure Int data 1 register to add source of interrupt */ - rslt = bmi160_get_regs(BMI160_INT_DATA_1_ADDR, &data, 1, dev); - if (rslt == BMI160_OK) - { - temp = data & ~BMI160_MOTION_SRC_INT_MASK; - data = temp | ((sig_mot_int_cfg->sig_data_src << 7) & BMI160_MOTION_SRC_INT_MASK); - - /* Write data to DATA 1 address */ - rslt = bmi160_set_regs(BMI160_INT_DATA_1_ADDR, &data, 1, dev); - } - - return rslt; -} - -/*! - * @brief This API configure the threshold, skip and proof time of - * sig motion interrupt. - */ -static int8_t config_sig_dur_threshold(const struct bmi160_acc_sig_mot_int_cfg *sig_mot_int_cfg, - const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t data; - uint8_t temp = 0; - - /* Configuring INT_MOTION registers */ - - /* Write significant motion threshold. - * This threshold is same as any motion threshold */ - data = sig_mot_int_cfg->sig_mot_thres; - - /* Write data to INT_MOTION 1 address */ - rslt = bmi160_set_regs(BMI160_INT_MOTION_1_ADDR, &data, 1, dev); - if (rslt == BMI160_OK) - { - rslt = bmi160_get_regs(BMI160_INT_MOTION_3_ADDR, &data, 1, dev); - if (rslt == BMI160_OK) - { - temp = data & ~BMI160_SIG_MOTION_SKIP_MASK; - - /* adding skip time of sig_motion interrupt*/ - data = temp | ((sig_mot_int_cfg->sig_mot_skip << 2) & BMI160_SIG_MOTION_SKIP_MASK); - temp = data & ~BMI160_SIG_MOTION_PROOF_MASK; - - /* adding proof time of sig_motion interrupt */ - data = temp | ((sig_mot_int_cfg->sig_mot_proof << 4) & BMI160_SIG_MOTION_PROOF_MASK); - - /* configure the int_sig_mot_sel bit to select - * significant motion interrupt */ - temp = data & ~BMI160_SIG_MOTION_SEL_MASK; - data = temp | ((sig_mot_int_cfg->sig_en << 1) & BMI160_SIG_MOTION_SEL_MASK); - rslt = bmi160_set_regs(BMI160_INT_MOTION_3_ADDR, &data, 1, dev); - } - } - - return rslt; -} - -/*! - * @brief This API enables the step detector interrupt. - */ -static int8_t enable_step_detect_int(const struct bmi160_acc_step_detect_int_cfg *step_detect_int_cfg, - const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t data = 0; - uint8_t temp = 0; - - /* Enable data ready interrupt in Int Enable 2 register */ - rslt = bmi160_get_regs(BMI160_INT_ENABLE_2_ADDR, &data, 1, dev); - if (rslt == BMI160_OK) - { - temp = data & ~BMI160_STEP_DETECT_INT_EN_MASK; - data = temp | ((step_detect_int_cfg->step_detector_en << 3) & BMI160_STEP_DETECT_INT_EN_MASK); - - /* Writing data to INT ENABLE 2 Address */ - rslt = bmi160_set_regs(BMI160_INT_ENABLE_2_ADDR, &data, 1, dev); - } - - return rslt; -} - -/*! - * @brief This API configure the step detector parameter. - */ -static int8_t config_step_detect(const struct bmi160_acc_step_detect_int_cfg *step_detect_int_cfg, - const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t temp = 0; - uint8_t data_array[2] = { 0 }; - - if (step_detect_int_cfg->step_detector_mode == BMI160_STEP_DETECT_NORMAL) - { - /* Normal mode setting */ - data_array[0] = 0x15; - data_array[1] = 0x03; - } - else if (step_detect_int_cfg->step_detector_mode == BMI160_STEP_DETECT_SENSITIVE) - { - /* Sensitive mode setting */ - data_array[0] = 0x2D; - data_array[1] = 0x00; - } - else if (step_detect_int_cfg->step_detector_mode == BMI160_STEP_DETECT_ROBUST) - { - /* Robust mode setting */ - data_array[0] = 0x1D; - data_array[1] = 0x07; - } - else if (step_detect_int_cfg->step_detector_mode == BMI160_STEP_DETECT_USER_DEFINE) - { - /* Non recommended User defined setting */ - /* Configuring STEP_CONFIG register */ - rslt = bmi160_get_regs(BMI160_INT_STEP_CONFIG_0_ADDR, &data_array[0], 2, dev); - if (rslt == BMI160_OK) - { - temp = data_array[0] & ~BMI160_STEP_DETECT_MIN_THRES_MASK; - - /* Adding min_threshold */ - data_array[0] = temp | ((step_detect_int_cfg->min_threshold << 3) & BMI160_STEP_DETECT_MIN_THRES_MASK); - temp = data_array[0] & ~BMI160_STEP_DETECT_STEPTIME_MIN_MASK; - - /* Adding steptime_min */ - data_array[0] = temp | ((step_detect_int_cfg->steptime_min) & BMI160_STEP_DETECT_STEPTIME_MIN_MASK); - temp = data_array[1] & ~BMI160_STEP_MIN_BUF_MASK; - - /* Adding steptime_min */ - data_array[1] = temp | ((step_detect_int_cfg->step_min_buf) & BMI160_STEP_MIN_BUF_MASK); - } - } - - /* Write data to STEP_CONFIG register */ - rslt = bmi160_set_regs(BMI160_INT_STEP_CONFIG_0_ADDR, data_array, 2, dev); - - return rslt; -} - -/*! - * @brief This API enables the single/double tap interrupt. - */ -static int8_t enable_tap_int(const struct bmi160_int_settg *int_config, - const struct bmi160_acc_tap_int_cfg *tap_int_cfg, - const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t data = 0; - uint8_t temp = 0; - - /* Enable single tap or double tap interrupt in Int Enable 0 register */ - rslt = bmi160_get_regs(BMI160_INT_ENABLE_0_ADDR, &data, 1, dev); - if (rslt == BMI160_OK) - { - if (int_config->int_type == BMI160_ACC_SINGLE_TAP_INT) - { - temp = data & ~BMI160_SINGLE_TAP_INT_EN_MASK; - data = temp | ((tap_int_cfg->tap_en << 5) & BMI160_SINGLE_TAP_INT_EN_MASK); - } - else - { - temp = data & ~BMI160_DOUBLE_TAP_INT_EN_MASK; - data = temp | ((tap_int_cfg->tap_en << 4) & BMI160_DOUBLE_TAP_INT_EN_MASK); - } - - /* Write to Enable 0 Address */ - rslt = bmi160_set_regs(BMI160_INT_ENABLE_0_ADDR, &data, 1, dev); - } - - return rslt; -} - -/*! - * @brief This API configure the interrupt PIN setting for - * tap interrupt. - */ -static int8_t config_tap_int_settg(const struct bmi160_int_settg *int_config, - const struct bmi160_acc_tap_int_cfg *tap_int_cfg, - const struct bmi160_dev *dev) -{ - int8_t rslt; - - /* Configure Interrupt pins */ - rslt = set_intr_pin_config(int_config, dev); - if (rslt == BMI160_OK) - { - rslt = map_feature_interrupt(int_config, dev); - if (rslt == BMI160_OK) - { - rslt = config_tap_data_src(tap_int_cfg, dev); - if (rslt == BMI160_OK) - { - rslt = config_tap_param(int_config, tap_int_cfg, dev); - } - } - } - - return rslt; -} - -/*! - * @brief This API configure the source of data(filter & pre-filter) - * for tap interrupt. - */ -static int8_t config_tap_data_src(const struct bmi160_acc_tap_int_cfg *tap_int_cfg, const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t data = 0; - uint8_t temp = 0; - - /* Configure Int data 0 register to add source of interrupt */ - rslt = bmi160_get_regs(BMI160_INT_DATA_0_ADDR, &data, 1, dev); - if (rslt == BMI160_OK) - { - temp = data & ~BMI160_TAP_SRC_INT_MASK; - data = temp | ((tap_int_cfg->tap_data_src << 3) & BMI160_TAP_SRC_INT_MASK); - - /* Write data to Data 0 address */ - rslt = bmi160_set_regs(BMI160_INT_DATA_0_ADDR, &data, 1, dev); - } - - return rslt; -} - -/*! - * @brief This API configure the parameters of tap interrupt. - * Threshold, quite, shock, and duration. - */ -static int8_t config_tap_param(const struct bmi160_int_settg *int_config, - const struct bmi160_acc_tap_int_cfg *tap_int_cfg, - const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t temp = 0; - uint8_t data = 0; - uint8_t data_array[2] = { 0 }; - uint8_t count = 0; - uint8_t dur, shock, quiet, thres; - - /* Configure tap 0 register for tap shock,tap quiet duration - * in case of single tap interrupt */ - rslt = bmi160_get_regs(BMI160_INT_TAP_0_ADDR, data_array, 2, dev); - if (rslt == BMI160_OK) - { - data = data_array[count]; - if (int_config->int_type == BMI160_ACC_DOUBLE_TAP_INT) - { - dur = (uint8_t)tap_int_cfg->tap_dur; - temp = (data & ~BMI160_TAP_DUR_MASK); - - /* Add tap duration data in case of - * double tap interrupt */ - data = temp | (dur & BMI160_TAP_DUR_MASK); - } - shock = (uint8_t)tap_int_cfg->tap_shock; - temp = data & ~BMI160_TAP_SHOCK_DUR_MASK; - data = temp | ((shock << 6) & BMI160_TAP_SHOCK_DUR_MASK); - quiet = (uint8_t)tap_int_cfg->tap_quiet; - temp = data & ~BMI160_TAP_QUIET_DUR_MASK; - data = temp | ((quiet << 7) & BMI160_TAP_QUIET_DUR_MASK); - data_array[count++] = data; - data = data_array[count]; - thres = (uint8_t)tap_int_cfg->tap_thr; - temp = data & ~BMI160_TAP_THRES_MASK; - data = temp | (thres & BMI160_TAP_THRES_MASK); - data_array[count++] = data; - - /* TAP 0 and TAP 1 address lie consecutively, - * hence writing data to respective registers at one go */ - - /* Writing to Tap 0 and Tap 1 Address simultaneously */ - rslt = bmi160_set_regs(BMI160_INT_TAP_0_ADDR, data_array, count, dev); - } - - return rslt; -} - -/*! - * @brief This API configure the secondary interface. - */ -static int8_t config_sec_if(const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t if_conf = 0; - uint8_t cmd = BMI160_AUX_NORMAL_MODE; - - /* set the aux power mode to normal*/ - rslt = bmi160_set_regs(BMI160_COMMAND_REG_ADDR, &cmd, 1, dev); - if (rslt == BMI160_OK) - { - /* 0.5ms delay - refer datasheet table 24*/ - dev->delay_ms(1); - rslt = bmi160_get_regs(BMI160_IF_CONF_ADDR, &if_conf, 1, dev); - if_conf |= (uint8_t)(1 << 5); - if (rslt == BMI160_OK) - { - /*enable the secondary interface also*/ - rslt = bmi160_set_regs(BMI160_IF_CONF_ADDR, &if_conf, 1, dev); - } - } - - return rslt; -} - -/*! - * @brief This API configure the ODR of the auxiliary sensor. - */ -static int8_t config_aux_odr(const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t aux_odr; - - rslt = bmi160_get_regs(BMI160_AUX_ODR_ADDR, &aux_odr, 1, dev); - if (rslt == BMI160_OK) - { - aux_odr = (uint8_t)(dev->aux_cfg.aux_odr); - - /* Set the secondary interface ODR - * i.e polling rate of secondary sensor */ - rslt = bmi160_set_regs(BMI160_AUX_ODR_ADDR, &aux_odr, 1, dev); - dev->delay_ms(BMI160_AUX_COM_DELAY); - } - - return rslt; -} - -/*! - * @brief This API maps the actual burst read length set by user. - */ -static int8_t map_read_len(uint16_t *len, const struct bmi160_dev *dev) -{ - int8_t rslt = BMI160_OK; - - switch (dev->aux_cfg.aux_rd_burst_len) - { - case BMI160_AUX_READ_LEN_0: - *len = 1; - break; - case BMI160_AUX_READ_LEN_1: - *len = 2; - break; - case BMI160_AUX_READ_LEN_2: - *len = 6; - break; - case BMI160_AUX_READ_LEN_3: - *len = 8; - break; - default: - rslt = BMI160_E_INVALID_INPUT; - break; - } - - return rslt; -} - -/*! - * @brief This API configure the settings of auxiliary sensor. - */ -static int8_t config_aux_settg(const struct bmi160_dev *dev) -{ - int8_t rslt; - - rslt = config_sec_if(dev); - if (rslt == BMI160_OK) - { - /* Configures the auxiliary interface settings */ - rslt = bmi160_config_aux_mode(dev); - } - - return rslt; -} - -/*! - * @brief This API extract the read data from auxiliary sensor. - */ -static int8_t extract_aux_read(uint16_t map_len, - uint8_t reg_addr, - uint8_t *aux_data, - uint16_t len, - const struct bmi160_dev *dev) -{ - int8_t rslt = BMI160_OK; - uint8_t data[8] = { 0, }; - uint8_t read_addr = BMI160_AUX_DATA_ADDR; - uint8_t count = 0; - uint8_t read_count; - uint8_t read_len = (uint8_t)map_len; - - for (; count < len;) - { - /* set address to read */ - rslt = bmi160_set_regs(BMI160_AUX_IF_2_ADDR, ®_addr, 1, dev); - dev->delay_ms(BMI160_AUX_COM_DELAY); - if (rslt == BMI160_OK) - { - rslt = bmi160_get_regs(read_addr, data, map_len, dev); - if (rslt == BMI160_OK) - { - read_count = 0; - - /* if read len is less the burst read len - * mention by user*/ - if (len < map_len) - { - read_len = (uint8_t)len; - } - else - { - if ((len - count) < map_len) - { - read_len = (uint8_t)(len - count); - } - } - for (; read_count < read_len; read_count++) - { - aux_data[count + read_count] = data[read_count]; - } - reg_addr += (uint8_t)map_len; - count += (uint8_t)map_len; - } - else - { - rslt = BMI160_E_COM_FAIL; - break; - } - } - } - - return rslt; -} - -/*! - * @brief This API enables the orient interrupt. - */ -static int8_t enable_orient_int(const struct bmi160_acc_orient_int_cfg *orient_int_cfg, const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t data = 0; - uint8_t temp = 0; - - /* Enable data ready interrupt in Int Enable 0 register */ - rslt = bmi160_get_regs(BMI160_INT_ENABLE_0_ADDR, &data, 1, dev); - if (rslt == BMI160_OK) - { - temp = data & ~BMI160_ORIENT_INT_EN_MASK; - data = temp | ((orient_int_cfg->orient_en << 6) & BMI160_ORIENT_INT_EN_MASK); - - /* write data to Int Enable 0 register */ - rslt = bmi160_set_regs(BMI160_INT_ENABLE_0_ADDR, &data, 1, dev); - } - - return rslt; -} - -/*! - * @brief This API configure the necessary setting of orientation interrupt. - */ -static int8_t config_orient_int_settg(const struct bmi160_acc_orient_int_cfg *orient_int_cfg, - const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t data = 0; - uint8_t temp = 0; - uint8_t data_array[2] = { 0, 0 }; - - /* Configuring INT_ORIENT registers */ - rslt = bmi160_get_regs(BMI160_INT_ORIENT_0_ADDR, data_array, 2, dev); - if (rslt == BMI160_OK) - { - data = data_array[0]; - temp = data & ~BMI160_ORIENT_MODE_MASK; - - /* Adding Orientation mode */ - data = temp | ((orient_int_cfg->orient_mode) & BMI160_ORIENT_MODE_MASK); - temp = data & ~BMI160_ORIENT_BLOCK_MASK; - - /* Adding Orientation blocking */ - data = temp | ((orient_int_cfg->orient_blocking << 2) & BMI160_ORIENT_BLOCK_MASK); - temp = data & ~BMI160_ORIENT_HYST_MASK; - - /* Adding Orientation hysteresis */ - data = temp | ((orient_int_cfg->orient_hyst << 4) & BMI160_ORIENT_HYST_MASK); - data_array[0] = data; - data = data_array[1]; - temp = data & ~BMI160_ORIENT_THETA_MASK; - - /* Adding Orientation threshold */ - data = temp | ((orient_int_cfg->orient_theta) & BMI160_ORIENT_THETA_MASK); - temp = data & ~BMI160_ORIENT_UD_ENABLE; - - /* Adding Orient_ud_en */ - data = temp | ((orient_int_cfg->orient_ud_en << 6) & BMI160_ORIENT_UD_ENABLE); - temp = data & ~BMI160_AXES_EN_MASK; - - /* Adding axes_en */ - data = temp | ((orient_int_cfg->axes_ex << 7) & BMI160_AXES_EN_MASK); - data_array[1] = data; - - /* Writing data to INT_ORIENT 0 and INT_ORIENT 1 - * registers simultaneously */ - rslt = bmi160_set_regs(BMI160_INT_ORIENT_0_ADDR, data_array, 2, dev); - } - - return rslt; -} - -/*! - * @brief This API enables the flat interrupt. - */ -static int8_t enable_flat_int(const struct bmi160_acc_flat_detect_int_cfg *flat_int, const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t data = 0; - uint8_t temp = 0; - - /* Enable flat interrupt in Int Enable 0 register */ - rslt = bmi160_get_regs(BMI160_INT_ENABLE_0_ADDR, &data, 1, dev); - if (rslt == BMI160_OK) - { - temp = data & ~BMI160_FLAT_INT_EN_MASK; - data = temp | ((flat_int->flat_en << 7) & BMI160_FLAT_INT_EN_MASK); - - /* write data to Int Enable 0 register */ - rslt = bmi160_set_regs(BMI160_INT_ENABLE_0_ADDR, &data, 1, dev); - } - - return rslt; -} - -/*! - * @brief This API configure the necessary setting of flat interrupt. - */ -static int8_t config_flat_int_settg(const struct bmi160_acc_flat_detect_int_cfg *flat_int, const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t data = 0; - uint8_t temp = 0; - uint8_t data_array[2] = { 0, 0 }; - - /* Configuring INT_FLAT register */ - rslt = bmi160_get_regs(BMI160_INT_FLAT_0_ADDR, data_array, 2, dev); - if (rslt == BMI160_OK) - { - data = data_array[0]; - temp = data & ~BMI160_FLAT_THRES_MASK; - - /* Adding flat theta */ - data = temp | ((flat_int->flat_theta) & BMI160_FLAT_THRES_MASK); - data_array[0] = data; - data = data_array[1]; - temp = data & ~BMI160_FLAT_HOLD_TIME_MASK; - - /* Adding flat hold time */ - data = temp | ((flat_int->flat_hold_time << 4) & BMI160_FLAT_HOLD_TIME_MASK); - temp = data & ~BMI160_FLAT_HYST_MASK; - - /* Adding flat hysteresis */ - data = temp | ((flat_int->flat_hy) & BMI160_FLAT_HYST_MASK); - data_array[1] = data; - - /* Writing data to INT_FLAT 0 and INT_FLAT 1 - * registers simultaneously */ - rslt = bmi160_set_regs(BMI160_INT_FLAT_0_ADDR, data_array, 2, dev); - } - - return rslt; -} - -/*! - * @brief This API enables the Low-g interrupt. - */ -static int8_t enable_low_g_int(const struct bmi160_acc_low_g_int_cfg *low_g_int, const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t data = 0; - uint8_t temp = 0; - - /* Enable low-g interrupt in Int Enable 1 register */ - rslt = bmi160_get_regs(BMI160_INT_ENABLE_1_ADDR, &data, 1, dev); - if (rslt == BMI160_OK) - { - temp = data & ~BMI160_LOW_G_INT_EN_MASK; - data = temp | ((low_g_int->low_en << 3) & BMI160_LOW_G_INT_EN_MASK); - - /* write data to Int Enable 0 register */ - rslt = bmi160_set_regs(BMI160_INT_ENABLE_1_ADDR, &data, 1, dev); - } - - return rslt; -} - -/*! - * @brief This API configure the source of data(filter & pre-filter) - * for low-g interrupt. - */ -static int8_t config_low_g_data_src(const struct bmi160_acc_low_g_int_cfg *low_g_int, const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t data = 0; - uint8_t temp = 0; - - /* Configure Int data 0 register to add source of interrupt */ - rslt = bmi160_get_regs(BMI160_INT_DATA_0_ADDR, &data, 1, dev); - if (rslt == BMI160_OK) - { - temp = data & ~BMI160_LOW_HIGH_SRC_INT_MASK; - data = temp | ((low_g_int->low_data_src << 7) & BMI160_LOW_HIGH_SRC_INT_MASK); - - /* Write data to Data 0 address */ - rslt = bmi160_set_regs(BMI160_INT_DATA_0_ADDR, &data, 1, dev); - } - - return rslt; -} - -/*! - * @brief This API configure the necessary setting of low-g interrupt. - */ -static int8_t config_low_g_int_settg(const struct bmi160_acc_low_g_int_cfg *low_g_int, const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t temp = 0; - uint8_t data_array[3] = { 0, 0, 0 }; - - /* Configuring INT_LOWHIGH register for low-g interrupt */ - rslt = bmi160_get_regs(BMI160_INT_LOWHIGH_2_ADDR, &data_array[2], 1, dev); - if (rslt == BMI160_OK) - { - temp = data_array[2] & ~BMI160_LOW_G_HYST_MASK; - - /* Adding low-g hysteresis */ - data_array[2] = temp | (low_g_int->low_hyst & BMI160_LOW_G_HYST_MASK); - temp = data_array[2] & ~BMI160_LOW_G_LOW_MODE_MASK; - - /* Adding low-mode */ - data_array[2] = temp | ((low_g_int->low_mode << 2) & BMI160_LOW_G_LOW_MODE_MASK); - - /* Adding low-g threshold */ - data_array[1] = low_g_int->low_thres; - - /* Adding low-g interrupt delay */ - data_array[0] = low_g_int->low_dur; - - /* Writing data to INT_LOWHIGH 0,1,2 registers simultaneously*/ - rslt = bmi160_set_regs(BMI160_INT_LOWHIGH_0_ADDR, data_array, 3, dev); - } - - return rslt; -} - -/*! - * @brief This API enables the high-g interrupt. - */ -static int8_t enable_high_g_int(const struct bmi160_acc_high_g_int_cfg *high_g_int_cfg, const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t data = 0; - uint8_t temp = 0; - - /* Enable low-g interrupt in Int Enable 1 register */ - rslt = bmi160_get_regs(BMI160_INT_ENABLE_1_ADDR, &data, 1, dev); - if (rslt == BMI160_OK) - { - /* Adding high-g X-axis */ - temp = data & ~BMI160_HIGH_G_X_INT_EN_MASK; - data = temp | (high_g_int_cfg->high_g_x & BMI160_HIGH_G_X_INT_EN_MASK); - - /* Adding high-g Y-axis */ - temp = data & ~BMI160_HIGH_G_Y_INT_EN_MASK; - data = temp | ((high_g_int_cfg->high_g_y << 1) & BMI160_HIGH_G_Y_INT_EN_MASK); - - /* Adding high-g Z-axis */ - temp = data & ~BMI160_HIGH_G_Z_INT_EN_MASK; - data = temp | ((high_g_int_cfg->high_g_z << 2) & BMI160_HIGH_G_Z_INT_EN_MASK); - - /* write data to Int Enable 0 register */ - rslt = bmi160_set_regs(BMI160_INT_ENABLE_1_ADDR, &data, 1, dev); - } - - return rslt; -} - -/*! - * @brief This API configure the source of data(filter & pre-filter) - * for high-g interrupt. - */ -static int8_t config_high_g_data_src(const struct bmi160_acc_high_g_int_cfg *high_g_int_cfg, - const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t data = 0; - uint8_t temp = 0; - - /* Configure Int data 0 register to add source of interrupt */ - rslt = bmi160_get_regs(BMI160_INT_DATA_0_ADDR, &data, 1, dev); - if (rslt == BMI160_OK) - { - temp = data & ~BMI160_LOW_HIGH_SRC_INT_MASK; - data = temp | ((high_g_int_cfg->high_data_src << 7) & BMI160_LOW_HIGH_SRC_INT_MASK); - - /* Write data to Data 0 address */ - rslt = bmi160_set_regs(BMI160_INT_DATA_0_ADDR, &data, 1, dev); - } - - return rslt; -} - -/*! - * @brief This API configure the necessary setting of high-g interrupt. - */ -static int8_t config_high_g_int_settg(const struct bmi160_acc_high_g_int_cfg *high_g_int_cfg, - const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t temp = 0; - uint8_t data_array[3] = { 0, 0, 0 }; - - rslt = bmi160_get_regs(BMI160_INT_LOWHIGH_2_ADDR, &data_array[0], 1, dev); - if (rslt == BMI160_OK) - { - temp = data_array[0] & ~BMI160_HIGH_G_HYST_MASK; - - /* Adding high-g hysteresis */ - data_array[0] = temp | ((high_g_int_cfg->high_hy << 6) & BMI160_HIGH_G_HYST_MASK); - - /* Adding high-g duration */ - data_array[1] = high_g_int_cfg->high_dur; - - /* Adding high-g threshold */ - data_array[2] = high_g_int_cfg->high_thres; - rslt = bmi160_set_regs(BMI160_INT_LOWHIGH_2_ADDR, data_array, 3, dev); - } - - return rslt; -} - -/*! - * @brief This API configure the behavioural setting of interrupt pin. - */ -static int8_t config_int_out_ctrl(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t temp = 0; - uint8_t data = 0; - - /* Configuration of output interrupt signals on pins INT1 and INT2 are - * done in BMI160_INT_OUT_CTRL_ADDR register*/ - rslt = bmi160_get_regs(BMI160_INT_OUT_CTRL_ADDR, &data, 1, dev); - if (rslt == BMI160_OK) - { - /* updating the interrupt pin structure to local structure */ - const struct bmi160_int_pin_settg *intr_pin_sett = &(int_config->int_pin_settg); - - /* Configuring channel 1 */ - if (int_config->int_channel == BMI160_INT_CHANNEL_1) - { - /* Output enable */ - temp = data & ~BMI160_INT1_OUTPUT_EN_MASK; - data = temp | ((intr_pin_sett->output_en << 3) & BMI160_INT1_OUTPUT_EN_MASK); - - /* Output mode */ - temp = data & ~BMI160_INT1_OUTPUT_MODE_MASK; - data = temp | ((intr_pin_sett->output_mode << 2) & BMI160_INT1_OUTPUT_MODE_MASK); - - /* Output type */ - temp = data & ~BMI160_INT1_OUTPUT_TYPE_MASK; - data = temp | ((intr_pin_sett->output_type << 1) & BMI160_INT1_OUTPUT_TYPE_MASK); - - /* edge control */ - temp = data & ~BMI160_INT1_EDGE_CTRL_MASK; - data = temp | ((intr_pin_sett->edge_ctrl) & BMI160_INT1_EDGE_CTRL_MASK); - } - else - { - /* Configuring channel 2 */ - /* Output enable */ - temp = data & ~BMI160_INT2_OUTPUT_EN_MASK; - data = temp | ((intr_pin_sett->output_en << 7) & BMI160_INT2_OUTPUT_EN_MASK); - - /* Output mode */ - temp = data & ~BMI160_INT2_OUTPUT_MODE_MASK; - data = temp | ((intr_pin_sett->output_mode << 6) & BMI160_INT2_OUTPUT_MODE_MASK); - - /* Output type */ - temp = data & ~BMI160_INT2_OUTPUT_TYPE_MASK; - data = temp | ((intr_pin_sett->output_type << 5) & BMI160_INT2_OUTPUT_TYPE_MASK); - - /* edge control */ - temp = data & ~BMI160_INT2_EDGE_CTRL_MASK; - data = temp | ((intr_pin_sett->edge_ctrl << 4) & BMI160_INT2_EDGE_CTRL_MASK); - } - rslt = bmi160_set_regs(BMI160_INT_OUT_CTRL_ADDR, &data, 1, dev); - } - - return rslt; -} - -/*! - * @brief This API configure the mode(input enable, latch or non-latch) of interrupt pin. - */ -static int8_t config_int_latch(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t temp = 0; - uint8_t data = 0; - - /* Configuration of latch on pins INT1 and INT2 are done in - * BMI160_INT_LATCH_ADDR register*/ - rslt = bmi160_get_regs(BMI160_INT_LATCH_ADDR, &data, 1, dev); - if (rslt == BMI160_OK) - { - /* updating the interrupt pin structure to local structure */ - const struct bmi160_int_pin_settg *intr_pin_sett = &(int_config->int_pin_settg); - if (int_config->int_channel == BMI160_INT_CHANNEL_1) - { - /* Configuring channel 1 */ - /* Input enable */ - temp = data & ~BMI160_INT1_INPUT_EN_MASK; - data = temp | ((intr_pin_sett->input_en << 4) & BMI160_INT1_INPUT_EN_MASK); - } - else - { - /* Configuring channel 2 */ - /* Input enable */ - temp = data & ~BMI160_INT2_INPUT_EN_MASK; - data = temp | ((intr_pin_sett->input_en << 5) & BMI160_INT2_INPUT_EN_MASK); - } - - /* In case of latch interrupt,update the latch duration */ - - /* Latching holds the interrupt for the amount of latch - * duration time */ - temp = data & ~BMI160_INT_LATCH_MASK; - data = temp | (intr_pin_sett->latch_dur & BMI160_INT_LATCH_MASK); - - /* OUT_CTRL_INT and LATCH_INT address lie consecutively, - * hence writing data to respective registers at one go */ - rslt = bmi160_set_regs(BMI160_INT_LATCH_ADDR, &data, 1, dev); - } - - return rslt; -} - -/*! - * @brief This API performs the self test for accelerometer of BMI160 - */ -static int8_t perform_accel_self_test(struct bmi160_dev *dev) -{ - int8_t rslt; - struct bmi160_sensor_data accel_pos, accel_neg; - - /* Enable Gyro self test bit */ - rslt = enable_accel_self_test(dev); - if (rslt == BMI160_OK) - { - /* Perform accel self test with positive excitation */ - rslt = accel_self_test_positive_excitation(&accel_pos, dev); - if (rslt == BMI160_OK) - { - /* Perform accel self test with negative excitation */ - rslt = accel_self_test_negative_excitation(&accel_neg, dev); - if (rslt == BMI160_OK) - { - /* Validate the self test result */ - rslt = validate_accel_self_test(&accel_pos, &accel_neg); - } - } - } - - return rslt; -} - -/*! - * @brief This API enables to perform the accel self test by setting proper - * configurations to facilitate accel self test - */ -static int8_t enable_accel_self_test(struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t reg_data; - - /* Set the Accel power mode as normal mode */ - dev->accel_cfg.power = BMI160_ACCEL_NORMAL_MODE; - - /* Set the sensor range configuration as 8G */ - dev->accel_cfg.range = BMI160_ACCEL_RANGE_8G; - rslt = bmi160_set_sens_conf(dev); - if (rslt == BMI160_OK) - { - /* Accel configurations are set to facilitate self test - * acc_odr - 1600Hz ; acc_bwp = 2 ; acc_us = 0 */ - reg_data = BMI160_ACCEL_SELF_TEST_CONFIG; - rslt = bmi160_set_regs(BMI160_ACCEL_CONFIG_ADDR, ®_data, 1, dev); - } - - return rslt; -} - -/*! - * @brief This API performs accel self test with positive excitation - */ -static int8_t accel_self_test_positive_excitation(struct bmi160_sensor_data *accel_pos, const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t reg_data; - - /* Enable accel self test with positive self-test excitation - * and with amplitude of deflection set as high */ - reg_data = BMI160_ACCEL_SELF_TEST_POSITIVE_EN; - rslt = bmi160_set_regs(BMI160_SELF_TEST_ADDR, ®_data, 1, dev); - if (rslt == BMI160_OK) - { - /* Read the data after a delay of 50ms - refer datasheet 2.8.1 accel self test*/ - dev->delay_ms(BMI160_ACCEL_SELF_TEST_DELAY); - rslt = bmi160_get_sensor_data(BMI160_ACCEL_ONLY, accel_pos, NULL, dev); - } - - return rslt; -} - -/*! - * @brief This API performs accel self test with negative excitation - */ -static int8_t accel_self_test_negative_excitation(struct bmi160_sensor_data *accel_neg, const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t reg_data; - - /* Enable accel self test with negative self-test excitation - * and with amplitude of deflection set as high */ - reg_data = BMI160_ACCEL_SELF_TEST_NEGATIVE_EN; - rslt = bmi160_set_regs(BMI160_SELF_TEST_ADDR, ®_data, 1, dev); - if (rslt == BMI160_OK) - { - /* Read the data after a delay of 50ms */ - dev->delay_ms(BMI160_ACCEL_SELF_TEST_DELAY); - rslt = bmi160_get_sensor_data(BMI160_ACCEL_ONLY, accel_neg, NULL, dev); - } - - return rslt; -} - -/*! - * @brief This API validates the accel self test results - */ -static int8_t validate_accel_self_test(const struct bmi160_sensor_data *accel_pos, - const struct bmi160_sensor_data *accel_neg) -{ - int8_t rslt; - - /* Validate the results of self test */ - if (((accel_neg->x - accel_pos->x) > BMI160_ACCEL_SELF_TEST_LIMIT) && - ((accel_neg->y - accel_pos->y) > BMI160_ACCEL_SELF_TEST_LIMIT) && - ((accel_neg->z - accel_pos->z) > BMI160_ACCEL_SELF_TEST_LIMIT)) - { - /* Self test pass condition */ - rslt = BMI160_OK; - } - else - { - rslt = BMI160_W_ACCEl_SELF_TEST_FAIL; - } - - return rslt; -} - -/*! - * @brief This API performs the self test for gyroscope of BMI160 - */ -static int8_t perform_gyro_self_test(const struct bmi160_dev *dev) -{ - int8_t rslt; - - /* Enable Gyro self test bit */ - rslt = enable_gyro_self_test(dev); - if (rslt == BMI160_OK) - { - /* Validate the gyro self test results */ - rslt = validate_gyro_self_test(dev); - } - - return rslt; -} - -/*! - * @brief This API enables the self test bit to trigger self test for Gyro - */ -static int8_t enable_gyro_self_test(const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t reg_data; - - /* Enable the Gyro self test bit to trigger the self test */ - rslt = bmi160_get_regs(BMI160_SELF_TEST_ADDR, ®_data, 1, dev); - if (rslt == BMI160_OK) - { - reg_data = BMI160_SET_BITS(reg_data, BMI160_GYRO_SELF_TEST, 1); - rslt = bmi160_set_regs(BMI160_SELF_TEST_ADDR, ®_data, 1, dev); - if (rslt == BMI160_OK) - { - /* Delay to enable gyro self test */ - dev->delay_ms(15); - } - } - - return rslt; -} - -/*! - * @brief This API validates the self test results of Gyro - */ -static int8_t validate_gyro_self_test(const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t reg_data; - - /* Validate the Gyro self test result */ - rslt = bmi160_get_regs(BMI160_STATUS_ADDR, ®_data, 1, dev); - if (rslt == BMI160_OK) - { - reg_data = BMI160_GET_BITS(reg_data, BMI160_GYRO_SELF_TEST_STATUS); - if (reg_data == BMI160_ENABLE) - { - /* Gyro self test success case */ - rslt = BMI160_OK; - } - else - { - rslt = BMI160_W_GYRO_SELF_TEST_FAIL; - } - } - - return rslt; -} - -/*! - * @brief This API sets FIFO full interrupt of the sensor.This interrupt - * occurs when the FIFO is full and the next full data sample would cause - * a FIFO overflow, which may delete the old samples. - */ -static int8_t set_fifo_full_int(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) -{ - int8_t rslt = BMI160_OK; - - /* Null-pointer check */ - if ((dev == NULL) || (dev->delay_ms == NULL)) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - /*enable the fifo full interrupt */ - rslt = enable_fifo_full_int(int_config, dev); - if (rslt == BMI160_OK) - { - /* Configure Interrupt pins */ - rslt = set_intr_pin_config(int_config, dev); - if (rslt == BMI160_OK) - { - rslt = map_hardware_interrupt(int_config, dev); - } - } - } - - return rslt; -} - -/*! - * @brief This enable the FIFO full interrupt engine. - */ -static int8_t enable_fifo_full_int(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t data = 0; - - rslt = bmi160_get_regs(BMI160_INT_ENABLE_1_ADDR, &data, 1, dev); - if (rslt == BMI160_OK) - { - data = BMI160_SET_BITS(data, BMI160_FIFO_FULL_INT, int_config->fifo_full_int_en); - - /* Writing data to INT ENABLE 1 Address */ - rslt = bmi160_set_regs(BMI160_INT_ENABLE_1_ADDR, &data, 1, dev); - } - - return rslt; -} - -/*! - * @brief This API sets FIFO watermark interrupt of the sensor.The FIFO - * watermark interrupt is fired, when the FIFO fill level is above a fifo - * watermark. - */ -static int8_t set_fifo_watermark_int(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) -{ - int8_t rslt = BMI160_OK; - - if ((dev == NULL) || (dev->delay_ms == NULL)) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - /* Enable fifo-watermark interrupt in Int Enable 1 register */ - rslt = enable_fifo_wtm_int(int_config, dev); - if (rslt == BMI160_OK) - { - /* Configure Interrupt pins */ - rslt = set_intr_pin_config(int_config, dev); - if (rslt == BMI160_OK) - { - rslt = map_hardware_interrupt(int_config, dev); - } - } - } - - return rslt; -} - -/*! - * @brief This enable the FIFO watermark interrupt engine. - */ -static int8_t enable_fifo_wtm_int(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) -{ - int8_t rslt; - uint8_t data = 0; - - rslt = bmi160_get_regs(BMI160_INT_ENABLE_1_ADDR, &data, 1, dev); - if (rslt == BMI160_OK) - { - data = BMI160_SET_BITS(data, BMI160_FIFO_WTM_INT, int_config->fifo_WTM_int_en); - - /* Writing data to INT ENABLE 1 Address */ - rslt = bmi160_set_regs(BMI160_INT_ENABLE_1_ADDR, &data, 1, dev); - } - - return rslt; -} - -/*! - * @brief This API is used to reset the FIFO related configurations - * in the fifo_frame structure. - */ -static void reset_fifo_data_structure(const struct bmi160_dev *dev) -{ - /*Prepare for next FIFO read by resetting FIFO's - * internal data structures*/ - dev->fifo->accel_byte_start_idx = 0; - dev->fifo->gyro_byte_start_idx = 0; - dev->fifo->aux_byte_start_idx = 0; - dev->fifo->sensor_time = 0; - dev->fifo->skipped_frame_count = 0; -} - -/*! - * @brief This API is used to read fifo_byte_counter value (i.e) - * current fill-level in Fifo buffer. - */ -static int8_t get_fifo_byte_counter(uint16_t *bytes_to_read, struct bmi160_dev const *dev) -{ - int8_t rslt = 0; - uint8_t data[2]; - uint8_t addr = BMI160_FIFO_LENGTH_ADDR; - - rslt |= bmi160_get_regs(addr, data, 2, dev); - data[1] = data[1] & BMI160_FIFO_BYTE_COUNTER_MASK; - - /* Available data in FIFO is stored in bytes_to_read*/ - *bytes_to_read = (((uint16_t)data[1] << 8) | ((uint16_t)data[0])); - - return rslt; -} - -/*! - * @brief This API is used to compute the number of bytes of accel FIFO data - * which is to be parsed in header-less mode - */ -static void get_accel_len_to_parse(uint16_t *data_index, - uint16_t *data_read_length, - const uint8_t *acc_frame_count, - const struct bmi160_dev *dev) -{ - /* Data start index */ - *data_index = dev->fifo->accel_byte_start_idx; - if (dev->fifo->fifo_data_enable == BMI160_FIFO_A_ENABLE) - { - *data_read_length = (*acc_frame_count) * BMI160_FIFO_A_LENGTH; - } - else if (dev->fifo->fifo_data_enable == BMI160_FIFO_G_A_ENABLE) - { - *data_read_length = (*acc_frame_count) * BMI160_FIFO_GA_LENGTH; - } - else if (dev->fifo->fifo_data_enable == BMI160_FIFO_M_A_ENABLE) - { - *data_read_length = (*acc_frame_count) * BMI160_FIFO_MA_LENGTH; - } - else if (dev->fifo->fifo_data_enable == BMI160_FIFO_M_G_A_ENABLE) - { - *data_read_length = (*acc_frame_count) * BMI160_FIFO_MGA_LENGTH; - } - else - { - /* When accel is not enabled ,there will be no accel data. - * so we update the data index as complete */ - *data_index = dev->fifo->length; - } - if (*data_read_length > dev->fifo->length) - { - /* Handling the case where more data is requested - * than that is available*/ - *data_read_length = dev->fifo->length; - } -} - -/*! - * @brief This API is used to parse the accelerometer data from the - * FIFO data in both header mode and header-less mode. - * It updates the idx value which is used to store the index of - * the current data byte which is parsed. - */ -static void unpack_accel_frame(struct bmi160_sensor_data *acc, - uint16_t *idx, - uint8_t *acc_idx, - uint8_t frame_info, - const struct bmi160_dev *dev) -{ - switch (frame_info) - { - case BMI160_FIFO_HEAD_A: - case BMI160_FIFO_A_ENABLE: - - /*Partial read, then skip the data*/ - if ((*idx + BMI160_FIFO_A_LENGTH) > dev->fifo->length) - { - /*Update the data index as complete*/ - *idx = dev->fifo->length; - break; - } - - /*Unpack the data array into the structure instance "acc" */ - unpack_accel_data(&acc[*acc_idx], *idx, dev); - - /*Move the data index*/ - *idx = *idx + BMI160_FIFO_A_LENGTH; - (*acc_idx)++; - break; - case BMI160_FIFO_HEAD_G_A: - case BMI160_FIFO_G_A_ENABLE: - - /*Partial read, then skip the data*/ - if ((*idx + BMI160_FIFO_GA_LENGTH) > dev->fifo->length) - { - /*Update the data index as complete*/ - *idx = dev->fifo->length; - break; - } - - /*Unpack the data array into structure instance "acc"*/ - unpack_accel_data(&acc[*acc_idx], *idx + BMI160_FIFO_G_LENGTH, dev); - - /*Move the data index*/ - *idx = *idx + BMI160_FIFO_GA_LENGTH; - (*acc_idx)++; - break; - case BMI160_FIFO_HEAD_M_A: - case BMI160_FIFO_M_A_ENABLE: - - /*Partial read, then skip the data*/ - if ((*idx + BMI160_FIFO_MA_LENGTH) > dev->fifo->length) - { - /*Update the data index as complete*/ - *idx = dev->fifo->length; - break; - } - - /*Unpack the data array into structure instance "acc"*/ - unpack_accel_data(&acc[*acc_idx], *idx + BMI160_FIFO_M_LENGTH, dev); - - /*Move the data index*/ - *idx = *idx + BMI160_FIFO_MA_LENGTH; - (*acc_idx)++; - break; - case BMI160_FIFO_HEAD_M_G_A: - case BMI160_FIFO_M_G_A_ENABLE: - - /*Partial read, then skip the data*/ - if ((*idx + BMI160_FIFO_MGA_LENGTH) > dev->fifo->length) - { - /*Update the data index as complete*/ - *idx = dev->fifo->length; - break; - } - - /*Unpack the data array into structure instance "acc"*/ - unpack_accel_data(&acc[*acc_idx], *idx + BMI160_FIFO_MG_LENGTH, dev); - - /*Move the data index*/ - *idx = *idx + BMI160_FIFO_MGA_LENGTH; - (*acc_idx)++; - break; - case BMI160_FIFO_HEAD_M: - case BMI160_FIFO_M_ENABLE: - (*idx) = (*idx) + BMI160_FIFO_M_LENGTH; - break; - case BMI160_FIFO_HEAD_G: - case BMI160_FIFO_G_ENABLE: - (*idx) = (*idx) + BMI160_FIFO_G_LENGTH; - break; - case BMI160_FIFO_HEAD_M_G: - case BMI160_FIFO_M_G_ENABLE: - (*idx) = (*idx) + BMI160_FIFO_MG_LENGTH; - break; - default: - break; - } -} - -/*! - * @brief This API is used to parse the accelerometer data from the - * FIFO data and store it in the instance of the structure bmi160_sensor_data. - */ -static void unpack_accel_data(struct bmi160_sensor_data *accel_data, - uint16_t data_start_index, - const struct bmi160_dev *dev) -{ - uint16_t data_lsb; - uint16_t data_msb; - - /* Accel raw x data */ - data_lsb = dev->fifo->data[data_start_index++]; - data_msb = dev->fifo->data[data_start_index++]; - accel_data->x = (int16_t)((data_msb << 8) | data_lsb); - - /* Accel raw y data */ - data_lsb = dev->fifo->data[data_start_index++]; - data_msb = dev->fifo->data[data_start_index++]; - accel_data->y = (int16_t)((data_msb << 8) | data_lsb); - - /* Accel raw z data */ - data_lsb = dev->fifo->data[data_start_index++]; - data_msb = dev->fifo->data[data_start_index++]; - accel_data->z = (int16_t)((data_msb << 8) | data_lsb); -} - -/*! - * @brief This API is used to parse the accelerometer data from the - * FIFO data in header mode. - */ -static void extract_accel_header_mode(struct bmi160_sensor_data *accel_data, - uint8_t *accel_length, - const struct bmi160_dev *dev) -{ - uint8_t frame_header = 0; - uint16_t data_index; - uint8_t accel_index = 0; - - for (data_index = dev->fifo->accel_byte_start_idx; data_index < dev->fifo->length;) - { - /* extracting Frame header */ - frame_header = (dev->fifo->data[data_index] & BMI160_FIFO_TAG_INTR_MASK); - - /*Index is moved to next byte where the data is starting*/ - data_index++; - switch (frame_header) - { - /* Accel frame */ - case BMI160_FIFO_HEAD_A: - case BMI160_FIFO_HEAD_M_A: - case BMI160_FIFO_HEAD_G_A: - case BMI160_FIFO_HEAD_M_G_A: - unpack_accel_frame(accel_data, &data_index, &accel_index, frame_header, dev); - break; - case BMI160_FIFO_HEAD_M: - move_next_frame(&data_index, BMI160_FIFO_M_LENGTH, dev); - break; - case BMI160_FIFO_HEAD_G: - move_next_frame(&data_index, BMI160_FIFO_G_LENGTH, dev); - break; - case BMI160_FIFO_HEAD_M_G: - move_next_frame(&data_index, BMI160_FIFO_MG_LENGTH, dev); - break; - - /* Sensor time frame */ - case BMI160_FIFO_HEAD_SENSOR_TIME: - unpack_sensortime_frame(&data_index, dev); - break; - - /* Skip frame */ - case BMI160_FIFO_HEAD_SKIP_FRAME: - unpack_skipped_frame(&data_index, dev); - break; - - /* Input config frame */ - case BMI160_FIFO_HEAD_INPUT_CONFIG: - move_next_frame(&data_index, 1, dev); - break; - case BMI160_FIFO_HEAD_OVER_READ: - - /* Update the data index as complete in case of Over read */ - data_index = dev->fifo->length; - break; - default: - break; - } - if (*accel_length == accel_index) - { - /* Number of frames to read completed */ - break; - } - } - - /*Update number of accel data read*/ - *accel_length = accel_index; - - /*Update the accel frame index*/ - dev->fifo->accel_byte_start_idx = data_index; -} - -/*! - * @brief This API computes the number of bytes of gyro FIFO data - * which is to be parsed in header-less mode - */ -static void get_gyro_len_to_parse(uint16_t *data_index, - uint16_t *data_read_length, - const uint8_t *gyro_frame_count, - const struct bmi160_dev *dev) -{ - /* Data start index */ - *data_index = dev->fifo->gyro_byte_start_idx; - if (dev->fifo->fifo_data_enable == BMI160_FIFO_G_ENABLE) - { - *data_read_length = (*gyro_frame_count) * BMI160_FIFO_G_LENGTH; - } - else if (dev->fifo->fifo_data_enable == BMI160_FIFO_G_A_ENABLE) - { - *data_read_length = (*gyro_frame_count) * BMI160_FIFO_GA_LENGTH; - } - else if (dev->fifo->fifo_data_enable == BMI160_FIFO_M_G_ENABLE) - { - *data_read_length = (*gyro_frame_count) * BMI160_FIFO_MG_LENGTH; - } - else if (dev->fifo->fifo_data_enable == BMI160_FIFO_M_G_A_ENABLE) - { - *data_read_length = (*gyro_frame_count) * BMI160_FIFO_MGA_LENGTH; - } - else - { - /* When gyro is not enabled ,there will be no gyro data. - * so we update the data index as complete */ - *data_index = dev->fifo->length; - } - if (*data_read_length > dev->fifo->length) - { - /* Handling the case where more data is requested - * than that is available*/ - *data_read_length = dev->fifo->length; - } -} - -/*! - * @brief This API is used to parse the gyroscope's data from the - * FIFO data in both header mode and header-less mode. - * It updates the idx value which is used to store the index of - * the current data byte which is parsed. - */ -static void unpack_gyro_frame(struct bmi160_sensor_data *gyro, - uint16_t *idx, - uint8_t *gyro_idx, - uint8_t frame_info, - const struct bmi160_dev *dev) -{ - switch (frame_info) - { - case BMI160_FIFO_HEAD_G: - case BMI160_FIFO_G_ENABLE: - - /*Partial read, then skip the data*/ - if ((*idx + BMI160_FIFO_G_LENGTH) > dev->fifo->length) - { - /*Update the data index as complete*/ - *idx = dev->fifo->length; - break; - } - - /*Unpack the data array into structure instance "gyro"*/ - unpack_gyro_data(&gyro[*gyro_idx], *idx, dev); - - /*Move the data index*/ - (*idx) = (*idx) + BMI160_FIFO_G_LENGTH; - (*gyro_idx)++; - break; - case BMI160_FIFO_HEAD_G_A: - case BMI160_FIFO_G_A_ENABLE: - - /*Partial read, then skip the data*/ - if ((*idx + BMI160_FIFO_GA_LENGTH) > dev->fifo->length) - { - /*Update the data index as complete*/ - *idx = dev->fifo->length; - break; - } - - /* Unpack the data array into structure instance "gyro" */ - unpack_gyro_data(&gyro[*gyro_idx], *idx, dev); - - /* Move the data index */ - *idx = *idx + BMI160_FIFO_GA_LENGTH; - (*gyro_idx)++; - break; - case BMI160_FIFO_HEAD_M_G_A: - case BMI160_FIFO_M_G_A_ENABLE: - - /*Partial read, then skip the data*/ - if ((*idx + BMI160_FIFO_MGA_LENGTH) > dev->fifo->length) - { - /*Update the data index as complete*/ - *idx = dev->fifo->length; - break; - } - - /*Unpack the data array into structure instance "gyro"*/ - unpack_gyro_data(&gyro[*gyro_idx], *idx + BMI160_FIFO_M_LENGTH, dev); - - /*Move the data index*/ - *idx = *idx + BMI160_FIFO_MGA_LENGTH; - (*gyro_idx)++; - break; - case BMI160_FIFO_HEAD_M_A: - case BMI160_FIFO_M_A_ENABLE: - - /* Move the data index */ - *idx = *idx + BMI160_FIFO_MA_LENGTH; - break; - case BMI160_FIFO_HEAD_M: - case BMI160_FIFO_M_ENABLE: - (*idx) = (*idx) + BMI160_FIFO_M_LENGTH; - break; - case BMI160_FIFO_HEAD_M_G: - case BMI160_FIFO_M_G_ENABLE: - - /*Partial read, then skip the data*/ - if ((*idx + BMI160_FIFO_MG_LENGTH) > dev->fifo->length) - { - /*Update the data index as complete*/ - *idx = dev->fifo->length; - break; - } - - /*Unpack the data array into structure instance "gyro"*/ - unpack_gyro_data(&gyro[*gyro_idx], *idx + BMI160_FIFO_M_LENGTH, dev); - - /*Move the data index*/ - (*idx) = (*idx) + BMI160_FIFO_MG_LENGTH; - (*gyro_idx)++; - break; - case BMI160_FIFO_HEAD_A: - case BMI160_FIFO_A_ENABLE: - - /*Move the data index*/ - *idx = *idx + BMI160_FIFO_A_LENGTH; - break; - default: - break; - } -} - -/*! - * @brief This API is used to parse the gyro data from the - * FIFO data and store it in the instance of the structure bmi160_sensor_data. - */ -static void unpack_gyro_data(struct bmi160_sensor_data *gyro_data, - uint16_t data_start_index, - const struct bmi160_dev *dev) -{ - uint16_t data_lsb; - uint16_t data_msb; - - /* Gyro raw x data */ - data_lsb = dev->fifo->data[data_start_index++]; - data_msb = dev->fifo->data[data_start_index++]; - gyro_data->x = (int16_t)((data_msb << 8) | data_lsb); - - /* Gyro raw y data */ - data_lsb = dev->fifo->data[data_start_index++]; - data_msb = dev->fifo->data[data_start_index++]; - gyro_data->y = (int16_t)((data_msb << 8) | data_lsb); - - /* Gyro raw z data */ - data_lsb = dev->fifo->data[data_start_index++]; - data_msb = dev->fifo->data[data_start_index++]; - gyro_data->z = (int16_t)((data_msb << 8) | data_lsb); -} - -/*! - * @brief This API is used to parse the gyro data from the - * FIFO data in header mode. - */ -static void extract_gyro_header_mode(struct bmi160_sensor_data *gyro_data, - uint8_t *gyro_length, - const struct bmi160_dev *dev) -{ - uint8_t frame_header = 0; - uint16_t data_index; - uint8_t gyro_index = 0; - - for (data_index = dev->fifo->gyro_byte_start_idx; data_index < dev->fifo->length;) - { - /* extracting Frame header */ - frame_header = (dev->fifo->data[data_index] & BMI160_FIFO_TAG_INTR_MASK); - - /*Index is moved to next byte where the data is starting*/ - data_index++; - switch (frame_header) - { - /* GYRO frame */ - case BMI160_FIFO_HEAD_G: - case BMI160_FIFO_HEAD_G_A: - case BMI160_FIFO_HEAD_M_G: - case BMI160_FIFO_HEAD_M_G_A: - unpack_gyro_frame(gyro_data, &data_index, &gyro_index, frame_header, dev); - break; - case BMI160_FIFO_HEAD_A: - move_next_frame(&data_index, BMI160_FIFO_A_LENGTH, dev); - break; - case BMI160_FIFO_HEAD_M: - move_next_frame(&data_index, BMI160_FIFO_M_LENGTH, dev); - break; - case BMI160_FIFO_HEAD_M_A: - move_next_frame(&data_index, BMI160_FIFO_M_LENGTH, dev); - break; - - /* Sensor time frame */ - case BMI160_FIFO_HEAD_SENSOR_TIME: - unpack_sensortime_frame(&data_index, dev); - break; - - /* Skip frame */ - case BMI160_FIFO_HEAD_SKIP_FRAME: - unpack_skipped_frame(&data_index, dev); - break; - - /* Input config frame */ - case BMI160_FIFO_HEAD_INPUT_CONFIG: - move_next_frame(&data_index, 1, dev); - break; - case BMI160_FIFO_HEAD_OVER_READ: - - /* Update the data index as complete in case of over read */ - data_index = dev->fifo->length; - break; - default: - break; - } - if (*gyro_length == gyro_index) - { - /*Number of frames to read completed*/ - break; - } - } - - /*Update number of gyro data read*/ - *gyro_length = gyro_index; - - /*Update the gyro frame index*/ - dev->fifo->gyro_byte_start_idx = data_index; -} - -/*! - * @brief This API computes the number of bytes of aux FIFO data - * which is to be parsed in header-less mode - */ -static void get_aux_len_to_parse(uint16_t *data_index, - uint16_t *data_read_length, - const uint8_t *aux_frame_count, - const struct bmi160_dev *dev) -{ - /* Data start index */ - *data_index = dev->fifo->gyro_byte_start_idx; - if (dev->fifo->fifo_data_enable == BMI160_FIFO_M_ENABLE) - { - *data_read_length = (*aux_frame_count) * BMI160_FIFO_M_LENGTH; - } - else if (dev->fifo->fifo_data_enable == BMI160_FIFO_M_A_ENABLE) - { - *data_read_length = (*aux_frame_count) * BMI160_FIFO_MA_LENGTH; - } - else if (dev->fifo->fifo_data_enable == BMI160_FIFO_M_G_ENABLE) - { - *data_read_length = (*aux_frame_count) * BMI160_FIFO_MG_LENGTH; - } - else if (dev->fifo->fifo_data_enable == BMI160_FIFO_M_G_A_ENABLE) - { - *data_read_length = (*aux_frame_count) * BMI160_FIFO_MGA_LENGTH; - } - else - { - /* When aux is not enabled ,there will be no aux data. - * so we update the data index as complete */ - *data_index = dev->fifo->length; - } - if (*data_read_length > dev->fifo->length) - { - /* Handling the case where more data is requested - * than that is available */ - *data_read_length = dev->fifo->length; - } -} - -/*! - * @brief This API is used to parse the aux's data from the - * FIFO data in both header mode and header-less mode. - * It updates the idx value which is used to store the index of - * the current data byte which is parsed - */ -static void unpack_aux_frame(struct bmi160_aux_data *aux_data, - uint16_t *idx, - uint8_t *aux_index, - uint8_t frame_info, - const struct bmi160_dev *dev) -{ - switch (frame_info) - { - case BMI160_FIFO_HEAD_M: - case BMI160_FIFO_M_ENABLE: - - /* Partial read, then skip the data */ - if ((*idx + BMI160_FIFO_M_LENGTH) > dev->fifo->length) - { - /* Update the data index as complete */ - *idx = dev->fifo->length; - break; - } - - /* Unpack the data array into structure instance */ - unpack_aux_data(&aux_data[*aux_index], *idx, dev); - - /* Move the data index */ - *idx = *idx + BMI160_FIFO_M_LENGTH; - (*aux_index)++; - break; - case BMI160_FIFO_HEAD_M_A: - case BMI160_FIFO_M_A_ENABLE: - - /* Partial read, then skip the data */ - if ((*idx + BMI160_FIFO_MA_LENGTH) > dev->fifo->length) - { - /* Update the data index as complete */ - *idx = dev->fifo->length; - break; - } - - /* Unpack the data array into structure instance */ - unpack_aux_data(&aux_data[*aux_index], *idx, dev); - - /* Move the data index */ - *idx = *idx + BMI160_FIFO_MA_LENGTH; - (*aux_index)++; - break; - case BMI160_FIFO_HEAD_M_G: - case BMI160_FIFO_M_G_ENABLE: - - /* Partial read, then skip the data */ - if ((*idx + BMI160_FIFO_MG_LENGTH) > dev->fifo->length) - { - /* Update the data index as complete */ - *idx = dev->fifo->length; - break; - } - - /* Unpack the data array into structure instance */ - unpack_aux_data(&aux_data[*aux_index], *idx, dev); - - /* Move the data index */ - (*idx) = (*idx) + BMI160_FIFO_MG_LENGTH; - (*aux_index)++; - break; - case BMI160_FIFO_HEAD_M_G_A: - case BMI160_FIFO_M_G_A_ENABLE: - - /*Partial read, then skip the data*/ - if ((*idx + BMI160_FIFO_MGA_LENGTH) > dev->fifo->length) - { - /* Update the data index as complete */ - *idx = dev->fifo->length; - break; - } - - /* Unpack the data array into structure instance */ - unpack_aux_data(&aux_data[*aux_index], *idx, dev); - - /*Move the data index*/ - *idx = *idx + BMI160_FIFO_MGA_LENGTH; - (*aux_index)++; - break; - case BMI160_FIFO_HEAD_G: - case BMI160_FIFO_G_ENABLE: - - /* Move the data index */ - (*idx) = (*idx) + BMI160_FIFO_G_LENGTH; - break; - case BMI160_FIFO_HEAD_G_A: - case BMI160_FIFO_G_A_ENABLE: - - /* Move the data index */ - *idx = *idx + BMI160_FIFO_GA_LENGTH; - break; - case BMI160_FIFO_HEAD_A: - case BMI160_FIFO_A_ENABLE: - - /* Move the data index */ - *idx = *idx + BMI160_FIFO_A_LENGTH; - break; - default: - break; - } -} - -/*! - * @brief This API is used to parse the aux data from the - * FIFO data and store it in the instance of the structure bmi160_aux_data. - */ -static void unpack_aux_data(struct bmi160_aux_data *aux_data, uint16_t data_start_index, const struct bmi160_dev *dev) -{ - /* Aux data bytes */ - aux_data->data[0] = dev->fifo->data[data_start_index++]; - aux_data->data[1] = dev->fifo->data[data_start_index++]; - aux_data->data[2] = dev->fifo->data[data_start_index++]; - aux_data->data[3] = dev->fifo->data[data_start_index++]; - aux_data->data[4] = dev->fifo->data[data_start_index++]; - aux_data->data[5] = dev->fifo->data[data_start_index++]; - aux_data->data[6] = dev->fifo->data[data_start_index++]; - aux_data->data[7] = dev->fifo->data[data_start_index++]; -} - -/*! - * @brief This API is used to parse the aux data from the - * FIFO data in header mode. - */ -static void extract_aux_header_mode(struct bmi160_aux_data *aux_data, uint8_t *aux_length, const struct bmi160_dev *dev) -{ - uint8_t frame_header = 0; - uint16_t data_index; - uint8_t aux_index = 0; - - for (data_index = dev->fifo->aux_byte_start_idx; data_index < dev->fifo->length;) - { - /* extracting Frame header */ - frame_header = (dev->fifo->data[data_index] & BMI160_FIFO_TAG_INTR_MASK); - - /*Index is moved to next byte where the data is starting*/ - data_index++; - switch (frame_header) - { - /* Aux frame */ - case BMI160_FIFO_HEAD_M: - case BMI160_FIFO_HEAD_M_A: - case BMI160_FIFO_HEAD_M_G: - case BMI160_FIFO_HEAD_M_G_A: - unpack_aux_frame(aux_data, &data_index, &aux_index, frame_header, dev); - break; - case BMI160_FIFO_HEAD_G: - move_next_frame(&data_index, BMI160_FIFO_G_LENGTH, dev); - break; - case BMI160_FIFO_HEAD_G_A: - move_next_frame(&data_index, BMI160_FIFO_GA_LENGTH, dev); - break; - case BMI160_FIFO_HEAD_A: - move_next_frame(&data_index, BMI160_FIFO_A_LENGTH, dev); - break; - - /* Sensor time frame */ - case BMI160_FIFO_HEAD_SENSOR_TIME: - unpack_sensortime_frame(&data_index, dev); - break; - - /* Skip frame */ - case BMI160_FIFO_HEAD_SKIP_FRAME: - unpack_skipped_frame(&data_index, dev); - break; - - /* Input config frame */ - case BMI160_FIFO_HEAD_INPUT_CONFIG: - move_next_frame(&data_index, 1, dev); - break; - case BMI160_FIFO_HEAD_OVER_READ: - - /* Update the data index as complete in case - * of over read */ - data_index = dev->fifo->length; - break; - default: - - /* Update the data index as complete in case of - * getting other headers like 0x00 */ - data_index = dev->fifo->length; - break; - } - if (*aux_length == aux_index) - { - /*Number of frames to read completed*/ - break; - } - } - - /* Update number of aux data read */ - *aux_length = aux_index; - - /* Update the aux frame index */ - dev->fifo->aux_byte_start_idx = data_index; -} - -/*! - * @brief This API checks the presence of non-valid frames in the read fifo data. - */ -static void check_frame_validity(uint16_t *data_index, const struct bmi160_dev *dev) -{ - if ((*data_index + 2) < dev->fifo->length) - { - /* Check if FIFO is empty */ - if ((dev->fifo->data[*data_index] == FIFO_CONFIG_MSB_CHECK) && - (dev->fifo->data[*data_index + 1] == FIFO_CONFIG_LSB_CHECK)) - { - /*Update the data index as complete*/ - *data_index = dev->fifo->length; - } - } -} - -/*! - * @brief This 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 void move_next_frame(uint16_t *data_index, uint8_t current_frame_length, const struct bmi160_dev *dev) -{ - /*Partial read, then move the data index to last data*/ - if ((*data_index + current_frame_length) > dev->fifo->length) - { - /*Update the data index as complete*/ - *data_index = dev->fifo->length; - } - else - { - /*Move the data index to next frame*/ - *data_index = *data_index + current_frame_length; - } -} - -/*! - * @brief This API is used to parse and store the sensor time from the - * FIFO data in the structure instance dev. - */ -static void unpack_sensortime_frame(uint16_t *data_index, const struct bmi160_dev *dev) -{ - uint32_t sensor_time_byte3 = 0; - uint16_t sensor_time_byte2 = 0; - uint8_t sensor_time_byte1 = 0; - - /*Partial read, then move the data index to last data*/ - if ((*data_index + BMI160_SENSOR_TIME_LENGTH) > dev->fifo->length) - { - /*Update the data index as complete*/ - *data_index = dev->fifo->length; - } - else - { - sensor_time_byte3 = dev->fifo->data[(*data_index) + BMI160_SENSOR_TIME_MSB_BYTE] << 16; - sensor_time_byte2 = dev->fifo->data[(*data_index) + BMI160_SENSOR_TIME_XLSB_BYTE] << 8; - sensor_time_byte1 = dev->fifo->data[(*data_index)]; - - /* Sensor time */ - dev->fifo->sensor_time = (uint32_t)(sensor_time_byte3 | sensor_time_byte2 | sensor_time_byte1); - *data_index = (*data_index) + BMI160_SENSOR_TIME_LENGTH; - } -} - -/*! - * @brief This API is used to parse and store the skipped_frame_count from - * the FIFO data in the structure instance dev. - */ -static void unpack_skipped_frame(uint16_t *data_index, const struct bmi160_dev *dev) -{ - /*Partial read, then move the data index to last data*/ - if (*data_index >= dev->fifo->length) - { - /*Update the data index as complete*/ - *data_index = dev->fifo->length; - } - else - { - dev->fifo->skipped_frame_count = dev->fifo->data[*data_index]; - - /*Move the data index*/ - *data_index = (*data_index) + 1; - } -} - -/*! - * @brief This API is used to get the FOC status from the sensor - */ -static int8_t get_foc_status(uint8_t *foc_status, struct bmi160_dev const *dev) -{ - int8_t rslt; - uint8_t data; - - /* Read the FOC status from sensor */ - rslt = bmi160_get_regs(BMI160_STATUS_ADDR, &data, 1, dev); - if (rslt == BMI160_OK) - { - /* Get the foc_status bit */ - *foc_status = BMI160_GET_BITS(data, BMI160_FOC_STATUS); - } - - return rslt; -} - -/*! - * @brief This API is used to configure the offset enable bits in the sensor - */ -static int8_t configure_offset_enable(const struct bmi160_foc_conf *foc_conf, struct bmi160_dev const *dev) -{ - int8_t rslt; - uint8_t data; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if (rslt != BMI160_OK) - { - rslt = BMI160_E_NULL_PTR; - } - else - { - /* Read the FOC config from the sensor */ - rslt = bmi160_get_regs(BMI160_OFFSET_CONF_ADDR, &data, 1, dev); - if (rslt == BMI160_OK) - { - /* Set the offset enable/disable for gyro */ - data = BMI160_SET_BITS(data, BMI160_GYRO_OFFSET_EN, foc_conf->gyro_off_en); - - /* Set the offset enable/disable for accel */ - data = BMI160_SET_BITS(data, BMI160_ACCEL_OFFSET_EN, foc_conf->acc_off_en); - - /* Set the offset config in the sensor */ - rslt = bmi160_set_regs(BMI160_OFFSET_CONF_ADDR, &data, 1, dev); - } - } - - return rslt; -} -static int8_t trigger_foc(struct bmi160_offsets *offset, struct bmi160_dev const *dev) -{ - int8_t rslt; - uint8_t foc_status; - uint8_t cmd = BMI160_START_FOC_CMD; - uint8_t timeout = 0; - uint8_t data_array[20]; - - /* Start the FOC process */ - rslt = bmi160_set_regs(BMI160_COMMAND_REG_ADDR, &cmd, 1, dev); - if (rslt == BMI160_OK) - { - /* Check the FOC status*/ - rslt = get_foc_status(&foc_status, dev); - if ((rslt != BMI160_OK) || (foc_status != BMI160_ENABLE)) - { - while ((foc_status != BMI160_ENABLE) && (timeout < 11)) - { - /* Maximum time of 250ms is given in 10 - * steps of 25ms each - 250ms refer datasheet 2.9.1 */ - dev->delay_ms(25); - - /* Check the FOC status*/ - rslt = get_foc_status(&foc_status, dev); - timeout++; - } - if ((rslt == BMI160_OK) && (foc_status == BMI160_ENABLE)) - { - /* Get offset values from sensor */ - rslt = bmi160_get_offsets(offset, dev); - } - else - { - /* FOC failure case */ - rslt = BMI160_FOC_FAILURE; - } - } - if (rslt == BMI160_OK) - { - /* Read registers 0x04-0x17 */ - rslt = bmi160_get_regs(BMI160_GYRO_DATA_ADDR, data_array, 20, dev); - } - } - - return rslt; -} - -/** @}*/ +/** +* 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 bmi160.c +* @date 10/01/2020 +* @version 3.8.1 +* +*/ + +/*! + * @defgroup bmi160 + * @brief + * @{*/ + +#include "bmi160.h" + +/* Below look up table follows the enum bmi160_int_types. + * Hence any change should match to the enum bmi160_int_types + */ +const uint8_t int_mask_lookup_table[13] = { + BMI160_INT1_SLOPE_MASK, BMI160_INT1_SLOPE_MASK, BMI160_INT2_LOW_STEP_DETECT_MASK, BMI160_INT1_DOUBLE_TAP_MASK, + BMI160_INT1_SINGLE_TAP_MASK, BMI160_INT1_ORIENT_MASK, BMI160_INT1_FLAT_MASK, BMI160_INT1_HIGH_G_MASK, + BMI160_INT1_LOW_G_MASK, BMI160_INT1_NO_MOTION_MASK, BMI160_INT2_DATA_READY_MASK, BMI160_INT2_FIFO_FULL_MASK, + BMI160_INT2_FIFO_WM_MASK +}; + +/*********************************************************************/ +/* Static function declarations */ + +/*! + * @brief This API configures the pins to fire the + * interrupt signal when it occurs + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t set_intr_pin_config(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); + +/*! + * @brief This API sets the any-motion interrupt of the sensor. + * This interrupt occurs when accel values exceeds preset threshold + * for a certain period of time. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t set_accel_any_motion_int(struct bmi160_int_settg *int_config, struct bmi160_dev *dev); + +/*! + * @brief This API sets tap interrupts.Interrupt is fired when + * tap movements happen. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t set_accel_tap_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); + +/*! + * @brief This API sets the data ready interrupt for both accel and gyro. + * This interrupt occurs when new accel and gyro data come. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t set_accel_gyro_data_ready_int(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); + +/*! + * @brief This API sets the significant motion interrupt of the sensor.This + * interrupt occurs when there is change in user location. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t set_accel_sig_motion_int(struct bmi160_int_settg *int_config, struct bmi160_dev *dev); + +/*! + * @brief This API sets the no motion/slow motion interrupt of the sensor. + * Slow motion is similar to any motion interrupt.No motion interrupt + * occurs when slope bet. two accel values falls below preset threshold + * for preset duration. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t set_accel_no_motion_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); + +/*! + * @brief This API sets the step detection interrupt.This interrupt + * occurs when the single step causes accel values to go above + * preset threshold. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t set_accel_step_detect_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); + +/*! + * @brief This API sets the orientation interrupt of the sensor.This + * interrupt occurs when there is orientation change in the sensor + * with respect to gravitational field vector g. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t set_accel_orientation_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); + +/*! + * @brief This API sets the flat interrupt of the sensor.This interrupt + * occurs in case of flat orientation + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t set_accel_flat_detect_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); + +/*! + * @brief This API sets the low-g interrupt of the sensor.This interrupt + * occurs during free-fall. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t set_accel_low_g_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); + +/*! + * @brief This API sets the high-g interrupt of the sensor.The interrupt + * occurs if the absolute value of acceleration data of any enabled axis + * exceeds the programmed threshold and the sign of the value does not + * change for a preset duration. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t set_accel_high_g_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); + +/*! + * @brief This API sets the default configuration parameters of accel & gyro. + * Also maintain the previous state of configurations. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static void default_param_settg(struct bmi160_dev *dev); + +/*! + * @brief This API is used to validate the device structure pointer for + * null conditions. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t null_ptr_check(const struct bmi160_dev *dev); + +/*! + * @brief This API set the accel configuration. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t set_accel_conf(struct bmi160_dev *dev); + +/*! + * @brief This API check the accel configuration. + * + * @param[in] data : Pointer to store the updated accel config. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t check_accel_config(uint8_t *data, const struct bmi160_dev *dev); + +/*! + * @brief This API process the accel odr. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t process_accel_odr(uint8_t *data, const struct bmi160_dev *dev); + +/*! + * @brief This API process the accel bandwidth. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t process_accel_bw(uint8_t *data, const struct bmi160_dev *dev); + +/*! + * @brief This API process the accel range. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t process_accel_range(uint8_t *data, const struct bmi160_dev *dev); + +/*! + * @brief This API checks the invalid settings for ODR & Bw for Accel and Gyro. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t check_invalid_settg(const struct bmi160_dev *dev); + +/*! + * @brief This API set the gyro configuration. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t set_gyro_conf(struct bmi160_dev *dev); + +/*! + * @brief This API check the gyro configuration. + * + * @param[in] data : Pointer to store the updated gyro config. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t check_gyro_config(uint8_t *data, const struct bmi160_dev *dev); + +/*! + * @brief This API process the gyro odr. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t process_gyro_odr(uint8_t *data, const struct bmi160_dev *dev); + +/*! + * @brief This API process the gyro bandwidth. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t process_gyro_bw(uint8_t *data, const struct bmi160_dev *dev); + +/*! + * @brief This API process the gyro range. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t process_gyro_range(uint8_t *data, const struct bmi160_dev *dev); + +/*! + * @brief This API sets the accel power mode. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t set_accel_pwr(struct bmi160_dev *dev); + +/*! + * @brief This API process the undersampling setting of Accel. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t process_under_sampling(uint8_t *data, const struct bmi160_dev *dev); + +/*! + * @brief This API sets the gyro power mode. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +static int8_t set_gyro_pwr(struct bmi160_dev *dev); + +/*! + * @brief This API reads accel data along with sensor time if time is requested + * by user. Kindly refer the user guide(README.md) for more info. + * + * @param[in] len : len to read no of bytes + * @param[out] accel : Structure pointer to store accel data + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t get_accel_data(uint8_t len, struct bmi160_sensor_data *accel, const struct bmi160_dev *dev); + +/*! + * @brief This API reads accel data along with sensor time if time is requested + * by user. Kindly refer the user guide(README.md) for more info. + * + * @param[in] len : len to read no of bytes + * @param[out] gyro : Structure pointer to store accel data + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t get_gyro_data(uint8_t len, struct bmi160_sensor_data *gyro, const struct bmi160_dev *dev); + +/*! + * @brief This API reads accel and gyro data along with sensor time + * if time is requested by user. + * Kindly refer the user guide(README.md) for more info. + * + * @param[in] len : len to read no of bytes + * @param[out] accel : Structure pointer to store accel data + * @param[out] gyro : Structure pointer to store accel data + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t get_accel_gyro_data(uint8_t len, + struct bmi160_sensor_data *accel, + struct bmi160_sensor_data *gyro, + const struct bmi160_dev *dev); + +/*! + * @brief This API enables the any-motion interrupt for accel. + * + * @param[in] any_motion_int_cfg : Structure instance of + * bmi160_acc_any_mot_int_cfg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t enable_accel_any_motion_int(const struct bmi160_acc_any_mot_int_cfg *any_motion_int_cfg, + struct bmi160_dev *dev); + +/*! + * @brief This API disable the sig-motion interrupt. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t disable_sig_motion_int(const struct bmi160_dev *dev); + +/*! + * @brief This API configure the source of data(filter & pre-filter) + * for any-motion interrupt. + * + * @param[in] any_motion_int_cfg : Structure instance of + * bmi160_acc_any_mot_int_cfg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_any_motion_src(const struct bmi160_acc_any_mot_int_cfg *any_motion_int_cfg, + const struct bmi160_dev *dev); + +/*! + * @brief This API configure the duration and threshold of + * any-motion interrupt. + * + * @param[in] any_motion_int_cfg : Structure instance of + * bmi160_acc_any_mot_int_cfg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_any_dur_threshold(const struct bmi160_acc_any_mot_int_cfg *any_motion_int_cfg, + const struct bmi160_dev *dev); + +/*! + * @brief This API configure necessary setting of any-motion interrupt. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] any_motion_int_cfg : Structure instance of + * bmi160_acc_any_mot_int_cfg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_any_motion_int_settg(const struct bmi160_int_settg *int_config, + const struct bmi160_acc_any_mot_int_cfg *any_motion_int_cfg, + const struct bmi160_dev *dev); + +/*! + * @brief This API enable the data ready interrupt. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t enable_data_ready_int(const struct bmi160_dev *dev); + +/*! + * @brief This API enables the no motion/slow motion interrupt. + * + * @param[in] no_mot_int_cfg : Structure instance of + * bmi160_acc_no_motion_int_cfg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t enable_no_motion_int(const struct bmi160_acc_no_motion_int_cfg *no_mot_int_cfg, + const struct bmi160_dev *dev); + +/*! + * @brief This API configure the interrupt PIN setting for + * no motion/slow motion interrupt. + * + * @param[in] int_config : structure instance of bmi160_int_settg. + * @param[in] no_mot_int_cfg : Structure instance of + * bmi160_acc_no_motion_int_cfg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_no_motion_int_settg(const struct bmi160_int_settg *int_config, + const struct bmi160_acc_no_motion_int_cfg *no_mot_int_cfg, + const struct bmi160_dev *dev); + +/*! + * @brief This API configure the source of interrupt for no motion. + * + * @param[in] no_mot_int_cfg : Structure instance of + * bmi160_acc_no_motion_int_cfg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_no_motion_data_src(const struct bmi160_acc_no_motion_int_cfg *no_mot_int_cfg, + const struct bmi160_dev *dev); + +/*! + * @brief This API configure the duration and threshold of + * no motion/slow motion interrupt along with selection of no/slow motion. + * + * @param[in] no_mot_int_cfg : Structure instance of + * bmi160_acc_no_motion_int_cfg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_no_motion_dur_thr(const struct bmi160_acc_no_motion_int_cfg *no_mot_int_cfg, + const struct bmi160_dev *dev); + +/*! + * @brief This API enables the sig-motion motion interrupt. + * + * @param[in] sig_mot_int_cfg : Structure instance of + * bmi160_acc_sig_mot_int_cfg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t enable_sig_motion_int(const struct bmi160_acc_sig_mot_int_cfg *sig_mot_int_cfg, struct bmi160_dev *dev); + +/*! + * @brief This API configure the interrupt PIN setting for + * significant motion interrupt. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] sig_mot_int_cfg : Structure instance of + * bmi160_acc_sig_mot_int_cfg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_sig_motion_int_settg(const struct bmi160_int_settg *int_config, + const struct bmi160_acc_sig_mot_int_cfg *sig_mot_int_cfg, + const struct bmi160_dev *dev); + +/*! + * @brief This API configure the source of data(filter & pre-filter) + * for sig motion interrupt. + * + * @param[in] sig_mot_int_cfg : Structure instance of + * bmi160_acc_sig_mot_int_cfg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_sig_motion_data_src(const struct bmi160_acc_sig_mot_int_cfg *sig_mot_int_cfg, + const struct bmi160_dev *dev); + +/*! + * @brief This API configure the threshold, skip and proof time of + * sig motion interrupt. + * + * @param[in] sig_mot_int_cfg : Structure instance of + * bmi160_acc_sig_mot_int_cfg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_sig_dur_threshold(const struct bmi160_acc_sig_mot_int_cfg *sig_mot_int_cfg, + const struct bmi160_dev *dev); + +/*! + * @brief This API enables the step detector interrupt. + * + * @param[in] step_detect_int_cfg : Structure instance of + * bmi160_acc_step_detect_int_cfg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t enable_step_detect_int(const struct bmi160_acc_step_detect_int_cfg *step_detect_int_cfg, + const struct bmi160_dev *dev); + +/*! + * @brief This API configure the step detector parameter. + * + * @param[in] step_detect_int_cfg : Structure instance of + * bmi160_acc_step_detect_int_cfg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_step_detect(const struct bmi160_acc_step_detect_int_cfg *step_detect_int_cfg, + const struct bmi160_dev *dev); + +/*! + * @brief This API enables the single/double tap interrupt. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t enable_tap_int(const struct bmi160_int_settg *int_config, + const struct bmi160_acc_tap_int_cfg *tap_int_cfg, + const struct bmi160_dev *dev); + +/*! + * @brief This API configure the interrupt PIN setting for + * tap interrupt. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] tap_int_cfg : Structure instance of bmi160_acc_tap_int_cfg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_tap_int_settg(const struct bmi160_int_settg *int_config, + const struct bmi160_acc_tap_int_cfg *tap_int_cfg, + const struct bmi160_dev *dev); + +/*! + * @brief This API configure the source of data(filter & pre-filter) + * for tap interrupt. + * + * @param[in] tap_int_cfg : Structure instance of bmi160_acc_tap_int_cfg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_tap_data_src(const struct bmi160_acc_tap_int_cfg *tap_int_cfg, const struct bmi160_dev *dev); + +/*! + * @brief This API configure the parameters of tap interrupt. + * Threshold, quite, shock, and duration. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] tap_int_cfg : Structure instance of bmi160_acc_tap_int_cfg. + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_tap_param(const struct bmi160_int_settg *int_config, + const struct bmi160_acc_tap_int_cfg *tap_int_cfg, + const struct bmi160_dev *dev); + +/*! + * @brief This API enable the external mode configuration. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_sec_if(const struct bmi160_dev *dev); + +/*! + * @brief This API configure the ODR of the auxiliary sensor. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_aux_odr(const struct bmi160_dev *dev); + +/*! + * @brief This API maps the actual burst read length set by user. + * + * @param[in] len : Pointer to store the read length. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t map_read_len(uint16_t *len, const struct bmi160_dev *dev); + +/*! + * @brief This API configure the settings of auxiliary sensor. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_aux_settg(const struct bmi160_dev *dev); + +/*! + * @brief This API extract the read data from auxiliary sensor. + * + * @param[in] map_len : burst read value. + * @param[in] reg_addr : Address of register to read. + * @param[in] aux_data : Pointer to store the read data. + * @param[in] len : length to read the data. + * @param[in] dev : Structure instance of bmi160_dev. + * @note : Refer user guide for detailed info. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t extract_aux_read(uint16_t map_len, + uint8_t reg_addr, + uint8_t *aux_data, + uint16_t len, + const struct bmi160_dev *dev); + +/*! + * @brief This API enables the orient interrupt. + * + * @param[in] orient_int_cfg : Structure instance of bmi160_acc_orient_int_cfg. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t enable_orient_int(const struct bmi160_acc_orient_int_cfg *orient_int_cfg, const struct bmi160_dev *dev); + +/*! + * @brief This API configure the necessary setting of orientation interrupt. + * + * @param[in] orient_int_cfg : Structure instance of bmi160_acc_orient_int_cfg. + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_orient_int_settg(const struct bmi160_acc_orient_int_cfg *orient_int_cfg, + const struct bmi160_dev *dev); + +/*! + * @brief This API enables the flat interrupt. + * + * @param[in] flat_int : Structure instance of bmi160_acc_flat_detect_int_cfg. + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t enable_flat_int(const struct bmi160_acc_flat_detect_int_cfg *flat_int, const struct bmi160_dev *dev); + +/*! + * @brief This API configure the necessary setting of flat interrupt. + * + * @param[in] flat_int : Structure instance of bmi160_acc_flat_detect_int_cfg. + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_flat_int_settg(const struct bmi160_acc_flat_detect_int_cfg *flat_int, + const struct bmi160_dev *dev); + +/*! + * @brief This API enables the Low-g interrupt. + * + * @param[in] low_g_int : Structure instance of bmi160_acc_low_g_int_cfg. + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t enable_low_g_int(const struct bmi160_acc_low_g_int_cfg *low_g_int, const struct bmi160_dev *dev); + +/*! + * @brief This API configure the source of data(filter & pre-filter) for low-g interrupt. + * + * @param[in] low_g_int : Structure instance of bmi160_acc_low_g_int_cfg. + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_low_g_data_src(const struct bmi160_acc_low_g_int_cfg *low_g_int, const struct bmi160_dev *dev); + +/*! + * @brief This API configure the necessary setting of low-g interrupt. + * + * @param[in] low_g_int : Structure instance of bmi160_acc_low_g_int_cfg. + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_low_g_int_settg(const struct bmi160_acc_low_g_int_cfg *low_g_int, const struct bmi160_dev *dev); + +/*! + * @brief This API enables the high-g interrupt. + * + * @param[in] high_g_int_cfg : Structure instance of bmi160_acc_high_g_int_cfg. + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t enable_high_g_int(const struct bmi160_acc_high_g_int_cfg *high_g_int_cfg, const struct bmi160_dev *dev); + +/*! + * @brief This API configure the source of data(filter & pre-filter) + * for high-g interrupt. + * + * @param[in] high_g_int_cfg : Structure instance of bmi160_acc_high_g_int_cfg. + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_high_g_data_src(const struct bmi160_acc_high_g_int_cfg *high_g_int_cfg, + const struct bmi160_dev *dev); + +/*! + * @brief This API configure the necessary setting of high-g interrupt. + * + * @param[in] high_g_int_cfg : Structure instance of bmi160_acc_high_g_int_cfg. + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_high_g_int_settg(const struct bmi160_acc_high_g_int_cfg *high_g_int_cfg, + const struct bmi160_dev *dev); + +/*! + * @brief This API configure the behavioural setting of interrupt pin. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_int_out_ctrl(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); + +/*! + * @brief This API configure the mode(input enable, latch or non-latch) of interrupt pin. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t config_int_latch(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); + +/*! + * @brief This API performs the self test for accelerometer of BMI160 + * + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t perform_accel_self_test(struct bmi160_dev *dev); + +/*! + * @brief This API enables to perform the accel self test by setting proper + * configurations to facilitate accel self test + * + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t enable_accel_self_test(struct bmi160_dev *dev); + +/*! + * @brief This API performs accel self test with positive excitation + * + * @param[in] accel_pos : Structure pointer to store accel data + * for positive excitation + * @param[in] dev : structure instance of bmi160_dev + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t accel_self_test_positive_excitation(struct bmi160_sensor_data *accel_pos, const struct bmi160_dev *dev); + +/*! + * @brief This API performs accel self test with negative excitation + * + * @param[in] accel_neg : Structure pointer to store accel data + * for negative excitation + * @param[in] dev : structure instance of bmi160_dev + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t accel_self_test_negative_excitation(struct bmi160_sensor_data *accel_neg, const struct bmi160_dev *dev); + +/*! + * @brief This API validates the accel self test results + * + * @param[in] accel_pos : Structure pointer to store accel data + * for positive excitation + * @param[in] accel_neg : Structure pointer to store accel data + * for negative excitation + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error / +ve value -> Self test fail + */ +static int8_t validate_accel_self_test(const struct bmi160_sensor_data *accel_pos, + const struct bmi160_sensor_data *accel_neg); + +/*! + * @brief This API performs the self test for gyroscope of BMI160 + * + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t perform_gyro_self_test(const struct bmi160_dev *dev); + +/*! + * @brief This API enables the self test bit to trigger self test for gyro + * + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t enable_gyro_self_test(const struct bmi160_dev *dev); + +/*! + * @brief This API validates the self test results of gyro + * + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t validate_gyro_self_test(const struct bmi160_dev *dev); + +/*! + * @brief This API sets FIFO full interrupt of the sensor.This interrupt + * occurs when the FIFO is full and the next full data sample would cause + * a FIFO overflow, which may delete the old samples. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t set_fifo_full_int(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); + +/*! + * @brief This enable the FIFO full interrupt engine. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t enable_fifo_full_int(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); + +/*! + * @brief This API sets FIFO watermark interrupt of the sensor.The FIFO + * watermark interrupt is fired, when the FIFO fill level is above a fifo + * watermark. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t set_fifo_watermark_int(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); + +/*! + * @brief This enable the FIFO watermark interrupt engine. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t enable_fifo_wtm_int(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); + +/*! + * @brief This API is used to reset the FIFO related configurations + * in the fifo_frame structure. + * + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static void reset_fifo_data_structure(const struct bmi160_dev *dev); + +/*! + * @brief This API is used to read number of bytes filled + * currently in FIFO buffer. + * + * @param[in] bytes_to_read : Number of bytes available in FIFO at the + * instant which is obtained from FIFO counter. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + * @retval Any non zero value -> Fail + * + */ +static int8_t get_fifo_byte_counter(uint16_t *bytes_to_read, struct bmi160_dev const *dev); + +/*! + * @brief This API is used to compute the number of bytes of accel FIFO data + * which is to be parsed in header-less mode + * + * @param[out] data_index : The start index for parsing data + * @param[out] data_read_length : Number of bytes to be parsed + * @param[in] acc_frame_count : Number of accelerometer frames to be read + * @param[in] dev : Structure instance of bmi160_dev. + * + */ +static void get_accel_len_to_parse(uint16_t *data_index, + uint16_t *data_read_length, + const uint8_t *acc_frame_count, + const struct bmi160_dev *dev); + +/*! + * @brief This API is used to parse the accelerometer data from the + * FIFO data in both header mode and header-less mode. + * It updates the idx value which is used to store the index of + * the current data byte which is parsed. + * + * @param[in,out] acc : structure instance of sensor data + * @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) frames parsed + * @param[in] frame_info : It consists of either fifo_data_enable + * parameter in header-less mode or + * frame header data in header mode + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static void unpack_accel_frame(struct bmi160_sensor_data *acc, + uint16_t *idx, + uint8_t *acc_idx, + uint8_t frame_info, + const struct bmi160_dev *dev); + +/*! + * @brief This API is used to parse the accelerometer data from the + * FIFO data and store it in the instance of the structure bmi160_sensor_data. + * + * @param[in,out] accel_data : structure instance of sensor data + * @param[in,out] data_start_index : Index value of number of bytes parsed + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static void unpack_accel_data(struct bmi160_sensor_data *accel_data, + uint16_t data_start_index, + const struct bmi160_dev *dev); + +/*! + * @brief This API is used to parse the accelerometer data from the + * FIFO data in header mode. + * + * @param[in,out] accel_data : Structure instance of sensor data + * @param[in,out] accel_length : Number of accelerometer frames + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static void extract_accel_header_mode(struct bmi160_sensor_data *accel_data, + uint8_t *accel_length, + const struct bmi160_dev *dev); + +/*! + * @brief This API computes the number of bytes of gyro FIFO data + * which is to be parsed in header-less mode + * + * @param[out] data_index : The start index for parsing data + * @param[out] data_read_length : No of bytes to be parsed from FIFO buffer + * @param[in] gyro_frame_count : Number of Gyro data frames to be read + * @param[in] dev : Structure instance of bmi160_dev. + */ +static void get_gyro_len_to_parse(uint16_t *data_index, + uint16_t *data_read_length, + const uint8_t *gyro_frame_count, + const struct bmi160_dev *dev); + +/*! + * @brief This API is used to parse the gyroscope's data from the + * FIFO data in both header mode and header-less mode. + * It updates the idx value which is used to store the index of + * the current data byte which is parsed. + * + * @param[in,out] gyro : structure instance of sensor data + * @param[in,out] idx : Index value of number of bytes parsed + * @param[in,out] gyro_idx : Index value of gyro data + * (x,y,z axes) frames parsed + * @param[in] frame_info : It consists of either fifo_data_enable + * parameter in header-less mode or + * frame header data in header mode + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static void unpack_gyro_frame(struct bmi160_sensor_data *gyro, + uint16_t *idx, + uint8_t *gyro_idx, + uint8_t frame_info, + const struct bmi160_dev *dev); + +/*! + * @brief This API is used to parse the gyro data from the + * FIFO data and store it in the instance of the structure bmi160_sensor_data. + * + * @param[in,out] gyro_data : structure instance of sensor data + * @param[in,out] data_start_index : Index value of number of bytes parsed + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static void unpack_gyro_data(struct bmi160_sensor_data *gyro_data, + uint16_t data_start_index, + const struct bmi160_dev *dev); + +/*! + * @brief This API is used to parse the gyro data from the + * FIFO data in header mode. + * + * @param[in,out] gyro_data : Structure instance of sensor data + * @param[in,out] gyro_length : Number of gyro frames + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static void extract_gyro_header_mode(struct bmi160_sensor_data *gyro_data, + uint8_t *gyro_length, + const struct bmi160_dev *dev); + +/*! + * @brief This API computes the number of bytes of aux FIFO data + * which is to be parsed in header-less mode + * + * @param[out] data_index : The start index for parsing data + * @param[out] data_read_length : No of bytes to be parsed from FIFO buffer + * @param[in] aux_frame_count : Number of Aux data frames to be read + * @param[in] dev : Structure instance of bmi160_dev. + */ +static void get_aux_len_to_parse(uint16_t *data_index, + uint16_t *data_read_length, + const uint8_t *aux_frame_count, + const struct bmi160_dev *dev); + +/*! + * @brief This API is used to parse the aux's data from the + * FIFO data in both header mode and header-less mode. + * It updates the idx value which is used to store the index of + * the current data byte which is parsed + * + * @param[in,out] aux_data : structure instance of sensor data + * @param[in,out] idx : Index value of number of bytes parsed + * @param[in,out] aux_index : Index value of gyro data + * (x,y,z axes) frames parsed + * @param[in] frame_info : It consists of either fifo_data_enable + * parameter in header-less mode or + * frame header data in header mode + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static void unpack_aux_frame(struct bmi160_aux_data *aux_data, + uint16_t *idx, + uint8_t *aux_index, + uint8_t frame_info, + const struct bmi160_dev *dev); + +/*! + * @brief This API is used to parse the aux data from the + * FIFO data and store it in the instance of the structure bmi160_aux_data. + * + * @param[in,out] aux_data : structure instance of sensor data + * @param[in,out] data_start_index : Index value of number of bytes parsed + * @param[in] dev : structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static void unpack_aux_data(struct bmi160_aux_data *aux_data, uint16_t data_start_index, const struct bmi160_dev *dev); + +/*! + * @brief This API is used to parse the aux data from the + * FIFO data in header mode. + * + * @param[in,out] aux_data : Structure instance of sensor data + * @param[in,out] aux_length : Number of aux frames + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static void extract_aux_header_mode(struct bmi160_aux_data *aux_data, uint8_t *aux_length, + const struct bmi160_dev *dev); + +/*! + * @brief This API checks the presence of non-valid frames in the read fifo data. + * + * @param[in,out] data_index : The index of the current data to + * be parsed from fifo data + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static void check_frame_validity(uint16_t *data_index, const struct bmi160_dev *dev); + +/*! + * @brief This 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 a particular frame + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static void move_next_frame(uint16_t *data_index, uint8_t current_frame_length, const struct bmi160_dev *dev); + +/*! + * @brief This API is used to parse and store the sensor time from the + * FIFO data in the structure instance dev. + * + * @param[in,out] data_index : Index of the FIFO data which + * has the sensor time. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static void unpack_sensortime_frame(uint16_t *data_index, const struct bmi160_dev *dev); + +/*! + * @brief This API is used to parse and store the skipped_frame_count from + * the FIFO data in the structure instance dev. + * + * @param[in,out] data_index : Index of the FIFO data which + * has the skipped frame count. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static void unpack_skipped_frame(uint16_t *data_index, const struct bmi160_dev *dev); + +/*! + * @brief This API is used to get the FOC status from the sensor + * + * @param[in,out] foc_status : Result of FOC status. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t get_foc_status(uint8_t *foc_status, struct bmi160_dev const *dev); + +/*! + * @brief This API is used to configure the offset enable bits in the sensor + * + * @param[in,out] foc_conf : Structure instance of bmi160_foc_conf which + * has the FOC and offset configurations + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t configure_offset_enable(const struct bmi160_foc_conf *foc_conf, struct bmi160_dev const *dev); + +/*! + * @brief This API is used to trigger the FOC in the sensor + * + * @param[in,out] offset : Structure instance of bmi160_offsets which + * reads and stores the offset values after FOC + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t trigger_foc(struct bmi160_offsets *offset, struct bmi160_dev const *dev); + +/*! + * @brief This API is used to map/unmap the Dataready(Accel & Gyro), FIFO full + * and FIFO watermark interrupt + * + * @param[in] int_config : Structure instance of bmi160_int_settg which + * stores the interrupt type and interrupt channel + * configurations to map/unmap the interrupt pins + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t map_hardware_interrupt(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); + +/*! + * @brief This API is used to map/unmap the Any/Sig motion, Step det/Low-g, + * Double tap, Single tap, Orientation, Flat, High-G, Nomotion interrupt pins. + * + * @param[in] int_config : Structure instance of bmi160_int_settg which + * stores the interrupt type and interrupt channel + * configurations to map/unmap the interrupt pins + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +static int8_t map_feature_interrupt(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev); + +/*********************** User function definitions ****************************/ + +/*! + * @brief This API reads the data from the given register address + * of sensor. + */ +int8_t bmi160_get_regs(uint8_t reg_addr, uint8_t *data, uint16_t len, const struct bmi160_dev *dev) +{ + int8_t rslt = BMI160_OK; + + /* 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 indx = 0; + + /* Null-pointer check */ + if ((dev == NULL) || (dev->read == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else if (len == 0) + { + rslt = BMI160_READ_WRITE_LENGHT_INVALID; + } + else + { + /* Configuring reg_addr for SPI Interface */ + if (dev->interface == BMI160_SPI_INTF) + { + reg_addr = (reg_addr | BMI160_SPI_RD_MASK); + } + rslt = dev->read(dev->id, reg_addr, temp_buf, temp_len); + + if (rslt == BMI160_OK) + { + /* Read the data from the position next to dummy byte */ + while (indx < len) + { + data[indx] = temp_buf[indx]; + indx++; + } + } + else + { + rslt = BMI160_E_COM_FAIL; + } + } + + return rslt; +} + +/*! + * @brief This API writes the given data to the register address + * of sensor. + */ +int8_t bmi160_set_regs(uint8_t reg_addr, uint8_t *data, uint16_t len, const struct bmi160_dev *dev) +{ + int8_t rslt = BMI160_OK; + uint8_t count = 0; + + /* Null-pointer check */ + if ((dev == NULL) || (dev->write == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else if (len == 0) + { + rslt = BMI160_READ_WRITE_LENGHT_INVALID; + } + else + { + /* Configuring reg_addr for SPI Interface */ + if (dev->interface == BMI160_SPI_INTF) + { + reg_addr = (reg_addr & BMI160_SPI_WR_MASK); + } + if ((dev->prev_accel_cfg.power == BMI160_ACCEL_NORMAL_MODE) || + (dev->prev_gyro_cfg.power == BMI160_GYRO_NORMAL_MODE)) + { + rslt = dev->write(dev->id, reg_addr, data, len); + + /* Kindly refer bmi160 data sheet section 3.2.4 */ + dev->delay_ms(1); + + } + else + { + /*Burst write is not allowed in + * suspend & low power mode */ + for (; count < len; count++) + { + rslt = dev->write(dev->id, reg_addr, &data[count], 1); + reg_addr++; + + /* Kindly refer bmi160 data sheet section 3.2.4 */ + dev->delay_ms(1); + + } + } + if (rslt != BMI160_OK) + { + rslt = BMI160_E_COM_FAIL; + } + } + + return rslt; +} + +/*! + * @brief This API is the entry point for sensor.It performs + * the selection of I2C/SPI read mechanism according to the + * selected interface and reads the chip-id of bmi160 sensor. + */ +int8_t bmi160_init(struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data; + uint8_t try = 3; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + + /* An extra dummy byte is read during SPI read */ + if (dev->interface == BMI160_SPI_INTF) + { + dev->dummy_byte = 1; + } + else + { + dev->dummy_byte = 0; + } + + /* Dummy read of 0x7F register to enable SPI Interface + * if SPI is used */ + if ((rslt == BMI160_OK) && (dev->interface == BMI160_SPI_INTF)) + { + rslt = bmi160_get_regs(BMI160_SPI_COMM_TEST_ADDR, &data, 1, dev); + } + + if (rslt == BMI160_OK) + { + /* Assign chip id as zero */ + dev->chip_id = 0; + + while ((try--) && (dev->chip_id != BMI160_CHIP_ID)) + { + /* Read chip_id */ + rslt = bmi160_get_regs(BMI160_CHIP_ID_ADDR, &dev->chip_id, 1, dev); + } + if ((rslt == BMI160_OK) && (dev->chip_id == BMI160_CHIP_ID)) + { + dev->any_sig_sel = BMI160_BOTH_ANY_SIG_MOTION_DISABLED; + + /* Soft reset */ + rslt = bmi160_soft_reset(dev); + } + else + { + rslt = BMI160_E_DEV_NOT_FOUND; + } + } + + return rslt; +} + +/*! + * @brief This API resets and restarts the device. + * All register values are overwritten with default parameters. + */ +int8_t bmi160_soft_reset(struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = BMI160_SOFT_RESET_CMD; + + /* Null-pointer check */ + if ((dev == NULL) || (dev->delay_ms == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /* Reset the device */ + rslt = bmi160_set_regs(BMI160_COMMAND_REG_ADDR, &data, 1, dev); + dev->delay_ms(BMI160_SOFT_RESET_DELAY_MS); + if ((rslt == BMI160_OK) && (dev->interface == BMI160_SPI_INTF)) + { + /* Dummy read of 0x7F register to enable SPI Interface + * if SPI is used */ + rslt = bmi160_get_regs(BMI160_SPI_COMM_TEST_ADDR, &data, 1, dev); + } + if (rslt == BMI160_OK) + { + /* Update the default parameters */ + default_param_settg(dev); + } + } + + return rslt; +} + +/*! + * @brief This API configures the power mode, range and bandwidth + * of sensor. + */ +int8_t bmi160_set_sens_conf(struct bmi160_dev *dev) +{ + int8_t rslt = BMI160_OK; + + /* Null-pointer check */ + if ((dev == NULL) || (dev->delay_ms == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + rslt = set_accel_conf(dev); + if (rslt == BMI160_OK) + { + rslt = set_gyro_conf(dev); + if (rslt == BMI160_OK) + { + /* write power mode for accel and gyro */ + rslt = bmi160_set_power_mode(dev); + if (rslt == BMI160_OK) + { + rslt = check_invalid_settg(dev); + } + } + } + } + + return rslt; +} + +/*! + * @brief This API sets the power mode of the sensor. + */ +int8_t bmi160_set_power_mode(struct bmi160_dev *dev) +{ + int8_t rslt = 0; + + /* Null-pointer check */ + if ((dev == NULL) || (dev->delay_ms == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + rslt = set_accel_pwr(dev); + if (rslt == BMI160_OK) + { + rslt = set_gyro_pwr(dev); + } + } + + return rslt; +} + +/*! + * @brief This API gets the power mode of the sensor. + */ +int8_t bmi160_get_power_mode(struct bmi160_pmu_status *pmu_status, const struct bmi160_dev *dev) +{ + int8_t rslt = 0; + uint8_t power_mode = 0; + + /* Null-pointer check */ + if ((dev == NULL) || (dev->delay_ms == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + rslt = bmi160_get_regs(BMI160_PMU_STATUS_ADDR, &power_mode, 1, dev); + if (rslt == BMI160_OK) + { + /* Power mode of the accel,gyro,aux sensor is obtained */ + pmu_status->aux_pmu_status = BMI160_GET_BITS_POS_0(power_mode, BMI160_MAG_POWER_MODE); + pmu_status->gyro_pmu_status = BMI160_GET_BITS(power_mode, BMI160_GYRO_POWER_MODE); + pmu_status->accel_pmu_status = BMI160_GET_BITS(power_mode, BMI160_ACCEL_POWER_MODE); + } + } + + return rslt; +} + +/*! + * @brief This API reads sensor data, stores it in + * the bmi160_sensor_data structure pointer passed by the user. + */ +int8_t bmi160_get_sensor_data(uint8_t select_sensor, + struct bmi160_sensor_data *accel, + struct bmi160_sensor_data *gyro, + const struct bmi160_dev *dev) +{ + int8_t rslt = BMI160_OK; + uint8_t time_sel; + uint8_t sen_sel; + uint8_t len = 0; + + /*Extract the sensor and time select information*/ + sen_sel = select_sensor & BMI160_SEN_SEL_MASK; + time_sel = ((sen_sel & BMI160_TIME_SEL) >> 2); + sen_sel = sen_sel & (BMI160_ACCEL_SEL | BMI160_GYRO_SEL); + if (time_sel == 1) + { + len = 3; + } + + /* Null-pointer check */ + if (dev != NULL) + { + switch (sen_sel) + { + case BMI160_ACCEL_ONLY: + + /* Null-pointer check */ + if (accel == NULL) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + rslt = get_accel_data(len, accel, dev); + } + break; + case BMI160_GYRO_ONLY: + + /* Null-pointer check */ + if (gyro == NULL) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + rslt = get_gyro_data(len, gyro, dev); + } + break; + case BMI160_BOTH_ACCEL_AND_GYRO: + + /* Null-pointer check */ + if ((gyro == NULL) || (accel == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + rslt = get_accel_gyro_data(len, accel, gyro, dev); + } + break; + default: + rslt = BMI160_E_INVALID_INPUT; + break; + } + } + else + { + rslt = BMI160_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API configures the necessary interrupt based on + * the user settings in the bmi160_int_settg structure instance. + */ +int8_t bmi160_set_int_config(struct bmi160_int_settg *int_config, struct bmi160_dev *dev) +{ + int8_t rslt = BMI160_OK; + + switch (int_config->int_type) + { + case BMI160_ACC_ANY_MOTION_INT: + + /*Any-motion interrupt*/ + rslt = set_accel_any_motion_int(int_config, dev); + break; + case BMI160_ACC_SIG_MOTION_INT: + + /* Significant motion interrupt */ + rslt = set_accel_sig_motion_int(int_config, dev); + break; + case BMI160_ACC_SLOW_NO_MOTION_INT: + + /* Slow or no motion interrupt */ + rslt = set_accel_no_motion_int(int_config, dev); + break; + case BMI160_ACC_DOUBLE_TAP_INT: + case BMI160_ACC_SINGLE_TAP_INT: + + /* Double tap and single tap Interrupt */ + rslt = set_accel_tap_int(int_config, dev); + break; + case BMI160_STEP_DETECT_INT: + + /* Step detector interrupt */ + rslt = set_accel_step_detect_int(int_config, dev); + break; + case BMI160_ACC_ORIENT_INT: + + /* Orientation interrupt */ + rslt = set_accel_orientation_int(int_config, dev); + break; + case BMI160_ACC_FLAT_INT: + + /* Flat detection interrupt */ + rslt = set_accel_flat_detect_int(int_config, dev); + break; + case BMI160_ACC_LOW_G_INT: + + /* Low-g interrupt */ + rslt = set_accel_low_g_int(int_config, dev); + break; + case BMI160_ACC_HIGH_G_INT: + + /* High-g interrupt */ + rslt = set_accel_high_g_int(int_config, dev); + break; + case BMI160_ACC_GYRO_DATA_RDY_INT: + + /* Data ready interrupt */ + rslt = set_accel_gyro_data_ready_int(int_config, dev); + break; + case BMI160_ACC_GYRO_FIFO_FULL_INT: + + /* Fifo full interrupt */ + rslt = set_fifo_full_int(int_config, dev); + break; + case BMI160_ACC_GYRO_FIFO_WATERMARK_INT: + + /* Fifo water-mark interrupt */ + rslt = set_fifo_watermark_int(int_config, dev); + break; + case BMI160_FIFO_TAG_INT_PIN: + + /* Fifo tagging feature support */ + /* Configure Interrupt pins */ + rslt = set_intr_pin_config(int_config, dev); + break; + default: + break; + } + + return rslt; +} + +/*! + * @brief This API enables or disable the step counter feature. + * 1 - enable step counter (0 - disable) + */ +int8_t bmi160_set_step_counter(uint8_t step_cnt_enable, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt != BMI160_OK) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + rslt = bmi160_get_regs(BMI160_INT_STEP_CONFIG_1_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + if (step_cnt_enable == BMI160_ENABLE) + { + data |= (uint8_t)(step_cnt_enable << 3); + } + else + { + data &= ~BMI160_STEP_COUNT_EN_BIT_MASK; + } + rslt = bmi160_set_regs(BMI160_INT_STEP_CONFIG_1_ADDR, &data, 1, dev); + } + } + + return rslt; +} + +/*! + * @brief This API reads the step counter value. + */ +int8_t bmi160_read_step_counter(uint16_t *step_val, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data[2] = { 0, 0 }; + uint16_t msb = 0; + uint8_t lsb = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt != BMI160_OK) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + rslt = bmi160_get_regs(BMI160_INT_STEP_CNT_0_ADDR, data, 2, dev); + if (rslt == BMI160_OK) + { + lsb = data[0]; + msb = data[1] << 8; + *step_val = msb | lsb; + } + } + + return rslt; +} + +/*! + * @brief This API reads the mention no of byte of data from the given + * register address of auxiliary sensor. + */ +int8_t bmi160_aux_read(uint8_t reg_addr, uint8_t *aux_data, uint16_t len, const struct bmi160_dev *dev) +{ + int8_t rslt = BMI160_OK; + uint16_t map_len = 0; + + /* Null-pointer check */ + if ((dev == NULL) || (dev->read == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + if (dev->aux_cfg.aux_sensor_enable == BMI160_ENABLE) + { + rslt = map_read_len(&map_len, dev); + if (rslt == BMI160_OK) + { + rslt = extract_aux_read(map_len, reg_addr, aux_data, len, dev); + } + } + else + { + rslt = BMI160_E_INVALID_INPUT; + } + } + + return rslt; +} + +/*! + * @brief This API writes the mention no of byte of data to the given + * register address of auxiliary sensor. + */ +int8_t bmi160_aux_write(uint8_t reg_addr, uint8_t *aux_data, uint16_t len, const struct bmi160_dev *dev) +{ + int8_t rslt = BMI160_OK; + uint8_t count = 0; + + /* Null-pointer check */ + if ((dev == NULL) || (dev->write == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + for (; count < len; count++) + { + /* set data to write */ + rslt = bmi160_set_regs(BMI160_AUX_IF_4_ADDR, aux_data, 1, dev); + dev->delay_ms(BMI160_AUX_COM_DELAY); + if (rslt == BMI160_OK) + { + /* set address to write */ + rslt = bmi160_set_regs(BMI160_AUX_IF_3_ADDR, ®_addr, 1, dev); + dev->delay_ms(BMI160_AUX_COM_DELAY); + if (rslt == BMI160_OK && (count < len - 1)) + { + aux_data++; + reg_addr++; + } + } + } + } + + return rslt; +} + +/*! + * @brief This API initialize the auxiliary sensor + * in order to access it. + */ +int8_t bmi160_aux_init(const struct bmi160_dev *dev) +{ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt != BMI160_OK) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + if (dev->aux_cfg.aux_sensor_enable == BMI160_ENABLE) + { + /* Configures the auxiliary sensor interface settings */ + rslt = config_aux_settg(dev); + } + else + { + rslt = BMI160_E_INVALID_INPUT; + } + } + + return rslt; +} + +/*! + * @brief This API is used to setup the auxiliary sensor of bmi160 in auto mode + * Thus enabling the auto update of 8 bytes of data from auxiliary sensor + * to BMI160 register address 0x04 to 0x0B + */ +int8_t bmi160_set_aux_auto_mode(uint8_t *data_addr, struct bmi160_dev *dev) +{ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt != BMI160_OK) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + if (dev->aux_cfg.aux_sensor_enable == BMI160_ENABLE) + { + /* Write the aux. address to read in 0x4D of BMI160*/ + rslt = bmi160_set_regs(BMI160_AUX_IF_2_ADDR, data_addr, 1, dev); + dev->delay_ms(BMI160_AUX_COM_DELAY); + if (rslt == BMI160_OK) + { + /* Configure the polling ODR for + * auxiliary sensor */ + rslt = config_aux_odr(dev); + if (rslt == BMI160_OK) + { + /* Disable the aux. manual mode, i.e aux. + * sensor is in auto-mode (data-mode) */ + dev->aux_cfg.manual_enable = BMI160_DISABLE; + rslt = bmi160_config_aux_mode(dev); + + /* Auxiliary sensor data is obtained + * in auto mode from this point */ + } + } + } + else + { + rslt = BMI160_E_INVALID_INPUT; + } + } + + return rslt; +} + +/*! + * @brief This API configures the 0x4C register and settings like + * Auxiliary sensor manual enable/ disable and aux burst read length. + */ +int8_t bmi160_config_aux_mode(const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t aux_if[2] = { (uint8_t)(dev->aux_cfg.aux_i2c_addr * 2), 0 }; + + rslt = bmi160_get_regs(BMI160_AUX_IF_1_ADDR, &aux_if[1], 1, dev); + if (rslt == BMI160_OK) + { + /* update the Auxiliary interface to manual/auto mode */ + aux_if[1] = BMI160_SET_BITS(aux_if[1], BMI160_MANUAL_MODE_EN, dev->aux_cfg.manual_enable); + + /* update the burst read length defined by user */ + aux_if[1] = BMI160_SET_BITS_POS_0(aux_if[1], BMI160_AUX_READ_BURST, dev->aux_cfg.aux_rd_burst_len); + + /* Set the secondary interface address and manual mode + * along with burst read length */ + rslt = bmi160_set_regs(BMI160_AUX_IF_0_ADDR, &aux_if[0], 2, dev); + dev->delay_ms(BMI160_AUX_COM_DELAY); + } + + return rslt; +} + +/*! + * @brief This API is used to read the raw uncompensated auxiliary sensor + * data of 8 bytes from BMI160 register address 0x04 to 0x0B + */ +int8_t bmi160_read_aux_data_auto_mode(uint8_t *aux_data, const struct bmi160_dev *dev) +{ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt != BMI160_OK) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + if ((dev->aux_cfg.aux_sensor_enable == BMI160_ENABLE) && (dev->aux_cfg.manual_enable == BMI160_DISABLE)) + { + /* Read the aux. sensor's raw data */ + rslt = bmi160_get_regs(BMI160_AUX_DATA_ADDR, aux_data, 8, dev); + } + else + { + rslt = BMI160_E_INVALID_INPUT; + } + } + + return rslt; +} + +/*! + * @brief This is used to perform self test of accel/gyro of the BMI160 sensor + */ +int8_t bmi160_perform_self_test(uint8_t select_sensor, struct bmi160_dev *dev) +{ + int8_t rslt; + int8_t self_test_rslt = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt != BMI160_OK) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + + /* Proceed if null check is fine */ + switch (select_sensor) + { + case BMI160_ACCEL_ONLY: + rslt = perform_accel_self_test(dev); + break; + case BMI160_GYRO_ONLY: + + /* Set the power mode as normal mode */ + dev->gyro_cfg.power = BMI160_GYRO_NORMAL_MODE; + rslt = bmi160_set_power_mode(dev); + + /* Perform gyro self test */ + if (rslt == BMI160_OK) + { + /* Perform gyro self test */ + rslt = perform_gyro_self_test(dev); + } + + break; + default: + rslt = BMI160_E_INVALID_INPUT; + break; + } + + /* Check to ensure bus error does not occur */ + if (rslt >= BMI160_OK) + { + /* Store the status of self test result */ + self_test_rslt = rslt; + + /* Perform soft reset */ + rslt = bmi160_soft_reset(dev); + + } + + /* Check to ensure bus operations are success */ + if (rslt == BMI160_OK) + { + /* Restore self_test_rslt as return value */ + rslt = self_test_rslt; + } + } + + return rslt; +} + +/*! + * @brief This API reads the data from fifo buffer. + */ +int8_t bmi160_get_fifo_data(struct bmi160_dev const *dev) +{ + int8_t rslt = 0; + uint16_t bytes_to_read = 0; + uint16_t user_fifo_len = 0; + + /* check the bmi160 structure as NULL*/ + if ((dev == NULL) || (dev->fifo->data == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + reset_fifo_data_structure(dev); + + /* get current FIFO fill-level*/ + rslt = get_fifo_byte_counter(&bytes_to_read, dev); + if (rslt == BMI160_OK) + { + user_fifo_len = dev->fifo->length; + if ((dev->fifo->length > bytes_to_read)) + { + /* Handling the case where user requests + * more data than available in FIFO */ + dev->fifo->length = bytes_to_read; + } + if ((dev->fifo->fifo_time_enable == BMI160_FIFO_TIME_ENABLE) && + (bytes_to_read + BMI160_FIFO_BYTES_OVERREAD <= user_fifo_len)) + { + /* Handling case of sensor time availability*/ + dev->fifo->length = dev->fifo->length + BMI160_FIFO_BYTES_OVERREAD; + } + + /* read only the filled bytes in the FIFO Buffer */ + rslt = bmi160_get_regs(BMI160_FIFO_DATA_ADDR, dev->fifo->data, dev->fifo->length, dev); + } + } + + return rslt; +} + +/*! + * @brief This API writes fifo_flush command to command register.This + * action clears all data in the Fifo without changing fifo configuration + * settings + */ +int8_t bmi160_set_fifo_flush(const struct bmi160_dev *dev) +{ + int8_t rslt = 0; + uint8_t data = BMI160_FIFO_FLUSH_VALUE; + uint8_t reg_addr = BMI160_COMMAND_REG_ADDR; + + /* Check the bmi160_dev structure for NULL address*/ + if (dev == NULL) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + rslt = bmi160_set_regs(reg_addr, &data, BMI160_ONE, dev); + } + + return rslt; +} + +/*! + * @brief This API sets the FIFO configuration in the sensor. + */ +int8_t bmi160_set_fifo_config(uint8_t config, uint8_t enable, struct bmi160_dev const *dev) +{ + int8_t rslt = 0; + uint8_t data = 0; + uint8_t reg_addr = BMI160_FIFO_CONFIG_1_ADDR; + uint8_t fifo_config = config & BMI160_FIFO_CONFIG_1_MASK; + + /* Check the bmi160_dev structure for NULL address*/ + if (dev == NULL) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + rslt = bmi160_get_regs(reg_addr, &data, BMI160_ONE, dev); + if (rslt == BMI160_OK) + { + if (fifo_config > 0) + { + if (enable == BMI160_ENABLE) + { + data = data | fifo_config; + } + else + { + data = data & (~fifo_config); + } + } + + /* write fifo frame content configuration*/ + rslt = bmi160_set_regs(reg_addr, &data, BMI160_ONE, dev); + if (rslt == BMI160_OK) + { + /* read fifo frame content configuration*/ + rslt = bmi160_get_regs(reg_addr, &data, BMI160_ONE, dev); + if (rslt == BMI160_OK) + { + /* extract fifo header enabled status */ + dev->fifo->fifo_header_enable = data & BMI160_FIFO_HEAD_ENABLE; + + /* extract accel/gyr/aux. data enabled status */ + dev->fifo->fifo_data_enable = data & BMI160_FIFO_M_G_A_ENABLE; + + /* extract fifo sensor time enabled status */ + dev->fifo->fifo_time_enable = data & BMI160_FIFO_TIME_ENABLE; + } + } + } + } + + return rslt; +} + +/*! @brief This API is used to configure the down sampling ratios of + * the accel and gyro data for FIFO.Also, it configures filtered or + * pre-filtered data for accel and gyro. + * + */ +int8_t bmi160_set_fifo_down(uint8_t fifo_down, const struct bmi160_dev *dev) +{ + int8_t rslt = 0; + uint8_t data = 0; + uint8_t reg_addr = BMI160_FIFO_DOWN_ADDR; + + /* Check the bmi160_dev structure for NULL address*/ + if (dev == NULL) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + rslt = bmi160_get_regs(reg_addr, &data, BMI160_ONE, dev); + if (rslt == BMI160_OK) + { + data = data | fifo_down; + rslt = bmi160_set_regs(reg_addr, &data, BMI160_ONE, dev); + } + } + + return rslt; +} + +/*! + * @brief This API sets the FIFO watermark level in the sensor. + * + */ +int8_t bmi160_set_fifo_wm(uint8_t fifo_wm, const struct bmi160_dev *dev) +{ + int8_t rslt = 0; + uint8_t data = fifo_wm; + uint8_t reg_addr = BMI160_FIFO_CONFIG_0_ADDR; + + /* Check the bmi160_dev structure for NULL address*/ + if (dev == NULL) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + rslt = bmi160_set_regs(reg_addr, &data, BMI160_ONE, dev); + } + + return rslt; +} + +/*! + * @brief This API parses and extracts the accelerometer frames from + * FIFO data read by the "bmi160_get_fifo_data" API and stores it in + * the "accel_data" structure instance. + */ +int8_t bmi160_extract_accel(struct bmi160_sensor_data *accel_data, uint8_t *accel_length, struct bmi160_dev const *dev) +{ + int8_t rslt = 0; + uint16_t data_index = 0; + uint16_t data_read_length = 0; + uint8_t accel_index = 0; + uint8_t fifo_data_enable = 0; + + if (dev == NULL || dev->fifo == NULL || dev->fifo->data == NULL) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /* Parsing the FIFO data in header-less mode */ + if (dev->fifo->fifo_header_enable == 0) + { + /* Number of bytes to be parsed from FIFO */ + get_accel_len_to_parse(&data_index, &data_read_length, accel_length, dev); + for (; data_index < data_read_length;) + { + /*Check for the availability of next two bytes of FIFO data */ + check_frame_validity(&data_index, dev); + fifo_data_enable = dev->fifo->fifo_data_enable; + unpack_accel_frame(accel_data, &data_index, &accel_index, fifo_data_enable, dev); + } + + /* update number of accel data read*/ + *accel_length = accel_index; + + /*update the accel byte index*/ + dev->fifo->accel_byte_start_idx = data_index; + } + else + { + /* Parsing the FIFO data in header mode */ + extract_accel_header_mode(accel_data, accel_length, dev); + } + } + + return rslt; +} + +/*! + * @brief This API parses and extracts the gyro frames from + * FIFO data read by the "bmi160_get_fifo_data" API and stores it in + * the "gyro_data" structure instance. + */ +int8_t bmi160_extract_gyro(struct bmi160_sensor_data *gyro_data, uint8_t *gyro_length, struct bmi160_dev const *dev) +{ + int8_t rslt = 0; + uint16_t data_index = 0; + uint16_t data_read_length = 0; + uint8_t gyro_index = 0; + uint8_t fifo_data_enable = 0; + + if (dev == NULL || dev->fifo->data == NULL) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /* Parsing the FIFO data in header-less mode */ + if (dev->fifo->fifo_header_enable == 0) + { + /* Number of bytes to be parsed from FIFO */ + get_gyro_len_to_parse(&data_index, &data_read_length, gyro_length, dev); + for (; data_index < data_read_length;) + { + /*Check for the availability of next two bytes of FIFO data */ + check_frame_validity(&data_index, dev); + fifo_data_enable = dev->fifo->fifo_data_enable; + unpack_gyro_frame(gyro_data, &data_index, &gyro_index, fifo_data_enable, dev); + } + + /* update number of gyro data read */ + *gyro_length = gyro_index; + + /* update the gyro byte index */ + dev->fifo->gyro_byte_start_idx = data_index; + } + else + { + /* Parsing the FIFO data in header mode */ + extract_gyro_header_mode(gyro_data, gyro_length, dev); + } + } + + return rslt; +} + +/*! + * @brief This API parses and extracts the aux frames from + * FIFO data read by the "bmi160_get_fifo_data" API and stores it in + * the "aux_data" structure instance. + */ +int8_t bmi160_extract_aux(struct bmi160_aux_data *aux_data, uint8_t *aux_len, struct bmi160_dev const *dev) +{ + int8_t rslt = 0; + uint16_t data_index = 0; + uint16_t data_read_length = 0; + uint8_t aux_index = 0; + uint8_t fifo_data_enable = 0; + + if ((dev == NULL) || (dev->fifo->data == NULL) || (aux_data == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /* Parsing the FIFO data in header-less mode */ + if (dev->fifo->fifo_header_enable == 0) + { + /* Number of bytes to be parsed from FIFO */ + get_aux_len_to_parse(&data_index, &data_read_length, aux_len, dev); + for (; data_index < data_read_length;) + { + /* Check for the availability of next two + * bytes of FIFO data */ + check_frame_validity(&data_index, dev); + fifo_data_enable = dev->fifo->fifo_data_enable; + unpack_aux_frame(aux_data, &data_index, &aux_index, fifo_data_enable, dev); + } + + /* update number of aux data read */ + *aux_len = aux_index; + + /* update the aux byte index */ + dev->fifo->aux_byte_start_idx = data_index; + } + else + { + /* Parsing the FIFO data in header mode */ + extract_aux_header_mode(aux_data, aux_len, dev); + } + } + + return rslt; +} + +/*! + * @brief This API starts the FOC of accel and gyro + * + * @note FOC should not be used in low-power mode of sensor + * + * @note Accel FOC targets values of +1g , 0g , -1g + * Gyro FOC always targets value of 0 dps + */ +int8_t bmi160_start_foc(const struct bmi160_foc_conf *foc_conf, + struct bmi160_offsets *offset, + struct bmi160_dev const *dev) +{ + int8_t rslt; + uint8_t data; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt != BMI160_OK) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /* Set the offset enable bits */ + rslt = configure_offset_enable(foc_conf, dev); + if (rslt == BMI160_OK) + { + /* Read the FOC config from the sensor */ + rslt = bmi160_get_regs(BMI160_FOC_CONF_ADDR, &data, 1, dev); + + /* Set the FOC config for gyro */ + data = BMI160_SET_BITS(data, BMI160_GYRO_FOC_EN, foc_conf->foc_gyr_en); + + /* Set the FOC config for accel xyz axes */ + data = BMI160_SET_BITS(data, BMI160_ACCEL_FOC_X_CONF, foc_conf->foc_acc_x); + data = BMI160_SET_BITS(data, BMI160_ACCEL_FOC_Y_CONF, foc_conf->foc_acc_y); + data = BMI160_SET_BITS_POS_0(data, BMI160_ACCEL_FOC_Z_CONF, foc_conf->foc_acc_z); + if (rslt == BMI160_OK) + { + /* Set the FOC config in the sensor */ + rslt = bmi160_set_regs(BMI160_FOC_CONF_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + /* Procedure to trigger + * FOC and check status */ + rslt = trigger_foc(offset, dev); + } + } + } + } + + return rslt; +} + +/*! + * @brief This API reads and stores the offset values of accel and gyro + */ +int8_t bmi160_get_offsets(struct bmi160_offsets *offset, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data[7]; + uint8_t lsb, msb; + int16_t offset_msb, offset_lsb; + int16_t offset_data; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt != BMI160_OK) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /* Read the FOC config from the sensor */ + rslt = bmi160_get_regs(BMI160_OFFSET_ADDR, data, 7, dev); + + /* Accel offsets */ + offset->off_acc_x = (int8_t)data[0]; + offset->off_acc_y = (int8_t)data[1]; + offset->off_acc_z = (int8_t)data[2]; + + /* Gyro x-axis offset */ + lsb = data[3]; + msb = BMI160_GET_BITS_POS_0(data[6], BMI160_GYRO_OFFSET_X); + offset_msb = (int16_t)(msb << 14); + offset_lsb = lsb << 6; + offset_data = offset_msb | offset_lsb; + + /* Divide by 64 to get the Right shift by 6 value */ + offset->off_gyro_x = (int16_t)(offset_data / 64); + + /* Gyro y-axis offset */ + lsb = data[4]; + msb = BMI160_GET_BITS(data[6], BMI160_GYRO_OFFSET_Y); + offset_msb = (int16_t)(msb << 14); + offset_lsb = lsb << 6; + offset_data = offset_msb | offset_lsb; + + /* Divide by 64 to get the Right shift by 6 value */ + offset->off_gyro_y = (int16_t)(offset_data / 64); + + /* Gyro z-axis offset */ + lsb = data[5]; + msb = BMI160_GET_BITS(data[6], BMI160_GYRO_OFFSET_Z); + offset_msb = (int16_t)(msb << 14); + offset_lsb = lsb << 6; + offset_data = offset_msb | offset_lsb; + + /* Divide by 64 to get the Right shift by 6 value */ + offset->off_gyro_z = (int16_t)(offset_data / 64); + } + + return rslt; +} + +/*! + * @brief This API writes the offset values of accel and gyro to + * the sensor but these values will be reset on POR or soft reset. + */ +int8_t bmi160_set_offsets(const struct bmi160_foc_conf *foc_conf, + const struct bmi160_offsets *offset, + struct bmi160_dev const *dev) +{ + int8_t rslt; + uint8_t data[7]; + uint8_t x_msb, y_msb, z_msb; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt != BMI160_OK) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /* Update the accel offset */ + data[0] = (uint8_t)offset->off_acc_x; + data[1] = (uint8_t)offset->off_acc_y; + data[2] = (uint8_t)offset->off_acc_z; + + /* Update the LSB of gyro offset */ + data[3] = BMI160_GET_LSB(offset->off_gyro_x); + data[4] = BMI160_GET_LSB(offset->off_gyro_y); + data[5] = BMI160_GET_LSB(offset->off_gyro_z); + + /* Update the MSB of gyro offset */ + x_msb = BMI160_GET_BITS(offset->off_gyro_x, BMI160_GYRO_OFFSET); + y_msb = BMI160_GET_BITS(offset->off_gyro_y, BMI160_GYRO_OFFSET); + z_msb = BMI160_GET_BITS(offset->off_gyro_z, BMI160_GYRO_OFFSET); + data[6] = (uint8_t)(z_msb << 4 | y_msb << 2 | x_msb); + + /* Set the offset enable/disable for gyro and accel */ + data[6] = BMI160_SET_BITS(data[6], BMI160_GYRO_OFFSET_EN, foc_conf->gyro_off_en); + data[6] = BMI160_SET_BITS(data[6], BMI160_ACCEL_OFFSET_EN, foc_conf->acc_off_en); + + /* Set the offset config and values in the sensor */ + rslt = bmi160_set_regs(BMI160_OFFSET_ADDR, data, 7, dev); + } + + return rslt; +} + +/*! + * @brief This API writes the image registers values to NVM which is + * stored even after POR or soft reset + */ +int8_t bmi160_update_nvm(struct bmi160_dev const *dev) +{ + int8_t rslt; + uint8_t data; + uint8_t cmd = BMI160_NVM_BACKUP_EN; + + /* Read the nvm_prog_en configuration */ + rslt = bmi160_get_regs(BMI160_CONF_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + data = BMI160_SET_BITS(data, BMI160_NVM_UPDATE, 1); + + /* Set the nvm_prog_en bit in the sensor */ + rslt = bmi160_set_regs(BMI160_CONF_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + /* Update NVM */ + rslt = bmi160_set_regs(BMI160_COMMAND_REG_ADDR, &cmd, 1, dev); + if (rslt == BMI160_OK) + { + /* Check for NVM ready status */ + rslt = bmi160_get_regs(BMI160_STATUS_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + data = BMI160_GET_BITS(data, BMI160_NVM_STATUS); + if (data != BMI160_ENABLE) + { + /* Delay to update NVM */ + dev->delay_ms(25); + } + } + } + } + } + + return rslt; +} + +/*! + * @brief This API gets the interrupt status from the sensor. + */ +int8_t bmi160_get_int_status(enum bmi160_int_status_sel int_status_sel, + union bmi160_int_status *int_status, + struct bmi160_dev const *dev) +{ + int8_t rslt = 0; + + /* To get the status of all interrupts */ + if (int_status_sel == BMI160_INT_STATUS_ALL) + { + rslt = bmi160_get_regs(BMI160_INT_STATUS_ADDR, &int_status->data[0], 4, dev); + } + else + { + if (int_status_sel & BMI160_INT_STATUS_0) + { + rslt = bmi160_get_regs(BMI160_INT_STATUS_ADDR, &int_status->data[0], 1, dev); + } + if (int_status_sel & BMI160_INT_STATUS_1) + { + rslt = bmi160_get_regs(BMI160_INT_STATUS_ADDR + 1, &int_status->data[1], 1, dev); + } + if (int_status_sel & BMI160_INT_STATUS_2) + { + rslt = bmi160_get_regs(BMI160_INT_STATUS_ADDR + 2, &int_status->data[2], 1, dev); + } + if (int_status_sel & BMI160_INT_STATUS_3) + { + rslt = bmi160_get_regs(BMI160_INT_STATUS_ADDR + 3, &int_status->data[3], 1, dev); + } + } + + return rslt; +} + +/*********************** Local function definitions ***************************/ + +/*! + * @brief This API sets the any-motion interrupt of the sensor. + * This interrupt occurs when accel values exceeds preset threshold + * for a certain period of time. + */ +static int8_t set_accel_any_motion_int(struct bmi160_int_settg *int_config, struct bmi160_dev *dev) +{ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt != BMI160_OK) || (int_config == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /* updating the interrupt structure to local structure */ + struct bmi160_acc_any_mot_int_cfg *any_motion_int_cfg = &(int_config->int_type_cfg.acc_any_motion_int); + rslt = enable_accel_any_motion_int(any_motion_int_cfg, dev); + if (rslt == BMI160_OK) + { + rslt = config_any_motion_int_settg(int_config, any_motion_int_cfg, dev); + } + } + + return rslt; +} + +/*! + * @brief This API sets tap interrupts.Interrupt is fired when + * tap movements happen. + */ +static int8_t set_accel_tap_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) +{ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt != BMI160_OK) || (int_config == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /* updating the interrupt structure to local structure */ + struct bmi160_acc_tap_int_cfg *tap_int_cfg = &(int_config->int_type_cfg.acc_tap_int); + rslt = enable_tap_int(int_config, tap_int_cfg, dev); + if (rslt == BMI160_OK) + { + /* Configure Interrupt pins */ + rslt = set_intr_pin_config(int_config, dev); + if (rslt == BMI160_OK) + { + rslt = config_tap_int_settg(int_config, tap_int_cfg, dev); + } + } + } + + return rslt; +} + +/*! + * @brief This API sets the data ready interrupt for both accel and gyro. + * This interrupt occurs when new accel and gyro data comes. + */ +static int8_t set_accel_gyro_data_ready_int(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) +{ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt != BMI160_OK) || (int_config == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + rslt = enable_data_ready_int(dev); + if (rslt == BMI160_OK) + { + /* Configure Interrupt pins */ + rslt = set_intr_pin_config(int_config, dev); + if (rslt == BMI160_OK) + { + rslt = map_hardware_interrupt(int_config, dev); + } + } + } + + return rslt; +} + +/*! + * @brief This API sets the significant motion interrupt of the sensor.This + * interrupt occurs when there is change in user location. + */ +static int8_t set_accel_sig_motion_int(struct bmi160_int_settg *int_config, struct bmi160_dev *dev) +{ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt != BMI160_OK) || (int_config == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /* updating the interrupt structure to local structure */ + struct bmi160_acc_sig_mot_int_cfg *sig_mot_int_cfg = &(int_config->int_type_cfg.acc_sig_motion_int); + rslt = enable_sig_motion_int(sig_mot_int_cfg, dev); + if (rslt == BMI160_OK) + { + rslt = config_sig_motion_int_settg(int_config, sig_mot_int_cfg, dev); + } + } + + return rslt; +} + +/*! + * @brief This API sets the no motion/slow motion interrupt of the sensor. + * Slow motion is similar to any motion interrupt.No motion interrupt + * occurs when slope bet. two accel values falls below preset threshold + * for preset duration. + */ +static int8_t set_accel_no_motion_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) +{ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt != BMI160_OK) || (int_config == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /* updating the interrupt structure to local structure */ + struct bmi160_acc_no_motion_int_cfg *no_mot_int_cfg = &(int_config->int_type_cfg.acc_no_motion_int); + rslt = enable_no_motion_int(no_mot_int_cfg, dev); + if (rslt == BMI160_OK) + { + /* Configure the INT PIN settings*/ + rslt = config_no_motion_int_settg(int_config, no_mot_int_cfg, dev); + } + } + + return rslt; +} + +/*! + * @brief This API sets the step detection interrupt.This interrupt + * occurs when the single step causes accel values to go above + * preset threshold. + */ +static int8_t set_accel_step_detect_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) +{ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt != BMI160_OK) || (int_config == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /* updating the interrupt structure to local structure */ + struct bmi160_acc_step_detect_int_cfg *step_detect_int_cfg = &(int_config->int_type_cfg.acc_step_detect_int); + rslt = enable_step_detect_int(step_detect_int_cfg, dev); + if (rslt == BMI160_OK) + { + /* Configure Interrupt pins */ + rslt = set_intr_pin_config(int_config, dev); + if (rslt == BMI160_OK) + { + rslt = map_feature_interrupt(int_config, dev); + if (rslt == BMI160_OK) + { + rslt = config_step_detect(step_detect_int_cfg, dev); + } + } + } + } + + return rslt; +} + +/*! + * @brief This API sets the orientation interrupt of the sensor.This + * interrupt occurs when there is orientation change in the sensor + * with respect to gravitational field vector g. + */ +static int8_t set_accel_orientation_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) +{ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt != BMI160_OK) || (int_config == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /* updating the interrupt structure to local structure */ + struct bmi160_acc_orient_int_cfg *orient_int_cfg = &(int_config->int_type_cfg.acc_orient_int); + rslt = enable_orient_int(orient_int_cfg, dev); + if (rslt == BMI160_OK) + { + /* Configure Interrupt pins */ + rslt = set_intr_pin_config(int_config, dev); + if (rslt == BMI160_OK) + { + /* map INT pin to orient interrupt */ + rslt = map_feature_interrupt(int_config, dev); + if (rslt == BMI160_OK) + { + /* configure the + * orientation setting*/ + rslt = config_orient_int_settg(orient_int_cfg, dev); + } + } + } + } + + return rslt; +} + +/*! + * @brief This API sets the flat interrupt of the sensor.This interrupt + * occurs in case of flat orientation + */ +static int8_t set_accel_flat_detect_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) +{ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt != BMI160_OK) || (int_config == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /* updating the interrupt structure to local structure */ + struct bmi160_acc_flat_detect_int_cfg *flat_detect_int = &(int_config->int_type_cfg.acc_flat_int); + + /* enable the flat interrupt */ + rslt = enable_flat_int(flat_detect_int, dev); + if (rslt == BMI160_OK) + { + /* Configure Interrupt pins */ + rslt = set_intr_pin_config(int_config, dev); + if (rslt == BMI160_OK) + { + /* map INT pin to flat interrupt */ + rslt = map_feature_interrupt(int_config, dev); + if (rslt == BMI160_OK) + { + /* configure the flat setting*/ + rslt = config_flat_int_settg(flat_detect_int, dev); + } + } + } + } + + return rslt; +} + +/*! + * @brief This API sets the low-g interrupt of the sensor.This interrupt + * occurs during free-fall. + */ +static int8_t set_accel_low_g_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) +{ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt != BMI160_OK) || (int_config == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /* updating the interrupt structure to local structure */ + struct bmi160_acc_low_g_int_cfg *low_g_int = &(int_config->int_type_cfg.acc_low_g_int); + + /* Enable the low-g interrupt*/ + rslt = enable_low_g_int(low_g_int, dev); + if (rslt == BMI160_OK) + { + /* Configure Interrupt pins */ + rslt = set_intr_pin_config(int_config, dev); + if (rslt == BMI160_OK) + { + /* Map INT pin to low-g interrupt */ + rslt = map_feature_interrupt(int_config, dev); + if (rslt == BMI160_OK) + { + /* configure the data source + * for low-g interrupt*/ + rslt = config_low_g_data_src(low_g_int, dev); + if (rslt == BMI160_OK) + { + rslt = config_low_g_int_settg(low_g_int, dev); + } + } + } + } + } + + return rslt; +} + +/*! + * @brief This API sets the high-g interrupt of the sensor.The interrupt + * occurs if the absolute value of acceleration data of any enabled axis + * exceeds the programmed threshold and the sign of the value does not + * change for a preset duration. + */ +static int8_t set_accel_high_g_int(struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) +{ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt != BMI160_OK) || (int_config == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /* updating the interrupt structure to local structure */ + struct bmi160_acc_high_g_int_cfg *high_g_int_cfg = &(int_config->int_type_cfg.acc_high_g_int); + + /* Enable the high-g interrupt */ + rslt = enable_high_g_int(high_g_int_cfg, dev); + if (rslt == BMI160_OK) + { + /* Configure Interrupt pins */ + rslt = set_intr_pin_config(int_config, dev); + if (rslt == BMI160_OK) + { + /* Map INT pin to high-g interrupt */ + rslt = map_feature_interrupt(int_config, dev); + if (rslt == BMI160_OK) + { + /* configure the data source + * for high-g interrupt*/ + rslt = config_high_g_data_src(high_g_int_cfg, dev); + if (rslt == BMI160_OK) + { + rslt = config_high_g_int_settg(high_g_int_cfg, dev); + } + } + } + } + } + + return rslt; +} + +/*! + * @brief This API configures the pins to fire the + * interrupt signal when it occurs. + */ +static int8_t set_intr_pin_config(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) +{ + int8_t rslt; + + /* configure the behavioural settings of interrupt pin */ + rslt = config_int_out_ctrl(int_config, dev); + if (rslt == BMI160_OK) + { + rslt = config_int_latch(int_config, dev); + } + + 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 bmi160_dev *dev) +{ + int8_t rslt; + + if ((dev == NULL) || (dev->read == NULL) || (dev->write == NULL) || (dev->delay_ms == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /* Device structure is fine */ + rslt = BMI160_OK; + } + + return rslt; +} + +/*! + * @brief This API sets the default configuration parameters of accel & gyro. + * Also maintain the previous state of configurations. + */ +static void default_param_settg(struct bmi160_dev *dev) +{ + /* Initializing accel and gyro params with + * default values */ + dev->accel_cfg.bw = BMI160_ACCEL_BW_NORMAL_AVG4; + dev->accel_cfg.odr = BMI160_ACCEL_ODR_100HZ; + dev->accel_cfg.power = BMI160_ACCEL_SUSPEND_MODE; + dev->accel_cfg.range = BMI160_ACCEL_RANGE_2G; + dev->gyro_cfg.bw = BMI160_GYRO_BW_NORMAL_MODE; + dev->gyro_cfg.odr = BMI160_GYRO_ODR_100HZ; + dev->gyro_cfg.power = BMI160_GYRO_SUSPEND_MODE; + dev->gyro_cfg.range = BMI160_GYRO_RANGE_2000_DPS; + + /* To maintain the previous state of accel configuration */ + dev->prev_accel_cfg = dev->accel_cfg; + + /* To maintain the previous state of gyro configuration */ + dev->prev_gyro_cfg = dev->gyro_cfg; +} + +/*! + * @brief This API set the accel configuration. + */ +static int8_t set_accel_conf(struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data[2] = { 0 }; + + rslt = check_accel_config(data, dev); + if (rslt == BMI160_OK) + { + /* Write output data rate and bandwidth */ + rslt = bmi160_set_regs(BMI160_ACCEL_CONFIG_ADDR, &data[0], 1, dev); + if (rslt == BMI160_OK) + { + dev->prev_accel_cfg.odr = dev->accel_cfg.odr; + dev->prev_accel_cfg.bw = dev->accel_cfg.bw; + + /* write accel range */ + rslt = bmi160_set_regs(BMI160_ACCEL_RANGE_ADDR, &data[1], 1, dev); + if (rslt == BMI160_OK) + { + dev->prev_accel_cfg.range = dev->accel_cfg.range; + } + } + } + + return rslt; +} + +/*! + * @brief This API check the accel configuration. + */ +static int8_t check_accel_config(uint8_t *data, const struct bmi160_dev *dev) +{ + int8_t rslt; + + /* read accel Output data rate and bandwidth */ + rslt = bmi160_get_regs(BMI160_ACCEL_CONFIG_ADDR, data, 2, dev); + if (rslt == BMI160_OK) + { + rslt = process_accel_odr(&data[0], dev); + if (rslt == BMI160_OK) + { + rslt = process_accel_bw(&data[0], dev); + if (rslt == BMI160_OK) + { + rslt = process_accel_range(&data[1], dev); + } + } + } + + return rslt; +} + +/*! + * @brief This API process the accel odr. + */ +static int8_t process_accel_odr(uint8_t *data, const struct bmi160_dev *dev) +{ + int8_t rslt = 0; + uint8_t temp = 0; + uint8_t odr = 0; + + if (dev->accel_cfg.odr <= BMI160_ACCEL_ODR_MAX) + { + if (dev->accel_cfg.odr != dev->prev_accel_cfg.odr) + { + odr = (uint8_t)dev->accel_cfg.odr; + temp = *data & ~BMI160_ACCEL_ODR_MASK; + + /* Adding output data rate */ + *data = temp | (odr & BMI160_ACCEL_ODR_MASK); + } + } + else + { + rslt = BMI160_E_OUT_OF_RANGE; + } + + return rslt; +} + +/*! + * @brief This API process the accel bandwidth. + */ +static int8_t process_accel_bw(uint8_t *data, const struct bmi160_dev *dev) +{ + int8_t rslt = 0; + uint8_t temp = 0; + uint8_t bw = 0; + + if (dev->accel_cfg.bw <= BMI160_ACCEL_BW_MAX) + { + if (dev->accel_cfg.bw != dev->prev_accel_cfg.bw) + { + bw = (uint8_t)dev->accel_cfg.bw; + temp = *data & ~BMI160_ACCEL_BW_MASK; + + /* Adding bandwidth */ + *data = temp | ((bw << 4) & BMI160_ACCEL_ODR_MASK); + } + } + else + { + rslt = BMI160_E_OUT_OF_RANGE; + } + + return rslt; +} + +/*! + * @brief This API process the accel range. + */ +static int8_t process_accel_range(uint8_t *data, const struct bmi160_dev *dev) +{ + int8_t rslt = 0; + uint8_t temp = 0; + uint8_t range = 0; + + if (dev->accel_cfg.range <= BMI160_ACCEL_RANGE_MAX) + { + if (dev->accel_cfg.range != dev->prev_accel_cfg.range) + { + range = (uint8_t)dev->accel_cfg.range; + temp = *data & ~BMI160_ACCEL_RANGE_MASK; + + /* Adding range */ + *data = temp | (range & BMI160_ACCEL_RANGE_MASK); + } + } + else + { + rslt = BMI160_E_OUT_OF_RANGE; + } + + return rslt; +} + +/*! + * @brief This API checks the invalid settings for ODR & Bw for + * Accel and Gyro. + */ +static int8_t check_invalid_settg(const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + + /* read the error reg */ + rslt = bmi160_get_regs(BMI160_ERROR_REG_ADDR, &data, 1, dev); + data = data >> 1; + data = data & BMI160_ERR_REG_MASK; + if (data == 1) + { + rslt = BMI160_E_ACCEL_ODR_BW_INVALID; + } + else if (data == 2) + { + rslt = BMI160_E_GYRO_ODR_BW_INVALID; + } + else if (data == 3) + { + rslt = BMI160_E_LWP_PRE_FLTR_INT_INVALID; + } + else if (data == 7) + { + rslt = BMI160_E_LWP_PRE_FLTR_INVALID; + } + + return rslt; +} +static int8_t set_gyro_conf(struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data[2] = { 0 }; + + rslt = check_gyro_config(data, dev); + if (rslt == BMI160_OK) + { + /* Write output data rate and bandwidth */ + rslt = bmi160_set_regs(BMI160_GYRO_CONFIG_ADDR, &data[0], 1, dev); + if (rslt == BMI160_OK) + { + dev->prev_gyro_cfg.odr = dev->gyro_cfg.odr; + dev->prev_gyro_cfg.bw = dev->gyro_cfg.bw; + + /* Write gyro range */ + rslt = bmi160_set_regs(BMI160_GYRO_RANGE_ADDR, &data[1], 1, dev); + if (rslt == BMI160_OK) + { + dev->prev_gyro_cfg.range = dev->gyro_cfg.range; + } + } + } + + return rslt; +} + +/*! + * @brief This API check the gyro configuration. + */ +static int8_t check_gyro_config(uint8_t *data, const struct bmi160_dev *dev) +{ + int8_t rslt; + + /* read gyro Output data rate and bandwidth */ + rslt = bmi160_get_regs(BMI160_GYRO_CONFIG_ADDR, data, 2, dev); + if (rslt == BMI160_OK) + { + rslt = process_gyro_odr(&data[0], dev); + if (rslt == BMI160_OK) + { + rslt = process_gyro_bw(&data[0], dev); + if (rslt == BMI160_OK) + { + rslt = process_gyro_range(&data[1], dev); + } + } + } + + return rslt; +} + +/*! + * @brief This API process the gyro odr. + */ +static int8_t process_gyro_odr(uint8_t *data, const struct bmi160_dev *dev) +{ + int8_t rslt = 0; + uint8_t temp = 0; + uint8_t odr = 0; + + if (dev->gyro_cfg.odr <= BMI160_GYRO_ODR_MAX) + { + if (dev->gyro_cfg.odr != dev->prev_gyro_cfg.odr) + { + odr = (uint8_t)dev->gyro_cfg.odr; + temp = (*data & ~BMI160_GYRO_ODR_MASK); + + /* Adding output data rate */ + *data = temp | (odr & BMI160_GYRO_ODR_MASK); + } + } + else + { + rslt = BMI160_E_OUT_OF_RANGE; + } + + return rslt; +} + +/*! + * @brief This API process the gyro bandwidth. + */ +static int8_t process_gyro_bw(uint8_t *data, const struct bmi160_dev *dev) +{ + int8_t rslt = 0; + uint8_t temp = 0; + uint8_t bw = 0; + + if (dev->gyro_cfg.bw <= BMI160_GYRO_BW_MAX) + { + bw = (uint8_t)dev->gyro_cfg.bw; + temp = *data & ~BMI160_GYRO_BW_MASK; + + /* Adding bandwidth */ + *data = temp | ((bw << 4) & BMI160_GYRO_BW_MASK); + } + else + { + rslt = BMI160_E_OUT_OF_RANGE; + } + + return rslt; +} + +/*! + * @brief This API process the gyro range. + */ +static int8_t process_gyro_range(uint8_t *data, const struct bmi160_dev *dev) +{ + int8_t rslt = 0; + uint8_t temp = 0; + uint8_t range = 0; + + if (dev->gyro_cfg.range <= BMI160_GYRO_RANGE_MAX) + { + if (dev->gyro_cfg.range != dev->prev_gyro_cfg.range) + { + range = (uint8_t)dev->gyro_cfg.range; + temp = *data & ~BMI160_GYRO_RANGE_MSK; + + /* Adding range */ + *data = temp | (range & BMI160_GYRO_RANGE_MSK); + } + } + else + { + rslt = BMI160_E_OUT_OF_RANGE; + } + + return rslt; +} + +/*! + * @brief This API sets the accel power. + */ +static int8_t set_accel_pwr(struct bmi160_dev *dev) +{ + int8_t rslt = 0; + uint8_t data = 0; + + if ((dev->accel_cfg.power >= BMI160_ACCEL_SUSPEND_MODE) && (dev->accel_cfg.power <= BMI160_ACCEL_LOWPOWER_MODE)) + { + if (dev->accel_cfg.power != dev->prev_accel_cfg.power) + { + rslt = process_under_sampling(&data, dev); + if (rslt == BMI160_OK) + { + /* Write accel power */ + rslt = bmi160_set_regs(BMI160_COMMAND_REG_ADDR, &dev->accel_cfg.power, 1, dev); + + /* Add delay of 3.8 ms - refer data sheet table 24*/ + if (dev->prev_accel_cfg.power == BMI160_ACCEL_SUSPEND_MODE) + { + dev->delay_ms(BMI160_ACCEL_DELAY_MS); + } + dev->prev_accel_cfg.power = dev->accel_cfg.power; + } + } + } + else + { + rslt = BMI160_E_OUT_OF_RANGE; + } + + return rslt; +} + +/*! + * @brief This API process the undersampling setting of Accel. + */ +static int8_t process_under_sampling(uint8_t *data, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t temp = 0; + uint8_t pre_filter = 0; + + rslt = bmi160_get_regs(BMI160_ACCEL_CONFIG_ADDR, data, 1, dev); + if (rslt == BMI160_OK) + { + if (dev->accel_cfg.power == BMI160_ACCEL_LOWPOWER_MODE) + { + temp = *data & ~BMI160_ACCEL_UNDERSAMPLING_MASK; + + /* Set under-sampling parameter */ + *data = temp | ((1 << 7) & BMI160_ACCEL_UNDERSAMPLING_MASK); + + /* Write data */ + rslt = bmi160_set_regs(BMI160_ACCEL_CONFIG_ADDR, data, 1, dev); + + /* disable the pre-filter data in + * low power mode */ + if (rslt == BMI160_OK) + { + /* Disable the Pre-filter data*/ + rslt = bmi160_set_regs(BMI160_INT_DATA_0_ADDR, &pre_filter, 2, dev); + } + } + else if (*data & BMI160_ACCEL_UNDERSAMPLING_MASK) + { + temp = *data & ~BMI160_ACCEL_UNDERSAMPLING_MASK; + + /* disable under-sampling parameter + * if already enabled */ + *data = temp; + + /* Write data */ + rslt = bmi160_set_regs(BMI160_ACCEL_CONFIG_ADDR, data, 1, dev); + } + } + + return rslt; +} + +/*! + * @brief This API sets the gyro power mode. + */ +static int8_t set_gyro_pwr(struct bmi160_dev *dev) +{ + int8_t rslt = 0; + + if ((dev->gyro_cfg.power == BMI160_GYRO_SUSPEND_MODE) || (dev->gyro_cfg.power == BMI160_GYRO_NORMAL_MODE) || + (dev->gyro_cfg.power == BMI160_GYRO_FASTSTARTUP_MODE)) + { + if (dev->gyro_cfg.power != dev->prev_gyro_cfg.power) + { + /* Write gyro power */ + rslt = bmi160_set_regs(BMI160_COMMAND_REG_ADDR, &dev->gyro_cfg.power, 1, dev); + if (dev->prev_gyro_cfg.power == BMI160_GYRO_SUSPEND_MODE) + { + /* Delay of 80 ms - datasheet Table 24 */ + dev->delay_ms(BMI160_GYRO_DELAY_MS); + } + else if ((dev->prev_gyro_cfg.power == BMI160_GYRO_FASTSTARTUP_MODE) && + (dev->gyro_cfg.power == BMI160_GYRO_NORMAL_MODE)) + { + /* This delay is required for transition from + * fast-startup mode to normal mode - datasheet Table 3 */ + dev->delay_ms(10); + } + else + { + /* do nothing */ + } + dev->prev_gyro_cfg.power = dev->gyro_cfg.power; + } + } + else + { + rslt = BMI160_E_OUT_OF_RANGE; + } + + return rslt; +} + +/*! + * @brief This API reads accel data along with sensor time if time is requested + * by user. Kindly refer the user guide(README.md) for more info. + */ +static int8_t get_accel_data(uint8_t len, struct bmi160_sensor_data *accel, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t idx = 0; + uint8_t data_array[9] = { 0 }; + uint8_t time_0 = 0; + uint16_t time_1 = 0; + uint32_t time_2 = 0; + uint8_t lsb; + uint8_t msb; + int16_t msblsb; + + /* read accel sensor data along with time if requested */ + rslt = bmi160_get_regs(BMI160_ACCEL_DATA_ADDR, data_array, 6 + len, dev); + if (rslt == BMI160_OK) + { + /* Accel Data */ + lsb = data_array[idx++]; + msb = data_array[idx++]; + msblsb = (int16_t)((msb << 8) | lsb); + accel->x = msblsb; /* Data in X axis */ + lsb = data_array[idx++]; + msb = data_array[idx++]; + msblsb = (int16_t)((msb << 8) | lsb); + accel->y = msblsb; /* Data in Y axis */ + lsb = data_array[idx++]; + msb = data_array[idx++]; + msblsb = (int16_t)((msb << 8) | lsb); + accel->z = msblsb; /* Data in Z axis */ + if (len == 3) + { + time_0 = data_array[idx++]; + time_1 = (uint16_t)(data_array[idx++] << 8); + time_2 = (uint32_t)(data_array[idx++] << 16); + accel->sensortime = (uint32_t)(time_2 | time_1 | time_0); + } + else + { + accel->sensortime = 0; + } + } + else + { + rslt = BMI160_E_COM_FAIL; + } + + return rslt; +} + +/*! + * @brief This API reads accel data along with sensor time if time is requested + * by user. Kindly refer the user guide(README.md) for more info. + */ +static int8_t get_gyro_data(uint8_t len, struct bmi160_sensor_data *gyro, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t idx = 0; + uint8_t data_array[15] = { 0 }; + uint8_t time_0 = 0; + uint16_t time_1 = 0; + uint32_t time_2 = 0; + uint8_t lsb; + uint8_t msb; + int16_t msblsb; + + if (len == 0) + { + /* read gyro data only */ + rslt = bmi160_get_regs(BMI160_GYRO_DATA_ADDR, data_array, 6, dev); + if (rslt == BMI160_OK) + { + /* Gyro Data */ + lsb = data_array[idx++]; + msb = data_array[idx++]; + msblsb = (int16_t)((msb << 8) | lsb); + gyro->x = msblsb; /* Data in X axis */ + lsb = data_array[idx++]; + msb = data_array[idx++]; + msblsb = (int16_t)((msb << 8) | lsb); + gyro->y = msblsb; /* Data in Y axis */ + lsb = data_array[idx++]; + msb = data_array[idx++]; + msblsb = (int16_t)((msb << 8) | lsb); + gyro->z = msblsb; /* Data in Z axis */ + gyro->sensortime = 0; + } + else + { + rslt = BMI160_E_COM_FAIL; + } + } + else + { + /* read gyro sensor data along with time */ + rslt = bmi160_get_regs(BMI160_GYRO_DATA_ADDR, data_array, 12 + len, dev); + if (rslt == BMI160_OK) + { + /* Gyro Data */ + lsb = data_array[idx++]; + msb = data_array[idx++]; + msblsb = (int16_t)((msb << 8) | lsb); + gyro->x = msblsb; /* gyro X axis data */ + lsb = data_array[idx++]; + msb = data_array[idx++]; + msblsb = (int16_t)((msb << 8) | lsb); + gyro->y = msblsb; /* gyro Y axis data */ + lsb = data_array[idx++]; + msb = data_array[idx++]; + msblsb = (int16_t)((msb << 8) | lsb); + gyro->z = msblsb; /* gyro Z axis data */ + idx = idx + 6; + time_0 = data_array[idx++]; + time_1 = (uint16_t)(data_array[idx++] << 8); + time_2 = (uint32_t)(data_array[idx++] << 16); + gyro->sensortime = (uint32_t)(time_2 | time_1 | time_0); + } + else + { + rslt = BMI160_E_COM_FAIL; + } + } + + return rslt; +} + +/*! + * @brief This API reads accel and gyro data along with sensor time + * if time is requested by user. + * Kindly refer the user guide(README.md) for more info. + */ +static int8_t get_accel_gyro_data(uint8_t len, + struct bmi160_sensor_data *accel, + struct bmi160_sensor_data *gyro, + const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t idx = 0; + uint8_t data_array[15] = { 0 }; + uint8_t time_0 = 0; + uint16_t time_1 = 0; + uint32_t time_2 = 0; + uint8_t lsb; + uint8_t msb; + int16_t msblsb; + + /* read both accel and gyro sensor data + * along with time if requested */ + rslt = bmi160_get_regs(BMI160_GYRO_DATA_ADDR, data_array, 12 + len, dev); + if (rslt == BMI160_OK) + { + /* Gyro Data */ + lsb = data_array[idx++]; + msb = data_array[idx++]; + msblsb = (int16_t)((msb << 8) | lsb); + gyro->x = msblsb; /* gyro X axis data */ + lsb = data_array[idx++]; + msb = data_array[idx++]; + msblsb = (int16_t)((msb << 8) | lsb); + gyro->y = msblsb; /* gyro Y axis data */ + lsb = data_array[idx++]; + msb = data_array[idx++]; + msblsb = (int16_t)((msb << 8) | lsb); + gyro->z = msblsb; /* gyro Z axis data */ + /* Accel Data */ + lsb = data_array[idx++]; + msb = data_array[idx++]; + msblsb = (int16_t)((msb << 8) | lsb); + accel->x = (int16_t)msblsb; /* accel X axis data */ + lsb = data_array[idx++]; + msb = data_array[idx++]; + msblsb = (int16_t)((msb << 8) | lsb); + accel->y = (int16_t)msblsb; /* accel Y axis data */ + lsb = data_array[idx++]; + msb = data_array[idx++]; + msblsb = (int16_t)((msb << 8) | lsb); + accel->z = (int16_t)msblsb; /* accel Z axis data */ + if (len == 3) + { + time_0 = data_array[idx++]; + time_1 = (uint16_t)(data_array[idx++] << 8); + time_2 = (uint32_t)(data_array[idx++] << 16); + accel->sensortime = (uint32_t)(time_2 | time_1 | time_0); + gyro->sensortime = (uint32_t)(time_2 | time_1 | time_0); + } + else + { + accel->sensortime = 0; + gyro->sensortime = 0; + } + } + else + { + rslt = BMI160_E_COM_FAIL; + } + + return rslt; +} + +/*! + * @brief This API enables the any-motion interrupt for accel. + */ +static int8_t enable_accel_any_motion_int(const struct bmi160_acc_any_mot_int_cfg *any_motion_int_cfg, + struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + + /* Enable any motion x, any motion y, any motion z + * in Int Enable 0 register */ + rslt = bmi160_get_regs(BMI160_INT_ENABLE_0_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + if (any_motion_int_cfg->anymotion_en == BMI160_ENABLE) + { + temp = data & ~BMI160_ANY_MOTION_X_INT_EN_MASK; + + /* Adding Any_motion x axis */ + data = temp | (any_motion_int_cfg->anymotion_x & BMI160_ANY_MOTION_X_INT_EN_MASK); + temp = data & ~BMI160_ANY_MOTION_Y_INT_EN_MASK; + + /* Adding Any_motion y axis */ + data = temp | ((any_motion_int_cfg->anymotion_y << 1) & BMI160_ANY_MOTION_Y_INT_EN_MASK); + temp = data & ~BMI160_ANY_MOTION_Z_INT_EN_MASK; + + /* Adding Any_motion z axis */ + data = temp | ((any_motion_int_cfg->anymotion_z << 2) & BMI160_ANY_MOTION_Z_INT_EN_MASK); + + /* any-motion feature selected*/ + dev->any_sig_sel = BMI160_ANY_MOTION_ENABLED; + } + else + { + data = data & ~BMI160_ANY_MOTION_ALL_INT_EN_MASK; + + /* neither any-motion feature nor sig-motion selected */ + dev->any_sig_sel = BMI160_BOTH_ANY_SIG_MOTION_DISABLED; + } + + /* write data to Int Enable 0 register */ + rslt = bmi160_set_regs(BMI160_INT_ENABLE_0_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API disable the sig-motion interrupt. + */ +static int8_t disable_sig_motion_int(const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + + /* Disabling Significant motion interrupt if enabled */ + rslt = bmi160_get_regs(BMI160_INT_MOTION_3_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + temp = (data & BMI160_SIG_MOTION_SEL_MASK); + if (temp) + { + temp = data & ~BMI160_SIG_MOTION_SEL_MASK; + data = temp; + + /* Write data to register */ + rslt = bmi160_set_regs(BMI160_INT_MOTION_3_ADDR, &data, 1, dev); + } + } + + return rslt; +} + +/*! + * @brief This API is used to map/unmap the Any/Sig motion, Step det/Low-g, + * Double tap, Single tap, Orientation, Flat, High-G, Nomotion interrupt pins. + */ +static int8_t map_feature_interrupt(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data[3] = { 0, 0, 0 }; + uint8_t temp[3] = { 0, 0, 0 }; + + rslt = bmi160_get_regs(BMI160_INT_MAP_0_ADDR, data, 3, dev); + if (rslt == BMI160_OK) + { + temp[0] = data[0] & ~int_mask_lookup_table[int_config->int_type]; + temp[2] = data[2] & ~int_mask_lookup_table[int_config->int_type]; + switch (int_config->int_channel) + { + case BMI160_INT_CHANNEL_NONE: + data[0] = temp[0]; + data[2] = temp[2]; + break; + case BMI160_INT_CHANNEL_1: + data[0] = temp[0] | int_mask_lookup_table[int_config->int_type]; + data[2] = temp[2]; + break; + case BMI160_INT_CHANNEL_2: + data[2] = temp[2] | int_mask_lookup_table[int_config->int_type]; + data[0] = temp[0]; + break; + case BMI160_INT_CHANNEL_BOTH: + data[0] = temp[0] | int_mask_lookup_table[int_config->int_type]; + data[2] = temp[2] | int_mask_lookup_table[int_config->int_type]; + break; + default: + rslt = BMI160_E_OUT_OF_RANGE; + } + if (rslt == BMI160_OK) + { + rslt = bmi160_set_regs(BMI160_INT_MAP_0_ADDR, data, 3, dev); + } + } + + return rslt; +} + +/*! + * @brief This API is used to map/unmap the Dataready(Accel & Gyro), FIFO full + * and FIFO watermark interrupt. + */ +static int8_t map_hardware_interrupt(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + + rslt = bmi160_get_regs(BMI160_INT_MAP_1_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + temp = data & ~int_mask_lookup_table[int_config->int_type]; + temp = temp & ~((uint8_t)(int_mask_lookup_table[int_config->int_type] << 4)); + switch (int_config->int_channel) + { + case BMI160_INT_CHANNEL_NONE: + data = temp; + break; + case BMI160_INT_CHANNEL_1: + data = temp | (uint8_t)((int_mask_lookup_table[int_config->int_type]) << 4); + break; + case BMI160_INT_CHANNEL_2: + data = temp | int_mask_lookup_table[int_config->int_type]; + break; + case BMI160_INT_CHANNEL_BOTH: + data = temp | int_mask_lookup_table[int_config->int_type]; + data = data | (uint8_t)((int_mask_lookup_table[int_config->int_type]) << 4); + break; + default: + rslt = BMI160_E_OUT_OF_RANGE; + } + if (rslt == BMI160_OK) + { + rslt = bmi160_set_regs(BMI160_INT_MAP_1_ADDR, &data, 1, dev); + } + } + + return rslt; +} + +/*! + * @brief This API configure the source of data(filter & pre-filter) + * for any-motion interrupt. + */ +static int8_t config_any_motion_src(const struct bmi160_acc_any_mot_int_cfg *any_motion_int_cfg, + const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + + /* Configure Int data 1 register to add source of interrupt */ + rslt = bmi160_get_regs(BMI160_INT_DATA_1_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + temp = data & ~BMI160_MOTION_SRC_INT_MASK; + data = temp | ((any_motion_int_cfg->anymotion_data_src << 7) & BMI160_MOTION_SRC_INT_MASK); + + /* Write data to DATA 1 address */ + rslt = bmi160_set_regs(BMI160_INT_DATA_1_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API configure the duration and threshold of + * any-motion interrupt. + */ +static int8_t config_any_dur_threshold(const struct bmi160_acc_any_mot_int_cfg *any_motion_int_cfg, + const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + uint8_t data_array[2] = { 0 }; + uint8_t dur; + + /* Configure Int Motion 0 register */ + rslt = bmi160_get_regs(BMI160_INT_MOTION_0_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + /* slope duration */ + dur = (uint8_t)any_motion_int_cfg->anymotion_dur; + temp = data & ~BMI160_SLOPE_INT_DUR_MASK; + data = temp | (dur & BMI160_MOTION_SRC_INT_MASK); + data_array[0] = data; + + /* add slope threshold */ + data_array[1] = any_motion_int_cfg->anymotion_thr; + + /* INT MOTION 0 and INT MOTION 1 address lie consecutively, + * hence writing data to respective registers at one go */ + + /* Writing to Int_motion 0 and + * Int_motion 1 Address simultaneously */ + rslt = bmi160_set_regs(BMI160_INT_MOTION_0_ADDR, data_array, 2, dev); + } + + return rslt; +} + +/*! + * @brief This API configure necessary setting of any-motion interrupt. + */ +static int8_t config_any_motion_int_settg(const struct bmi160_int_settg *int_config, + const struct bmi160_acc_any_mot_int_cfg *any_motion_int_cfg, + const struct bmi160_dev *dev) +{ + int8_t rslt; + + /* Configure Interrupt pins */ + rslt = set_intr_pin_config(int_config, dev); + if (rslt == BMI160_OK) + { + rslt = disable_sig_motion_int(dev); + if (rslt == BMI160_OK) + { + rslt = map_feature_interrupt(int_config, dev); + if (rslt == BMI160_OK) + { + rslt = config_any_motion_src(any_motion_int_cfg, dev); + if (rslt == BMI160_OK) + { + rslt = config_any_dur_threshold(any_motion_int_cfg, dev); + } + } + } + } + + return rslt; +} + +/*! + * @brief This API enable the data ready interrupt. + */ +static int8_t enable_data_ready_int(const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + + /* Enable data ready interrupt in Int Enable 1 register */ + rslt = bmi160_get_regs(BMI160_INT_ENABLE_1_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + temp = data & ~BMI160_DATA_RDY_INT_EN_MASK; + data = temp | ((1 << 4) & BMI160_DATA_RDY_INT_EN_MASK); + + /* Writing data to INT ENABLE 1 Address */ + rslt = bmi160_set_regs(BMI160_INT_ENABLE_1_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API enables the no motion/slow motion interrupt. + */ +static int8_t enable_no_motion_int(const struct bmi160_acc_no_motion_int_cfg *no_mot_int_cfg, + const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + + /* Enable no motion x, no motion y, no motion z + * in Int Enable 2 register */ + rslt = bmi160_get_regs(BMI160_INT_ENABLE_2_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + if (no_mot_int_cfg->no_motion_x == 1) + { + temp = data & ~BMI160_NO_MOTION_X_INT_EN_MASK; + + /* Adding No_motion x axis */ + data = temp | (1 & BMI160_NO_MOTION_X_INT_EN_MASK); + } + if (no_mot_int_cfg->no_motion_y == 1) + { + temp = data & ~BMI160_NO_MOTION_Y_INT_EN_MASK; + + /* Adding No_motion x axis */ + data = temp | ((1 << 1) & BMI160_NO_MOTION_Y_INT_EN_MASK); + } + if (no_mot_int_cfg->no_motion_z == 1) + { + temp = data & ~BMI160_NO_MOTION_Z_INT_EN_MASK; + + /* Adding No_motion x axis */ + data = temp | ((1 << 2) & BMI160_NO_MOTION_Z_INT_EN_MASK); + } + + /* write data to Int Enable 2 register */ + rslt = bmi160_set_regs(BMI160_INT_ENABLE_2_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API configure the interrupt PIN setting for + * no motion/slow motion interrupt. + */ +static int8_t config_no_motion_int_settg(const struct bmi160_int_settg *int_config, + const struct bmi160_acc_no_motion_int_cfg *no_mot_int_cfg, + const struct bmi160_dev *dev) +{ + int8_t rslt; + + /* Configure Interrupt pins */ + rslt = set_intr_pin_config(int_config, dev); + if (rslt == BMI160_OK) + { + rslt = map_feature_interrupt(int_config, dev); + if (rslt == BMI160_OK) + { + rslt = config_no_motion_data_src(no_mot_int_cfg, dev); + if (rslt == BMI160_OK) + { + rslt = config_no_motion_dur_thr(no_mot_int_cfg, dev); + } + } + } + + return rslt; +} + +/*! + * @brief This API configure the source of interrupt for no motion. + */ +static int8_t config_no_motion_data_src(const struct bmi160_acc_no_motion_int_cfg *no_mot_int_cfg, + const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + + /* Configure Int data 1 register to add source of interrupt */ + rslt = bmi160_get_regs(BMI160_INT_DATA_1_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + temp = data & ~BMI160_MOTION_SRC_INT_MASK; + data = temp | ((no_mot_int_cfg->no_motion_src << 7) & BMI160_MOTION_SRC_INT_MASK); + + /* Write data to DATA 1 address */ + rslt = bmi160_set_regs(BMI160_INT_DATA_1_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API configure the duration and threshold of + * no motion/slow motion interrupt along with selection of no/slow motion. + */ +static int8_t config_no_motion_dur_thr(const struct bmi160_acc_no_motion_int_cfg *no_mot_int_cfg, + const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + uint8_t temp_1 = 0; + uint8_t reg_addr; + uint8_t data_array[2] = { 0 }; + + /* Configuring INT_MOTION register */ + reg_addr = BMI160_INT_MOTION_0_ADDR; + rslt = bmi160_get_regs(reg_addr, &data, 1, dev); + if (rslt == BMI160_OK) + { + temp = data & ~BMI160_NO_MOTION_INT_DUR_MASK; + + /* Adding no_motion duration */ + data = temp | ((no_mot_int_cfg->no_motion_dur << 2) & BMI160_NO_MOTION_INT_DUR_MASK); + + /* Write data to NO_MOTION 0 address */ + rslt = bmi160_set_regs(reg_addr, &data, 1, dev); + if (rslt == BMI160_OK) + { + reg_addr = BMI160_INT_MOTION_3_ADDR; + rslt = bmi160_get_regs(reg_addr, &data, 1, dev); + if (rslt == BMI160_OK) + { + temp = data & ~BMI160_NO_MOTION_SEL_BIT_MASK; + + /* Adding no_motion_sel bit */ + temp_1 = (no_mot_int_cfg->no_motion_sel & BMI160_NO_MOTION_SEL_BIT_MASK); + data = (temp | temp_1); + data_array[1] = data; + + /* Adding no motion threshold */ + data_array[0] = no_mot_int_cfg->no_motion_thres; + reg_addr = BMI160_INT_MOTION_2_ADDR; + + /* writing data to INT_MOTION 2 and INT_MOTION 3 + * address simultaneously */ + rslt = bmi160_set_regs(reg_addr, data_array, 2, dev); + } + } + } + + return rslt; +} + +/*! + * @brief This API enables the sig-motion motion interrupt. + */ +static int8_t enable_sig_motion_int(const struct bmi160_acc_sig_mot_int_cfg *sig_mot_int_cfg, struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + + /* For significant motion,enable any motion x,any motion y, + * any motion z in Int Enable 0 register */ + rslt = bmi160_get_regs(BMI160_INT_ENABLE_0_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + if (sig_mot_int_cfg->sig_en == BMI160_ENABLE) + { + temp = data & ~BMI160_SIG_MOTION_INT_EN_MASK; + data = temp | (7 & BMI160_SIG_MOTION_INT_EN_MASK); + + /* sig-motion feature selected*/ + dev->any_sig_sel = BMI160_SIG_MOTION_ENABLED; + } + else + { + data = data & ~BMI160_SIG_MOTION_INT_EN_MASK; + + /* neither any-motion feature nor sig-motion selected */ + dev->any_sig_sel = BMI160_BOTH_ANY_SIG_MOTION_DISABLED; + } + + /* write data to Int Enable 0 register */ + rslt = bmi160_set_regs(BMI160_INT_ENABLE_0_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API configure the interrupt PIN setting for + * significant motion interrupt. + */ +static int8_t config_sig_motion_int_settg(const struct bmi160_int_settg *int_config, + const struct bmi160_acc_sig_mot_int_cfg *sig_mot_int_cfg, + const struct bmi160_dev *dev) +{ + int8_t rslt; + + /* Configure Interrupt pins */ + rslt = set_intr_pin_config(int_config, dev); + if (rslt == BMI160_OK) + { + rslt = map_feature_interrupt(int_config, dev); + if (rslt == BMI160_OK) + { + rslt = config_sig_motion_data_src(sig_mot_int_cfg, dev); + if (rslt == BMI160_OK) + { + rslt = config_sig_dur_threshold(sig_mot_int_cfg, dev); + } + } + } + + return rslt; +} + +/*! + * @brief This API configure the source of data(filter & pre-filter) + * for sig motion interrupt. + */ +static int8_t config_sig_motion_data_src(const struct bmi160_acc_sig_mot_int_cfg *sig_mot_int_cfg, + const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + + /* Configure Int data 1 register to add source of interrupt */ + rslt = bmi160_get_regs(BMI160_INT_DATA_1_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + temp = data & ~BMI160_MOTION_SRC_INT_MASK; + data = temp | ((sig_mot_int_cfg->sig_data_src << 7) & BMI160_MOTION_SRC_INT_MASK); + + /* Write data to DATA 1 address */ + rslt = bmi160_set_regs(BMI160_INT_DATA_1_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API configure the threshold, skip and proof time of + * sig motion interrupt. + */ +static int8_t config_sig_dur_threshold(const struct bmi160_acc_sig_mot_int_cfg *sig_mot_int_cfg, + const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data; + uint8_t temp = 0; + + /* Configuring INT_MOTION registers */ + + /* Write significant motion threshold. + * This threshold is same as any motion threshold */ + data = sig_mot_int_cfg->sig_mot_thres; + + /* Write data to INT_MOTION 1 address */ + rslt = bmi160_set_regs(BMI160_INT_MOTION_1_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + rslt = bmi160_get_regs(BMI160_INT_MOTION_3_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + temp = data & ~BMI160_SIG_MOTION_SKIP_MASK; + + /* adding skip time of sig_motion interrupt*/ + data = temp | ((sig_mot_int_cfg->sig_mot_skip << 2) & BMI160_SIG_MOTION_SKIP_MASK); + temp = data & ~BMI160_SIG_MOTION_PROOF_MASK; + + /* adding proof time of sig_motion interrupt */ + data = temp | ((sig_mot_int_cfg->sig_mot_proof << 4) & BMI160_SIG_MOTION_PROOF_MASK); + + /* configure the int_sig_mot_sel bit to select + * significant motion interrupt */ + temp = data & ~BMI160_SIG_MOTION_SEL_MASK; + data = temp | ((sig_mot_int_cfg->sig_en << 1) & BMI160_SIG_MOTION_SEL_MASK); + rslt = bmi160_set_regs(BMI160_INT_MOTION_3_ADDR, &data, 1, dev); + } + } + + return rslt; +} + +/*! + * @brief This API enables the step detector interrupt. + */ +static int8_t enable_step_detect_int(const struct bmi160_acc_step_detect_int_cfg *step_detect_int_cfg, + const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + + /* Enable data ready interrupt in Int Enable 2 register */ + rslt = bmi160_get_regs(BMI160_INT_ENABLE_2_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + temp = data & ~BMI160_STEP_DETECT_INT_EN_MASK; + data = temp | ((step_detect_int_cfg->step_detector_en << 3) & BMI160_STEP_DETECT_INT_EN_MASK); + + /* Writing data to INT ENABLE 2 Address */ + rslt = bmi160_set_regs(BMI160_INT_ENABLE_2_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API configure the step detector parameter. + */ +static int8_t config_step_detect(const struct bmi160_acc_step_detect_int_cfg *step_detect_int_cfg, + const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t temp = 0; + uint8_t data_array[2] = { 0 }; + + if (step_detect_int_cfg->step_detector_mode == BMI160_STEP_DETECT_NORMAL) + { + /* Normal mode setting */ + data_array[0] = 0x15; + data_array[1] = 0x03; + } + else if (step_detect_int_cfg->step_detector_mode == BMI160_STEP_DETECT_SENSITIVE) + { + /* Sensitive mode setting */ + data_array[0] = 0x2D; + data_array[1] = 0x00; + } + else if (step_detect_int_cfg->step_detector_mode == BMI160_STEP_DETECT_ROBUST) + { + /* Robust mode setting */ + data_array[0] = 0x1D; + data_array[1] = 0x07; + } + else if (step_detect_int_cfg->step_detector_mode == BMI160_STEP_DETECT_USER_DEFINE) + { + /* Non recommended User defined setting */ + /* Configuring STEP_CONFIG register */ + rslt = bmi160_get_regs(BMI160_INT_STEP_CONFIG_0_ADDR, &data_array[0], 2, dev); + if (rslt == BMI160_OK) + { + temp = data_array[0] & ~BMI160_STEP_DETECT_MIN_THRES_MASK; + + /* Adding min_threshold */ + data_array[0] = temp | ((step_detect_int_cfg->min_threshold << 3) & BMI160_STEP_DETECT_MIN_THRES_MASK); + temp = data_array[0] & ~BMI160_STEP_DETECT_STEPTIME_MIN_MASK; + + /* Adding steptime_min */ + data_array[0] = temp | ((step_detect_int_cfg->steptime_min) & BMI160_STEP_DETECT_STEPTIME_MIN_MASK); + temp = data_array[1] & ~BMI160_STEP_MIN_BUF_MASK; + + /* Adding steptime_min */ + data_array[1] = temp | ((step_detect_int_cfg->step_min_buf) & BMI160_STEP_MIN_BUF_MASK); + } + } + + /* Write data to STEP_CONFIG register */ + rslt = bmi160_set_regs(BMI160_INT_STEP_CONFIG_0_ADDR, data_array, 2, dev); + + return rslt; +} + +/*! + * @brief This API enables the single/double tap interrupt. + */ +static int8_t enable_tap_int(const struct bmi160_int_settg *int_config, + const struct bmi160_acc_tap_int_cfg *tap_int_cfg, + const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + + /* Enable single tap or double tap interrupt in Int Enable 0 register */ + rslt = bmi160_get_regs(BMI160_INT_ENABLE_0_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + if (int_config->int_type == BMI160_ACC_SINGLE_TAP_INT) + { + temp = data & ~BMI160_SINGLE_TAP_INT_EN_MASK; + data = temp | ((tap_int_cfg->tap_en << 5) & BMI160_SINGLE_TAP_INT_EN_MASK); + } + else + { + temp = data & ~BMI160_DOUBLE_TAP_INT_EN_MASK; + data = temp | ((tap_int_cfg->tap_en << 4) & BMI160_DOUBLE_TAP_INT_EN_MASK); + } + + /* Write to Enable 0 Address */ + rslt = bmi160_set_regs(BMI160_INT_ENABLE_0_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API configure the interrupt PIN setting for + * tap interrupt. + */ +static int8_t config_tap_int_settg(const struct bmi160_int_settg *int_config, + const struct bmi160_acc_tap_int_cfg *tap_int_cfg, + const struct bmi160_dev *dev) +{ + int8_t rslt; + + /* Configure Interrupt pins */ + rslt = set_intr_pin_config(int_config, dev); + if (rslt == BMI160_OK) + { + rslt = map_feature_interrupt(int_config, dev); + if (rslt == BMI160_OK) + { + rslt = config_tap_data_src(tap_int_cfg, dev); + if (rslt == BMI160_OK) + { + rslt = config_tap_param(int_config, tap_int_cfg, dev); + } + } + } + + return rslt; +} + +/*! + * @brief This API configure the source of data(filter & pre-filter) + * for tap interrupt. + */ +static int8_t config_tap_data_src(const struct bmi160_acc_tap_int_cfg *tap_int_cfg, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + + /* Configure Int data 0 register to add source of interrupt */ + rslt = bmi160_get_regs(BMI160_INT_DATA_0_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + temp = data & ~BMI160_TAP_SRC_INT_MASK; + data = temp | ((tap_int_cfg->tap_data_src << 3) & BMI160_TAP_SRC_INT_MASK); + + /* Write data to Data 0 address */ + rslt = bmi160_set_regs(BMI160_INT_DATA_0_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API configure the parameters of tap interrupt. + * Threshold, quite, shock, and duration. + */ +static int8_t config_tap_param(const struct bmi160_int_settg *int_config, + const struct bmi160_acc_tap_int_cfg *tap_int_cfg, + const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t temp = 0; + uint8_t data = 0; + uint8_t data_array[2] = { 0 }; + uint8_t count = 0; + uint8_t dur, shock, quiet, thres; + + /* Configure tap 0 register for tap shock,tap quiet duration + * in case of single tap interrupt */ + rslt = bmi160_get_regs(BMI160_INT_TAP_0_ADDR, data_array, 2, dev); + if (rslt == BMI160_OK) + { + data = data_array[count]; + if (int_config->int_type == BMI160_ACC_DOUBLE_TAP_INT) + { + dur = (uint8_t)tap_int_cfg->tap_dur; + temp = (data & ~BMI160_TAP_DUR_MASK); + + /* Add tap duration data in case of + * double tap interrupt */ + data = temp | (dur & BMI160_TAP_DUR_MASK); + } + shock = (uint8_t)tap_int_cfg->tap_shock; + temp = data & ~BMI160_TAP_SHOCK_DUR_MASK; + data = temp | ((shock << 6) & BMI160_TAP_SHOCK_DUR_MASK); + quiet = (uint8_t)tap_int_cfg->tap_quiet; + temp = data & ~BMI160_TAP_QUIET_DUR_MASK; + data = temp | ((quiet << 7) & BMI160_TAP_QUIET_DUR_MASK); + data_array[count++] = data; + data = data_array[count]; + thres = (uint8_t)tap_int_cfg->tap_thr; + temp = data & ~BMI160_TAP_THRES_MASK; + data = temp | (thres & BMI160_TAP_THRES_MASK); + data_array[count++] = data; + + /* TAP 0 and TAP 1 address lie consecutively, + * hence writing data to respective registers at one go */ + + /* Writing to Tap 0 and Tap 1 Address simultaneously */ + rslt = bmi160_set_regs(BMI160_INT_TAP_0_ADDR, data_array, count, dev); + } + + return rslt; +} + +/*! + * @brief This API configure the secondary interface. + */ +static int8_t config_sec_if(const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t if_conf = 0; + uint8_t cmd = BMI160_AUX_NORMAL_MODE; + + /* set the aux power mode to normal*/ + rslt = bmi160_set_regs(BMI160_COMMAND_REG_ADDR, &cmd, 1, dev); + if (rslt == BMI160_OK) + { + /* 0.5ms delay - refer datasheet table 24*/ + dev->delay_ms(1); + rslt = bmi160_get_regs(BMI160_IF_CONF_ADDR, &if_conf, 1, dev); + if_conf |= (uint8_t)(1 << 5); + if (rslt == BMI160_OK) + { + /*enable the secondary interface also*/ + rslt = bmi160_set_regs(BMI160_IF_CONF_ADDR, &if_conf, 1, dev); + } + } + + return rslt; +} + +/*! + * @brief This API configure the ODR of the auxiliary sensor. + */ +static int8_t config_aux_odr(const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t aux_odr; + + rslt = bmi160_get_regs(BMI160_AUX_ODR_ADDR, &aux_odr, 1, dev); + if (rslt == BMI160_OK) + { + aux_odr = (uint8_t)(dev->aux_cfg.aux_odr); + + /* Set the secondary interface ODR + * i.e polling rate of secondary sensor */ + rslt = bmi160_set_regs(BMI160_AUX_ODR_ADDR, &aux_odr, 1, dev); + dev->delay_ms(BMI160_AUX_COM_DELAY); + } + + return rslt; +} + +/*! + * @brief This API maps the actual burst read length set by user. + */ +static int8_t map_read_len(uint16_t *len, const struct bmi160_dev *dev) +{ + int8_t rslt = BMI160_OK; + + switch (dev->aux_cfg.aux_rd_burst_len) + { + case BMI160_AUX_READ_LEN_0: + *len = 1; + break; + case BMI160_AUX_READ_LEN_1: + *len = 2; + break; + case BMI160_AUX_READ_LEN_2: + *len = 6; + break; + case BMI160_AUX_READ_LEN_3: + *len = 8; + break; + default: + rslt = BMI160_E_INVALID_INPUT; + break; + } + + return rslt; +} + +/*! + * @brief This API configure the settings of auxiliary sensor. + */ +static int8_t config_aux_settg(const struct bmi160_dev *dev) +{ + int8_t rslt; + + rslt = config_sec_if(dev); + if (rslt == BMI160_OK) + { + /* Configures the auxiliary interface settings */ + rslt = bmi160_config_aux_mode(dev); + } + + return rslt; +} + +/*! + * @brief This API extract the read data from auxiliary sensor. + */ +static int8_t extract_aux_read(uint16_t map_len, + uint8_t reg_addr, + uint8_t *aux_data, + uint16_t len, + const struct bmi160_dev *dev) +{ + int8_t rslt = BMI160_OK; + uint8_t data[8] = { 0, }; + uint8_t read_addr = BMI160_AUX_DATA_ADDR; + uint8_t count = 0; + uint8_t read_count; + uint8_t read_len = (uint8_t)map_len; + + for (; count < len;) + { + /* set address to read */ + rslt = bmi160_set_regs(BMI160_AUX_IF_2_ADDR, ®_addr, 1, dev); + dev->delay_ms(BMI160_AUX_COM_DELAY); + if (rslt == BMI160_OK) + { + rslt = bmi160_get_regs(read_addr, data, map_len, dev); + if (rslt == BMI160_OK) + { + read_count = 0; + + /* if read len is less the burst read len + * mention by user*/ + if (len < map_len) + { + read_len = (uint8_t)len; + } + else if ((len - count) < map_len) + { + read_len = (uint8_t)(len - count); + } + + for (; read_count < read_len; read_count++) + { + aux_data[count + read_count] = data[read_count]; + } + reg_addr += (uint8_t)map_len; + count += (uint8_t)map_len; + } + else + { + rslt = BMI160_E_COM_FAIL; + break; + } + } + } + + return rslt; +} + +/*! + * @brief This API enables the orient interrupt. + */ +static int8_t enable_orient_int(const struct bmi160_acc_orient_int_cfg *orient_int_cfg, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + + /* Enable data ready interrupt in Int Enable 0 register */ + rslt = bmi160_get_regs(BMI160_INT_ENABLE_0_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + temp = data & ~BMI160_ORIENT_INT_EN_MASK; + data = temp | ((orient_int_cfg->orient_en << 6) & BMI160_ORIENT_INT_EN_MASK); + + /* write data to Int Enable 0 register */ + rslt = bmi160_set_regs(BMI160_INT_ENABLE_0_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API configure the necessary setting of orientation interrupt. + */ +static int8_t config_orient_int_settg(const struct bmi160_acc_orient_int_cfg *orient_int_cfg, + const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + uint8_t data_array[2] = { 0, 0 }; + + /* Configuring INT_ORIENT registers */ + rslt = bmi160_get_regs(BMI160_INT_ORIENT_0_ADDR, data_array, 2, dev); + if (rslt == BMI160_OK) + { + data = data_array[0]; + temp = data & ~BMI160_ORIENT_MODE_MASK; + + /* Adding Orientation mode */ + data = temp | ((orient_int_cfg->orient_mode) & BMI160_ORIENT_MODE_MASK); + temp = data & ~BMI160_ORIENT_BLOCK_MASK; + + /* Adding Orientation blocking */ + data = temp | ((orient_int_cfg->orient_blocking << 2) & BMI160_ORIENT_BLOCK_MASK); + temp = data & ~BMI160_ORIENT_HYST_MASK; + + /* Adding Orientation hysteresis */ + data = temp | ((orient_int_cfg->orient_hyst << 4) & BMI160_ORIENT_HYST_MASK); + data_array[0] = data; + data = data_array[1]; + temp = data & ~BMI160_ORIENT_THETA_MASK; + + /* Adding Orientation threshold */ + data = temp | ((orient_int_cfg->orient_theta) & BMI160_ORIENT_THETA_MASK); + temp = data & ~BMI160_ORIENT_UD_ENABLE; + + /* Adding Orient_ud_en */ + data = temp | ((orient_int_cfg->orient_ud_en << 6) & BMI160_ORIENT_UD_ENABLE); + temp = data & ~BMI160_AXES_EN_MASK; + + /* Adding axes_en */ + data = temp | ((orient_int_cfg->axes_ex << 7) & BMI160_AXES_EN_MASK); + data_array[1] = data; + + /* Writing data to INT_ORIENT 0 and INT_ORIENT 1 + * registers simultaneously */ + rslt = bmi160_set_regs(BMI160_INT_ORIENT_0_ADDR, data_array, 2, dev); + } + + return rslt; +} + +/*! + * @brief This API enables the flat interrupt. + */ +static int8_t enable_flat_int(const struct bmi160_acc_flat_detect_int_cfg *flat_int, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + + /* Enable flat interrupt in Int Enable 0 register */ + rslt = bmi160_get_regs(BMI160_INT_ENABLE_0_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + temp = data & ~BMI160_FLAT_INT_EN_MASK; + data = temp | ((flat_int->flat_en << 7) & BMI160_FLAT_INT_EN_MASK); + + /* write data to Int Enable 0 register */ + rslt = bmi160_set_regs(BMI160_INT_ENABLE_0_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API configure the necessary setting of flat interrupt. + */ +static int8_t config_flat_int_settg(const struct bmi160_acc_flat_detect_int_cfg *flat_int, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + uint8_t data_array[2] = { 0, 0 }; + + /* Configuring INT_FLAT register */ + rslt = bmi160_get_regs(BMI160_INT_FLAT_0_ADDR, data_array, 2, dev); + if (rslt == BMI160_OK) + { + data = data_array[0]; + temp = data & ~BMI160_FLAT_THRES_MASK; + + /* Adding flat theta */ + data = temp | ((flat_int->flat_theta) & BMI160_FLAT_THRES_MASK); + data_array[0] = data; + data = data_array[1]; + temp = data & ~BMI160_FLAT_HOLD_TIME_MASK; + + /* Adding flat hold time */ + data = temp | ((flat_int->flat_hold_time << 4) & BMI160_FLAT_HOLD_TIME_MASK); + temp = data & ~BMI160_FLAT_HYST_MASK; + + /* Adding flat hysteresis */ + data = temp | ((flat_int->flat_hy) & BMI160_FLAT_HYST_MASK); + data_array[1] = data; + + /* Writing data to INT_FLAT 0 and INT_FLAT 1 + * registers simultaneously */ + rslt = bmi160_set_regs(BMI160_INT_FLAT_0_ADDR, data_array, 2, dev); + } + + return rslt; +} + +/*! + * @brief This API enables the Low-g interrupt. + */ +static int8_t enable_low_g_int(const struct bmi160_acc_low_g_int_cfg *low_g_int, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + + /* Enable low-g interrupt in Int Enable 1 register */ + rslt = bmi160_get_regs(BMI160_INT_ENABLE_1_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + temp = data & ~BMI160_LOW_G_INT_EN_MASK; + data = temp | ((low_g_int->low_en << 3) & BMI160_LOW_G_INT_EN_MASK); + + /* write data to Int Enable 0 register */ + rslt = bmi160_set_regs(BMI160_INT_ENABLE_1_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API configure the source of data(filter & pre-filter) + * for low-g interrupt. + */ +static int8_t config_low_g_data_src(const struct bmi160_acc_low_g_int_cfg *low_g_int, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + + /* Configure Int data 0 register to add source of interrupt */ + rslt = bmi160_get_regs(BMI160_INT_DATA_0_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + temp = data & ~BMI160_LOW_HIGH_SRC_INT_MASK; + data = temp | ((low_g_int->low_data_src << 7) & BMI160_LOW_HIGH_SRC_INT_MASK); + + /* Write data to Data 0 address */ + rslt = bmi160_set_regs(BMI160_INT_DATA_0_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API configure the necessary setting of low-g interrupt. + */ +static int8_t config_low_g_int_settg(const struct bmi160_acc_low_g_int_cfg *low_g_int, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t temp = 0; + uint8_t data_array[3] = { 0, 0, 0 }; + + /* Configuring INT_LOWHIGH register for low-g interrupt */ + rslt = bmi160_get_regs(BMI160_INT_LOWHIGH_2_ADDR, &data_array[2], 1, dev); + if (rslt == BMI160_OK) + { + temp = data_array[2] & ~BMI160_LOW_G_HYST_MASK; + + /* Adding low-g hysteresis */ + data_array[2] = temp | (low_g_int->low_hyst & BMI160_LOW_G_HYST_MASK); + temp = data_array[2] & ~BMI160_LOW_G_LOW_MODE_MASK; + + /* Adding low-mode */ + data_array[2] = temp | ((low_g_int->low_mode << 2) & BMI160_LOW_G_LOW_MODE_MASK); + + /* Adding low-g threshold */ + data_array[1] = low_g_int->low_thres; + + /* Adding low-g interrupt delay */ + data_array[0] = low_g_int->low_dur; + + /* Writing data to INT_LOWHIGH 0,1,2 registers simultaneously*/ + rslt = bmi160_set_regs(BMI160_INT_LOWHIGH_0_ADDR, data_array, 3, dev); + } + + return rslt; +} + +/*! + * @brief This API enables the high-g interrupt. + */ +static int8_t enable_high_g_int(const struct bmi160_acc_high_g_int_cfg *high_g_int_cfg, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + + /* Enable low-g interrupt in Int Enable 1 register */ + rslt = bmi160_get_regs(BMI160_INT_ENABLE_1_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + /* Adding high-g X-axis */ + temp = data & ~BMI160_HIGH_G_X_INT_EN_MASK; + data = temp | (high_g_int_cfg->high_g_x & BMI160_HIGH_G_X_INT_EN_MASK); + + /* Adding high-g Y-axis */ + temp = data & ~BMI160_HIGH_G_Y_INT_EN_MASK; + data = temp | ((high_g_int_cfg->high_g_y << 1) & BMI160_HIGH_G_Y_INT_EN_MASK); + + /* Adding high-g Z-axis */ + temp = data & ~BMI160_HIGH_G_Z_INT_EN_MASK; + data = temp | ((high_g_int_cfg->high_g_z << 2) & BMI160_HIGH_G_Z_INT_EN_MASK); + + /* write data to Int Enable 0 register */ + rslt = bmi160_set_regs(BMI160_INT_ENABLE_1_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API configure the source of data(filter & pre-filter) + * for high-g interrupt. + */ +static int8_t config_high_g_data_src(const struct bmi160_acc_high_g_int_cfg *high_g_int_cfg, + const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + uint8_t temp = 0; + + /* Configure Int data 0 register to add source of interrupt */ + rslt = bmi160_get_regs(BMI160_INT_DATA_0_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + temp = data & ~BMI160_LOW_HIGH_SRC_INT_MASK; + data = temp | ((high_g_int_cfg->high_data_src << 7) & BMI160_LOW_HIGH_SRC_INT_MASK); + + /* Write data to Data 0 address */ + rslt = bmi160_set_regs(BMI160_INT_DATA_0_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API configure the necessary setting of high-g interrupt. + */ +static int8_t config_high_g_int_settg(const struct bmi160_acc_high_g_int_cfg *high_g_int_cfg, + const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t temp = 0; + uint8_t data_array[3] = { 0, 0, 0 }; + + rslt = bmi160_get_regs(BMI160_INT_LOWHIGH_2_ADDR, &data_array[0], 1, dev); + if (rslt == BMI160_OK) + { + temp = data_array[0] & ~BMI160_HIGH_G_HYST_MASK; + + /* Adding high-g hysteresis */ + data_array[0] = temp | ((high_g_int_cfg->high_hy << 6) & BMI160_HIGH_G_HYST_MASK); + + /* Adding high-g duration */ + data_array[1] = high_g_int_cfg->high_dur; + + /* Adding high-g threshold */ + data_array[2] = high_g_int_cfg->high_thres; + rslt = bmi160_set_regs(BMI160_INT_LOWHIGH_2_ADDR, data_array, 3, dev); + } + + return rslt; +} + +/*! + * @brief This API configure the behavioural setting of interrupt pin. + */ +static int8_t config_int_out_ctrl(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t temp = 0; + uint8_t data = 0; + + /* Configuration of output interrupt signals on pins INT1 and INT2 are + * done in BMI160_INT_OUT_CTRL_ADDR register*/ + rslt = bmi160_get_regs(BMI160_INT_OUT_CTRL_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + /* updating the interrupt pin structure to local structure */ + const struct bmi160_int_pin_settg *intr_pin_sett = &(int_config->int_pin_settg); + + /* Configuring channel 1 */ + if (int_config->int_channel == BMI160_INT_CHANNEL_1) + { + /* Output enable */ + temp = data & ~BMI160_INT1_OUTPUT_EN_MASK; + data = temp | ((intr_pin_sett->output_en << 3) & BMI160_INT1_OUTPUT_EN_MASK); + + /* Output mode */ + temp = data & ~BMI160_INT1_OUTPUT_MODE_MASK; + data = temp | ((intr_pin_sett->output_mode << 2) & BMI160_INT1_OUTPUT_MODE_MASK); + + /* Output type */ + temp = data & ~BMI160_INT1_OUTPUT_TYPE_MASK; + data = temp | ((intr_pin_sett->output_type << 1) & BMI160_INT1_OUTPUT_TYPE_MASK); + + /* edge control */ + temp = data & ~BMI160_INT1_EDGE_CTRL_MASK; + data = temp | ((intr_pin_sett->edge_ctrl) & BMI160_INT1_EDGE_CTRL_MASK); + } + else + { + /* Configuring channel 2 */ + /* Output enable */ + temp = data & ~BMI160_INT2_OUTPUT_EN_MASK; + data = temp | ((intr_pin_sett->output_en << 7) & BMI160_INT2_OUTPUT_EN_MASK); + + /* Output mode */ + temp = data & ~BMI160_INT2_OUTPUT_MODE_MASK; + data = temp | ((intr_pin_sett->output_mode << 6) & BMI160_INT2_OUTPUT_MODE_MASK); + + /* Output type */ + temp = data & ~BMI160_INT2_OUTPUT_TYPE_MASK; + data = temp | ((intr_pin_sett->output_type << 5) & BMI160_INT2_OUTPUT_TYPE_MASK); + + /* edge control */ + temp = data & ~BMI160_INT2_EDGE_CTRL_MASK; + data = temp | ((intr_pin_sett->edge_ctrl << 4) & BMI160_INT2_EDGE_CTRL_MASK); + } + rslt = bmi160_set_regs(BMI160_INT_OUT_CTRL_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API configure the mode(input enable, latch or non-latch) of interrupt pin. + */ +static int8_t config_int_latch(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t temp = 0; + uint8_t data = 0; + + /* Configuration of latch on pins INT1 and INT2 are done in + * BMI160_INT_LATCH_ADDR register*/ + rslt = bmi160_get_regs(BMI160_INT_LATCH_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + /* updating the interrupt pin structure to local structure */ + const struct bmi160_int_pin_settg *intr_pin_sett = &(int_config->int_pin_settg); + if (int_config->int_channel == BMI160_INT_CHANNEL_1) + { + /* Configuring channel 1 */ + /* Input enable */ + temp = data & ~BMI160_INT1_INPUT_EN_MASK; + data = temp | ((intr_pin_sett->input_en << 4) & BMI160_INT1_INPUT_EN_MASK); + } + else + { + /* Configuring channel 2 */ + /* Input enable */ + temp = data & ~BMI160_INT2_INPUT_EN_MASK; + data = temp | ((intr_pin_sett->input_en << 5) & BMI160_INT2_INPUT_EN_MASK); + } + + /* In case of latch interrupt,update the latch duration */ + + /* Latching holds the interrupt for the amount of latch + * duration time */ + temp = data & ~BMI160_INT_LATCH_MASK; + data = temp | (intr_pin_sett->latch_dur & BMI160_INT_LATCH_MASK); + + /* OUT_CTRL_INT and LATCH_INT address lie consecutively, + * hence writing data to respective registers at one go */ + rslt = bmi160_set_regs(BMI160_INT_LATCH_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API performs the self test for accelerometer of BMI160 + */ +static int8_t perform_accel_self_test(struct bmi160_dev *dev) +{ + int8_t rslt; + struct bmi160_sensor_data accel_pos, accel_neg; + + /* Enable Gyro self test bit */ + rslt = enable_accel_self_test(dev); + if (rslt == BMI160_OK) + { + /* Perform accel self test with positive excitation */ + rslt = accel_self_test_positive_excitation(&accel_pos, dev); + if (rslt == BMI160_OK) + { + /* Perform accel self test with negative excitation */ + rslt = accel_self_test_negative_excitation(&accel_neg, dev); + if (rslt == BMI160_OK) + { + /* Validate the self test result */ + rslt = validate_accel_self_test(&accel_pos, &accel_neg); + } + } + } + + return rslt; +} + +/*! + * @brief This API enables to perform the accel self test by setting proper + * configurations to facilitate accel self test + */ +static int8_t enable_accel_self_test(struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t reg_data; + + /* Set the Accel power mode as normal mode */ + dev->accel_cfg.power = BMI160_ACCEL_NORMAL_MODE; + + /* Set the sensor range configuration as 8G */ + dev->accel_cfg.range = BMI160_ACCEL_RANGE_8G; + rslt = bmi160_set_sens_conf(dev); + if (rslt == BMI160_OK) + { + /* Accel configurations are set to facilitate self test + * acc_odr - 1600Hz ; acc_bwp = 2 ; acc_us = 0 */ + reg_data = BMI160_ACCEL_SELF_TEST_CONFIG; + rslt = bmi160_set_regs(BMI160_ACCEL_CONFIG_ADDR, ®_data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API performs accel self test with positive excitation + */ +static int8_t accel_self_test_positive_excitation(struct bmi160_sensor_data *accel_pos, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t reg_data; + + /* Enable accel self test with positive self-test excitation + * and with amplitude of deflection set as high */ + reg_data = BMI160_ACCEL_SELF_TEST_POSITIVE_EN; + rslt = bmi160_set_regs(BMI160_SELF_TEST_ADDR, ®_data, 1, dev); + if (rslt == BMI160_OK) + { + /* Read the data after a delay of 50ms - refer datasheet 2.8.1 accel self test*/ + dev->delay_ms(BMI160_ACCEL_SELF_TEST_DELAY); + rslt = bmi160_get_sensor_data(BMI160_ACCEL_ONLY, accel_pos, NULL, dev); + } + + return rslt; +} + +/*! + * @brief This API performs accel self test with negative excitation + */ +static int8_t accel_self_test_negative_excitation(struct bmi160_sensor_data *accel_neg, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t reg_data; + + /* Enable accel self test with negative self-test excitation + * and with amplitude of deflection set as high */ + reg_data = BMI160_ACCEL_SELF_TEST_NEGATIVE_EN; + rslt = bmi160_set_regs(BMI160_SELF_TEST_ADDR, ®_data, 1, dev); + if (rslt == BMI160_OK) + { + /* Read the data after a delay of 50ms */ + dev->delay_ms(BMI160_ACCEL_SELF_TEST_DELAY); + rslt = bmi160_get_sensor_data(BMI160_ACCEL_ONLY, accel_neg, NULL, dev); + } + + return rslt; +} + +/*! + * @brief This API validates the accel self test results + */ +static int8_t validate_accel_self_test(const struct bmi160_sensor_data *accel_pos, + const struct bmi160_sensor_data *accel_neg) +{ + int8_t rslt; + + /* Validate the results of self test */ + if (((accel_neg->x - accel_pos->x) > BMI160_ACCEL_SELF_TEST_LIMIT) && + ((accel_neg->y - accel_pos->y) > BMI160_ACCEL_SELF_TEST_LIMIT) && + ((accel_neg->z - accel_pos->z) > BMI160_ACCEL_SELF_TEST_LIMIT)) + { + /* Self test pass condition */ + rslt = BMI160_OK; + } + else + { + rslt = BMI160_W_ACCEl_SELF_TEST_FAIL; + } + + return rslt; +} + +/*! + * @brief This API performs the self test for gyroscope of BMI160 + */ +static int8_t perform_gyro_self_test(const struct bmi160_dev *dev) +{ + int8_t rslt; + + /* Enable Gyro self test bit */ + rslt = enable_gyro_self_test(dev); + if (rslt == BMI160_OK) + { + /* Validate the gyro self test a delay of 50ms */ + dev->delay_ms(50); + + /* Validate the gyro self test results */ + rslt = validate_gyro_self_test(dev); + } + + return rslt; +} + +/*! + * @brief This API enables the self test bit to trigger self test for Gyro + */ +static int8_t enable_gyro_self_test(const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t reg_data; + + /* Enable the Gyro self test bit to trigger the self test */ + rslt = bmi160_get_regs(BMI160_SELF_TEST_ADDR, ®_data, 1, dev); + if (rslt == BMI160_OK) + { + reg_data = BMI160_SET_BITS(reg_data, BMI160_GYRO_SELF_TEST, 1); + rslt = bmi160_set_regs(BMI160_SELF_TEST_ADDR, ®_data, 1, dev); + if (rslt == BMI160_OK) + { + /* Delay to enable gyro self test */ + dev->delay_ms(15); + } + } + + return rslt; +} + +/*! + * @brief This API validates the self test results of Gyro + */ +static int8_t validate_gyro_self_test(const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t reg_data; + + /* Validate the Gyro self test result */ + rslt = bmi160_get_regs(BMI160_STATUS_ADDR, ®_data, 1, dev); + if (rslt == BMI160_OK) + { + + reg_data = BMI160_GET_BITS(reg_data, BMI160_GYRO_SELF_TEST_STATUS); + if (reg_data == BMI160_ENABLE) + { + /* Gyro self test success case */ + rslt = BMI160_OK; + } + else + { + rslt = BMI160_W_GYRO_SELF_TEST_FAIL; + } + } + + return rslt; +} + +/*! + * @brief This API sets FIFO full interrupt of the sensor.This interrupt + * occurs when the FIFO is full and the next full data sample would cause + * a FIFO overflow, which may delete the old samples. + */ +static int8_t set_fifo_full_int(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) +{ + int8_t rslt = BMI160_OK; + + /* Null-pointer check */ + if ((dev == NULL) || (dev->delay_ms == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /*enable the fifo full interrupt */ + rslt = enable_fifo_full_int(int_config, dev); + if (rslt == BMI160_OK) + { + /* Configure Interrupt pins */ + rslt = set_intr_pin_config(int_config, dev); + if (rslt == BMI160_OK) + { + rslt = map_hardware_interrupt(int_config, dev); + } + } + } + + return rslt; +} + +/*! + * @brief This enable the FIFO full interrupt engine. + */ +static int8_t enable_fifo_full_int(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + + rslt = bmi160_get_regs(BMI160_INT_ENABLE_1_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + data = BMI160_SET_BITS(data, BMI160_FIFO_FULL_INT, int_config->fifo_full_int_en); + + /* Writing data to INT ENABLE 1 Address */ + rslt = bmi160_set_regs(BMI160_INT_ENABLE_1_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API sets FIFO watermark interrupt of the sensor.The FIFO + * watermark interrupt is fired, when the FIFO fill level is above a fifo + * watermark. + */ +static int8_t set_fifo_watermark_int(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) +{ + int8_t rslt = BMI160_OK; + + if ((dev == NULL) || (dev->delay_ms == NULL)) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /* Enable fifo-watermark interrupt in Int Enable 1 register */ + rslt = enable_fifo_wtm_int(int_config, dev); + if (rslt == BMI160_OK) + { + /* Configure Interrupt pins */ + rslt = set_intr_pin_config(int_config, dev); + if (rslt == BMI160_OK) + { + rslt = map_hardware_interrupt(int_config, dev); + } + } + } + + return rslt; +} + +/*! + * @brief This enable the FIFO watermark interrupt engine. + */ +static int8_t enable_fifo_wtm_int(const struct bmi160_int_settg *int_config, const struct bmi160_dev *dev) +{ + int8_t rslt; + uint8_t data = 0; + + rslt = bmi160_get_regs(BMI160_INT_ENABLE_1_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + data = BMI160_SET_BITS(data, BMI160_FIFO_WTM_INT, int_config->fifo_wtm_int_en); + + /* Writing data to INT ENABLE 1 Address */ + rslt = bmi160_set_regs(BMI160_INT_ENABLE_1_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API is used to reset the FIFO related configurations + * in the fifo_frame structure. + */ +static void reset_fifo_data_structure(const struct bmi160_dev *dev) +{ + /*Prepare for next FIFO read by resetting FIFO's + * internal data structures*/ + dev->fifo->accel_byte_start_idx = 0; + dev->fifo->gyro_byte_start_idx = 0; + dev->fifo->aux_byte_start_idx = 0; + dev->fifo->sensor_time = 0; + dev->fifo->skipped_frame_count = 0; +} + +/*! + * @brief This API is used to read fifo_byte_counter value (i.e) + * current fill-level in Fifo buffer. + */ +static int8_t get_fifo_byte_counter(uint16_t *bytes_to_read, struct bmi160_dev const *dev) +{ + int8_t rslt = 0; + uint8_t data[2]; + uint8_t addr = BMI160_FIFO_LENGTH_ADDR; + + rslt |= bmi160_get_regs(addr, data, 2, dev); + data[1] = data[1] & BMI160_FIFO_BYTE_COUNTER_MASK; + + /* Available data in FIFO is stored in bytes_to_read*/ + *bytes_to_read = (((uint16_t)data[1] << 8) | ((uint16_t)data[0])); + + return rslt; +} + +/*! + * @brief This API is used to compute the number of bytes of accel FIFO data + * which is to be parsed in header-less mode + */ +static void get_accel_len_to_parse(uint16_t *data_index, + uint16_t *data_read_length, + const uint8_t *acc_frame_count, + const struct bmi160_dev *dev) +{ + /* Data start index */ + *data_index = dev->fifo->accel_byte_start_idx; + if (dev->fifo->fifo_data_enable == BMI160_FIFO_A_ENABLE) + { + *data_read_length = (*acc_frame_count) * BMI160_FIFO_A_LENGTH; + } + else if (dev->fifo->fifo_data_enable == BMI160_FIFO_G_A_ENABLE) + { + *data_read_length = (*acc_frame_count) * BMI160_FIFO_GA_LENGTH; + } + else if (dev->fifo->fifo_data_enable == BMI160_FIFO_M_A_ENABLE) + { + *data_read_length = (*acc_frame_count) * BMI160_FIFO_MA_LENGTH; + } + else if (dev->fifo->fifo_data_enable == BMI160_FIFO_M_G_A_ENABLE) + { + *data_read_length = (*acc_frame_count) * BMI160_FIFO_MGA_LENGTH; + } + else + { + /* When accel is not enabled ,there will be no accel data. + * so we update the data index as complete */ + *data_index = dev->fifo->length; + } + if (*data_read_length > dev->fifo->length) + { + /* Handling the case where more data is requested + * than that is available*/ + *data_read_length = dev->fifo->length; + } +} + +/*! + * @brief This API is used to parse the accelerometer data from the + * FIFO data in both header mode and header-less mode. + * It updates the idx value which is used to store the index of + * the current data byte which is parsed. + */ +static void unpack_accel_frame(struct bmi160_sensor_data *acc, + uint16_t *idx, + uint8_t *acc_idx, + uint8_t frame_info, + const struct bmi160_dev *dev) +{ + switch (frame_info) + { + case BMI160_FIFO_HEAD_A: + case BMI160_FIFO_A_ENABLE: + + /*Partial read, then skip the data*/ + if ((*idx + BMI160_FIFO_A_LENGTH) > dev->fifo->length) + { + /*Update the data index as complete*/ + *idx = dev->fifo->length; + break; + } + + /*Unpack the data array into the structure instance "acc" */ + unpack_accel_data(&acc[*acc_idx], *idx, dev); + + /*Move the data index*/ + *idx = *idx + BMI160_FIFO_A_LENGTH; + (*acc_idx)++; + break; + case BMI160_FIFO_HEAD_G_A: + case BMI160_FIFO_G_A_ENABLE: + + /*Partial read, then skip the data*/ + if ((*idx + BMI160_FIFO_GA_LENGTH) > dev->fifo->length) + { + /*Update the data index as complete*/ + *idx = dev->fifo->length; + break; + } + + /*Unpack the data array into structure instance "acc"*/ + unpack_accel_data(&acc[*acc_idx], *idx + BMI160_FIFO_G_LENGTH, dev); + + /*Move the data index*/ + *idx = *idx + BMI160_FIFO_GA_LENGTH; + (*acc_idx)++; + break; + case BMI160_FIFO_HEAD_M_A: + case BMI160_FIFO_M_A_ENABLE: + + /*Partial read, then skip the data*/ + if ((*idx + BMI160_FIFO_MA_LENGTH) > dev->fifo->length) + { + /*Update the data index as complete*/ + *idx = dev->fifo->length; + break; + } + + /*Unpack the data array into structure instance "acc"*/ + unpack_accel_data(&acc[*acc_idx], *idx + BMI160_FIFO_M_LENGTH, dev); + + /*Move the data index*/ + *idx = *idx + BMI160_FIFO_MA_LENGTH; + (*acc_idx)++; + break; + case BMI160_FIFO_HEAD_M_G_A: + case BMI160_FIFO_M_G_A_ENABLE: + + /*Partial read, then skip the data*/ + if ((*idx + BMI160_FIFO_MGA_LENGTH) > dev->fifo->length) + { + /*Update the data index as complete*/ + *idx = dev->fifo->length; + break; + } + + /*Unpack the data array into structure instance "acc"*/ + unpack_accel_data(&acc[*acc_idx], *idx + BMI160_FIFO_MG_LENGTH, dev); + + /*Move the data index*/ + *idx = *idx + BMI160_FIFO_MGA_LENGTH; + (*acc_idx)++; + break; + case BMI160_FIFO_HEAD_M: + case BMI160_FIFO_M_ENABLE: + (*idx) = (*idx) + BMI160_FIFO_M_LENGTH; + break; + case BMI160_FIFO_HEAD_G: + case BMI160_FIFO_G_ENABLE: + (*idx) = (*idx) + BMI160_FIFO_G_LENGTH; + break; + case BMI160_FIFO_HEAD_M_G: + case BMI160_FIFO_M_G_ENABLE: + (*idx) = (*idx) + BMI160_FIFO_MG_LENGTH; + break; + default: + break; + } +} + +/*! + * @brief This API is used to parse the accelerometer data from the + * FIFO data and store it in the instance of the structure bmi160_sensor_data. + */ +static void unpack_accel_data(struct bmi160_sensor_data *accel_data, + uint16_t data_start_index, + const struct bmi160_dev *dev) +{ + uint16_t data_lsb; + uint16_t data_msb; + + /* Accel raw x data */ + data_lsb = dev->fifo->data[data_start_index++]; + data_msb = dev->fifo->data[data_start_index++]; + accel_data->x = (int16_t)((data_msb << 8) | data_lsb); + + /* Accel raw y data */ + data_lsb = dev->fifo->data[data_start_index++]; + data_msb = dev->fifo->data[data_start_index++]; + accel_data->y = (int16_t)((data_msb << 8) | data_lsb); + + /* Accel raw z data */ + data_lsb = dev->fifo->data[data_start_index++]; + data_msb = dev->fifo->data[data_start_index++]; + accel_data->z = (int16_t)((data_msb << 8) | data_lsb); +} + +/*! + * @brief This API is used to parse the accelerometer data from the + * FIFO data in header mode. + */ +static void extract_accel_header_mode(struct bmi160_sensor_data *accel_data, + uint8_t *accel_length, + const struct bmi160_dev *dev) +{ + uint8_t frame_header = 0; + uint16_t data_index; + uint8_t accel_index = 0; + + for (data_index = dev->fifo->accel_byte_start_idx; data_index < dev->fifo->length;) + { + /* extracting Frame header */ + frame_header = (dev->fifo->data[data_index] & BMI160_FIFO_TAG_INTR_MASK); + + /*Index is moved to next byte where the data is starting*/ + data_index++; + switch (frame_header) + { + /* Accel frame */ + case BMI160_FIFO_HEAD_A: + case BMI160_FIFO_HEAD_M_A: + case BMI160_FIFO_HEAD_G_A: + case BMI160_FIFO_HEAD_M_G_A: + unpack_accel_frame(accel_data, &data_index, &accel_index, frame_header, dev); + break; + case BMI160_FIFO_HEAD_M: + move_next_frame(&data_index, BMI160_FIFO_M_LENGTH, dev); + break; + case BMI160_FIFO_HEAD_G: + move_next_frame(&data_index, BMI160_FIFO_G_LENGTH, dev); + break; + case BMI160_FIFO_HEAD_M_G: + move_next_frame(&data_index, BMI160_FIFO_MG_LENGTH, dev); + break; + + /* Sensor time frame */ + case BMI160_FIFO_HEAD_SENSOR_TIME: + unpack_sensortime_frame(&data_index, dev); + break; + + /* Skip frame */ + case BMI160_FIFO_HEAD_SKIP_FRAME: + unpack_skipped_frame(&data_index, dev); + break; + + /* Input config frame */ + case BMI160_FIFO_HEAD_INPUT_CONFIG: + move_next_frame(&data_index, 1, dev); + break; + case BMI160_FIFO_HEAD_OVER_READ: + + /* Update the data index as complete in case of Over read */ + data_index = dev->fifo->length; + break; + default: + break; + } + if (*accel_length == accel_index) + { + /* Number of frames to read completed */ + break; + } + } + + /*Update number of accel data read*/ + *accel_length = accel_index; + + /*Update the accel frame index*/ + dev->fifo->accel_byte_start_idx = data_index; +} + +/*! + * @brief This API computes the number of bytes of gyro FIFO data + * which is to be parsed in header-less mode + */ +static void get_gyro_len_to_parse(uint16_t *data_index, + uint16_t *data_read_length, + const uint8_t *gyro_frame_count, + const struct bmi160_dev *dev) +{ + /* Data start index */ + *data_index = dev->fifo->gyro_byte_start_idx; + if (dev->fifo->fifo_data_enable == BMI160_FIFO_G_ENABLE) + { + *data_read_length = (*gyro_frame_count) * BMI160_FIFO_G_LENGTH; + } + else if (dev->fifo->fifo_data_enable == BMI160_FIFO_G_A_ENABLE) + { + *data_read_length = (*gyro_frame_count) * BMI160_FIFO_GA_LENGTH; + } + else if (dev->fifo->fifo_data_enable == BMI160_FIFO_M_G_ENABLE) + { + *data_read_length = (*gyro_frame_count) * BMI160_FIFO_MG_LENGTH; + } + else if (dev->fifo->fifo_data_enable == BMI160_FIFO_M_G_A_ENABLE) + { + *data_read_length = (*gyro_frame_count) * BMI160_FIFO_MGA_LENGTH; + } + else + { + /* When gyro is not enabled ,there will be no gyro data. + * so we update the data index as complete */ + *data_index = dev->fifo->length; + } + if (*data_read_length > dev->fifo->length) + { + /* Handling the case where more data is requested + * than that is available*/ + *data_read_length = dev->fifo->length; + } +} + +/*! + * @brief This API is used to parse the gyroscope's data from the + * FIFO data in both header mode and header-less mode. + * It updates the idx value which is used to store the index of + * the current data byte which is parsed. + */ +static void unpack_gyro_frame(struct bmi160_sensor_data *gyro, + uint16_t *idx, + uint8_t *gyro_idx, + uint8_t frame_info, + const struct bmi160_dev *dev) +{ + switch (frame_info) + { + case BMI160_FIFO_HEAD_G: + case BMI160_FIFO_G_ENABLE: + + /*Partial read, then skip the data*/ + if ((*idx + BMI160_FIFO_G_LENGTH) > dev->fifo->length) + { + /*Update the data index as complete*/ + *idx = dev->fifo->length; + break; + } + + /*Unpack the data array into structure instance "gyro"*/ + unpack_gyro_data(&gyro[*gyro_idx], *idx, dev); + + /*Move the data index*/ + (*idx) = (*idx) + BMI160_FIFO_G_LENGTH; + (*gyro_idx)++; + break; + case BMI160_FIFO_HEAD_G_A: + case BMI160_FIFO_G_A_ENABLE: + + /*Partial read, then skip the data*/ + if ((*idx + BMI160_FIFO_GA_LENGTH) > dev->fifo->length) + { + /*Update the data index as complete*/ + *idx = dev->fifo->length; + break; + } + + /* Unpack the data array into structure instance "gyro" */ + unpack_gyro_data(&gyro[*gyro_idx], *idx, dev); + + /* Move the data index */ + *idx = *idx + BMI160_FIFO_GA_LENGTH; + (*gyro_idx)++; + break; + case BMI160_FIFO_HEAD_M_G_A: + case BMI160_FIFO_M_G_A_ENABLE: + + /*Partial read, then skip the data*/ + if ((*idx + BMI160_FIFO_MGA_LENGTH) > dev->fifo->length) + { + /*Update the data index as complete*/ + *idx = dev->fifo->length; + break; + } + + /*Unpack the data array into structure instance "gyro"*/ + unpack_gyro_data(&gyro[*gyro_idx], *idx + BMI160_FIFO_M_LENGTH, dev); + + /*Move the data index*/ + *idx = *idx + BMI160_FIFO_MGA_LENGTH; + (*gyro_idx)++; + break; + case BMI160_FIFO_HEAD_M_A: + case BMI160_FIFO_M_A_ENABLE: + + /* Move the data index */ + *idx = *idx + BMI160_FIFO_MA_LENGTH; + break; + case BMI160_FIFO_HEAD_M: + case BMI160_FIFO_M_ENABLE: + (*idx) = (*idx) + BMI160_FIFO_M_LENGTH; + break; + case BMI160_FIFO_HEAD_M_G: + case BMI160_FIFO_M_G_ENABLE: + + /*Partial read, then skip the data*/ + if ((*idx + BMI160_FIFO_MG_LENGTH) > dev->fifo->length) + { + /*Update the data index as complete*/ + *idx = dev->fifo->length; + break; + } + + /*Unpack the data array into structure instance "gyro"*/ + unpack_gyro_data(&gyro[*gyro_idx], *idx + BMI160_FIFO_M_LENGTH, dev); + + /*Move the data index*/ + (*idx) = (*idx) + BMI160_FIFO_MG_LENGTH; + (*gyro_idx)++; + break; + case BMI160_FIFO_HEAD_A: + case BMI160_FIFO_A_ENABLE: + + /*Move the data index*/ + *idx = *idx + BMI160_FIFO_A_LENGTH; + break; + default: + break; + } +} + +/*! + * @brief This API is used to parse the gyro data from the + * FIFO data and store it in the instance of the structure bmi160_sensor_data. + */ +static void unpack_gyro_data(struct bmi160_sensor_data *gyro_data, + uint16_t data_start_index, + const struct bmi160_dev *dev) +{ + uint16_t data_lsb; + uint16_t data_msb; + + /* Gyro raw x data */ + data_lsb = dev->fifo->data[data_start_index++]; + data_msb = dev->fifo->data[data_start_index++]; + gyro_data->x = (int16_t)((data_msb << 8) | data_lsb); + + /* Gyro raw y data */ + data_lsb = dev->fifo->data[data_start_index++]; + data_msb = dev->fifo->data[data_start_index++]; + gyro_data->y = (int16_t)((data_msb << 8) | data_lsb); + + /* Gyro raw z data */ + data_lsb = dev->fifo->data[data_start_index++]; + data_msb = dev->fifo->data[data_start_index++]; + gyro_data->z = (int16_t)((data_msb << 8) | data_lsb); +} + +/*! + * @brief This API is used to parse the gyro data from the + * FIFO data in header mode. + */ +static void extract_gyro_header_mode(struct bmi160_sensor_data *gyro_data, + uint8_t *gyro_length, + const struct bmi160_dev *dev) +{ + uint8_t frame_header = 0; + uint16_t data_index; + uint8_t gyro_index = 0; + + for (data_index = dev->fifo->gyro_byte_start_idx; data_index < dev->fifo->length;) + { + /* extracting Frame header */ + frame_header = (dev->fifo->data[data_index] & BMI160_FIFO_TAG_INTR_MASK); + + /*Index is moved to next byte where the data is starting*/ + data_index++; + switch (frame_header) + { + /* GYRO frame */ + case BMI160_FIFO_HEAD_G: + case BMI160_FIFO_HEAD_G_A: + case BMI160_FIFO_HEAD_M_G: + case BMI160_FIFO_HEAD_M_G_A: + unpack_gyro_frame(gyro_data, &data_index, &gyro_index, frame_header, dev); + break; + case BMI160_FIFO_HEAD_A: + move_next_frame(&data_index, BMI160_FIFO_A_LENGTH, dev); + break; + case BMI160_FIFO_HEAD_M: + move_next_frame(&data_index, BMI160_FIFO_M_LENGTH, dev); + break; + case BMI160_FIFO_HEAD_M_A: + move_next_frame(&data_index, BMI160_FIFO_M_LENGTH, dev); + break; + + /* Sensor time frame */ + case BMI160_FIFO_HEAD_SENSOR_TIME: + unpack_sensortime_frame(&data_index, dev); + break; + + /* Skip frame */ + case BMI160_FIFO_HEAD_SKIP_FRAME: + unpack_skipped_frame(&data_index, dev); + break; + + /* Input config frame */ + case BMI160_FIFO_HEAD_INPUT_CONFIG: + move_next_frame(&data_index, 1, dev); + break; + case BMI160_FIFO_HEAD_OVER_READ: + + /* Update the data index as complete in case of over read */ + data_index = dev->fifo->length; + break; + default: + break; + } + if (*gyro_length == gyro_index) + { + /*Number of frames to read completed*/ + break; + } + } + + /*Update number of gyro data read*/ + *gyro_length = gyro_index; + + /*Update the gyro frame index*/ + dev->fifo->gyro_byte_start_idx = data_index; +} + +/*! + * @brief This API computes the number of bytes of aux FIFO data + * which is to be parsed in header-less mode + */ +static void get_aux_len_to_parse(uint16_t *data_index, + uint16_t *data_read_length, + const uint8_t *aux_frame_count, + const struct bmi160_dev *dev) +{ + /* Data start index */ + *data_index = dev->fifo->gyro_byte_start_idx; + if (dev->fifo->fifo_data_enable == BMI160_FIFO_M_ENABLE) + { + *data_read_length = (*aux_frame_count) * BMI160_FIFO_M_LENGTH; + } + else if (dev->fifo->fifo_data_enable == BMI160_FIFO_M_A_ENABLE) + { + *data_read_length = (*aux_frame_count) * BMI160_FIFO_MA_LENGTH; + } + else if (dev->fifo->fifo_data_enable == BMI160_FIFO_M_G_ENABLE) + { + *data_read_length = (*aux_frame_count) * BMI160_FIFO_MG_LENGTH; + } + else if (dev->fifo->fifo_data_enable == BMI160_FIFO_M_G_A_ENABLE) + { + *data_read_length = (*aux_frame_count) * BMI160_FIFO_MGA_LENGTH; + } + else + { + /* When aux is not enabled ,there will be no aux data. + * so we update the data index as complete */ + *data_index = dev->fifo->length; + } + if (*data_read_length > dev->fifo->length) + { + /* Handling the case where more data is requested + * than that is available */ + *data_read_length = dev->fifo->length; + } +} + +/*! + * @brief This API is used to parse the aux's data from the + * FIFO data in both header mode and header-less mode. + * It updates the idx value which is used to store the index of + * the current data byte which is parsed + */ +static void unpack_aux_frame(struct bmi160_aux_data *aux_data, + uint16_t *idx, + uint8_t *aux_index, + uint8_t frame_info, + const struct bmi160_dev *dev) +{ + switch (frame_info) + { + case BMI160_FIFO_HEAD_M: + case BMI160_FIFO_M_ENABLE: + + /* Partial read, then skip the data */ + if ((*idx + BMI160_FIFO_M_LENGTH) > dev->fifo->length) + { + /* Update the data index as complete */ + *idx = dev->fifo->length; + break; + } + + /* Unpack the data array into structure instance */ + unpack_aux_data(&aux_data[*aux_index], *idx, dev); + + /* Move the data index */ + *idx = *idx + BMI160_FIFO_M_LENGTH; + (*aux_index)++; + break; + case BMI160_FIFO_HEAD_M_A: + case BMI160_FIFO_M_A_ENABLE: + + /* Partial read, then skip the data */ + if ((*idx + BMI160_FIFO_MA_LENGTH) > dev->fifo->length) + { + /* Update the data index as complete */ + *idx = dev->fifo->length; + break; + } + + /* Unpack the data array into structure instance */ + unpack_aux_data(&aux_data[*aux_index], *idx, dev); + + /* Move the data index */ + *idx = *idx + BMI160_FIFO_MA_LENGTH; + (*aux_index)++; + break; + case BMI160_FIFO_HEAD_M_G: + case BMI160_FIFO_M_G_ENABLE: + + /* Partial read, then skip the data */ + if ((*idx + BMI160_FIFO_MG_LENGTH) > dev->fifo->length) + { + /* Update the data index as complete */ + *idx = dev->fifo->length; + break; + } + + /* Unpack the data array into structure instance */ + unpack_aux_data(&aux_data[*aux_index], *idx, dev); + + /* Move the data index */ + (*idx) = (*idx) + BMI160_FIFO_MG_LENGTH; + (*aux_index)++; + break; + case BMI160_FIFO_HEAD_M_G_A: + case BMI160_FIFO_M_G_A_ENABLE: + + /*Partial read, then skip the data*/ + if ((*idx + BMI160_FIFO_MGA_LENGTH) > dev->fifo->length) + { + /* Update the data index as complete */ + *idx = dev->fifo->length; + break; + } + + /* Unpack the data array into structure instance */ + unpack_aux_data(&aux_data[*aux_index], *idx, dev); + + /*Move the data index*/ + *idx = *idx + BMI160_FIFO_MGA_LENGTH; + (*aux_index)++; + break; + case BMI160_FIFO_HEAD_G: + case BMI160_FIFO_G_ENABLE: + + /* Move the data index */ + (*idx) = (*idx) + BMI160_FIFO_G_LENGTH; + break; + case BMI160_FIFO_HEAD_G_A: + case BMI160_FIFO_G_A_ENABLE: + + /* Move the data index */ + *idx = *idx + BMI160_FIFO_GA_LENGTH; + break; + case BMI160_FIFO_HEAD_A: + case BMI160_FIFO_A_ENABLE: + + /* Move the data index */ + *idx = *idx + BMI160_FIFO_A_LENGTH; + break; + default: + break; + } +} + +/*! + * @brief This API is used to parse the aux data from the + * FIFO data and store it in the instance of the structure bmi160_aux_data. + */ +static void unpack_aux_data(struct bmi160_aux_data *aux_data, uint16_t data_start_index, const struct bmi160_dev *dev) +{ + /* Aux data bytes */ + aux_data->data[0] = dev->fifo->data[data_start_index++]; + aux_data->data[1] = dev->fifo->data[data_start_index++]; + aux_data->data[2] = dev->fifo->data[data_start_index++]; + aux_data->data[3] = dev->fifo->data[data_start_index++]; + aux_data->data[4] = dev->fifo->data[data_start_index++]; + aux_data->data[5] = dev->fifo->data[data_start_index++]; + aux_data->data[6] = dev->fifo->data[data_start_index++]; + aux_data->data[7] = dev->fifo->data[data_start_index++]; +} + +/*! + * @brief This API is used to parse the aux data from the + * FIFO data in header mode. + */ +static void extract_aux_header_mode(struct bmi160_aux_data *aux_data, uint8_t *aux_length, const struct bmi160_dev *dev) +{ + uint8_t frame_header = 0; + uint16_t data_index; + uint8_t aux_index = 0; + + for (data_index = dev->fifo->aux_byte_start_idx; data_index < dev->fifo->length;) + { + /* extracting Frame header */ + frame_header = (dev->fifo->data[data_index] & BMI160_FIFO_TAG_INTR_MASK); + + /*Index is moved to next byte where the data is starting*/ + data_index++; + switch (frame_header) + { + /* Aux frame */ + case BMI160_FIFO_HEAD_M: + case BMI160_FIFO_HEAD_M_A: + case BMI160_FIFO_HEAD_M_G: + case BMI160_FIFO_HEAD_M_G_A: + unpack_aux_frame(aux_data, &data_index, &aux_index, frame_header, dev); + break; + case BMI160_FIFO_HEAD_G: + move_next_frame(&data_index, BMI160_FIFO_G_LENGTH, dev); + break; + case BMI160_FIFO_HEAD_G_A: + move_next_frame(&data_index, BMI160_FIFO_GA_LENGTH, dev); + break; + case BMI160_FIFO_HEAD_A: + move_next_frame(&data_index, BMI160_FIFO_A_LENGTH, dev); + break; + + /* Sensor time frame */ + case BMI160_FIFO_HEAD_SENSOR_TIME: + unpack_sensortime_frame(&data_index, dev); + break; + + /* Skip frame */ + case BMI160_FIFO_HEAD_SKIP_FRAME: + unpack_skipped_frame(&data_index, dev); + break; + + /* Input config frame */ + case BMI160_FIFO_HEAD_INPUT_CONFIG: + move_next_frame(&data_index, 1, dev); + break; + case BMI160_FIFO_HEAD_OVER_READ: + + /* Update the data index as complete in case + * of over read */ + data_index = dev->fifo->length; + break; + default: + + /* Update the data index as complete in case of + * getting other headers like 0x00 */ + data_index = dev->fifo->length; + break; + } + if (*aux_length == aux_index) + { + /*Number of frames to read completed*/ + break; + } + } + + /* Update number of aux data read */ + *aux_length = aux_index; + + /* Update the aux frame index */ + dev->fifo->aux_byte_start_idx = data_index; +} + +/*! + * @brief This API checks the presence of non-valid frames in the read fifo data. + */ +static void check_frame_validity(uint16_t *data_index, const struct bmi160_dev *dev) +{ + if ((*data_index + 2) < dev->fifo->length) + { + /* Check if FIFO is empty */ + if ((dev->fifo->data[*data_index] == FIFO_CONFIG_MSB_CHECK) && + (dev->fifo->data[*data_index + 1] == FIFO_CONFIG_LSB_CHECK)) + { + /*Update the data index as complete*/ + *data_index = dev->fifo->length; + } + } +} + +/*! + * @brief This 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 void move_next_frame(uint16_t *data_index, uint8_t current_frame_length, const struct bmi160_dev *dev) +{ + /*Partial read, then move the data index to last data*/ + if ((*data_index + current_frame_length) > dev->fifo->length) + { + /*Update the data index as complete*/ + *data_index = dev->fifo->length; + } + else + { + /*Move the data index to next frame*/ + *data_index = *data_index + current_frame_length; + } +} + +/*! + * @brief This API is used to parse and store the sensor time from the + * FIFO data in the structure instance dev. + */ +static void unpack_sensortime_frame(uint16_t *data_index, const struct bmi160_dev *dev) +{ + uint32_t sensor_time_byte3 = 0; + uint16_t sensor_time_byte2 = 0; + uint8_t sensor_time_byte1 = 0; + + /*Partial read, then move the data index to last data*/ + if ((*data_index + BMI160_SENSOR_TIME_LENGTH) > dev->fifo->length) + { + /*Update the data index as complete*/ + *data_index = dev->fifo->length; + } + else + { + sensor_time_byte3 = dev->fifo->data[(*data_index) + BMI160_SENSOR_TIME_MSB_BYTE] << 16; + sensor_time_byte2 = dev->fifo->data[(*data_index) + BMI160_SENSOR_TIME_XLSB_BYTE] << 8; + sensor_time_byte1 = dev->fifo->data[(*data_index)]; + + /* Sensor time */ + dev->fifo->sensor_time = (uint32_t)(sensor_time_byte3 | sensor_time_byte2 | sensor_time_byte1); + *data_index = (*data_index) + BMI160_SENSOR_TIME_LENGTH; + } +} + +/*! + * @brief This API is used to parse and store the skipped_frame_count from + * the FIFO data in the structure instance dev. + */ +static void unpack_skipped_frame(uint16_t *data_index, const struct bmi160_dev *dev) +{ + /*Partial read, then move the data index to last data*/ + if (*data_index >= dev->fifo->length) + { + /*Update the data index as complete*/ + *data_index = dev->fifo->length; + } + else + { + dev->fifo->skipped_frame_count = dev->fifo->data[*data_index]; + + /*Move the data index*/ + *data_index = (*data_index) + 1; + } +} + +/*! + * @brief This API is used to get the FOC status from the sensor + */ +static int8_t get_foc_status(uint8_t *foc_status, struct bmi160_dev const *dev) +{ + int8_t rslt; + uint8_t data; + + /* Read the FOC status from sensor */ + rslt = bmi160_get_regs(BMI160_STATUS_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + /* Get the foc_status bit */ + *foc_status = BMI160_GET_BITS(data, BMI160_FOC_STATUS); + } + + return rslt; +} + +/*! + * @brief This API is used to configure the offset enable bits in the sensor + */ +static int8_t configure_offset_enable(const struct bmi160_foc_conf *foc_conf, struct bmi160_dev const *dev) +{ + int8_t rslt; + uint8_t data; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt != BMI160_OK) + { + rslt = BMI160_E_NULL_PTR; + } + else + { + /* Read the FOC config from the sensor */ + rslt = bmi160_get_regs(BMI160_OFFSET_CONF_ADDR, &data, 1, dev); + if (rslt == BMI160_OK) + { + /* Set the offset enable/disable for gyro */ + data = BMI160_SET_BITS(data, BMI160_GYRO_OFFSET_EN, foc_conf->gyro_off_en); + + /* Set the offset enable/disable for accel */ + data = BMI160_SET_BITS(data, BMI160_ACCEL_OFFSET_EN, foc_conf->acc_off_en); + + /* Set the offset config in the sensor */ + rslt = bmi160_set_regs(BMI160_OFFSET_CONF_ADDR, &data, 1, dev); + } + } + + return rslt; +} +static int8_t trigger_foc(struct bmi160_offsets *offset, struct bmi160_dev const *dev) +{ + int8_t rslt; + uint8_t foc_status; + uint8_t cmd = BMI160_START_FOC_CMD; + uint8_t timeout = 0; + uint8_t data_array[20]; + + /* Start the FOC process */ + rslt = bmi160_set_regs(BMI160_COMMAND_REG_ADDR, &cmd, 1, dev); + if (rslt == BMI160_OK) + { + /* Check the FOC status*/ + rslt = get_foc_status(&foc_status, dev); + if ((rslt != BMI160_OK) || (foc_status != BMI160_ENABLE)) + { + while ((foc_status != BMI160_ENABLE) && (timeout < 11)) + { + /* Maximum time of 250ms is given in 10 + * steps of 25ms each - 250ms refer datasheet 2.9.1 */ + dev->delay_ms(25); + + /* Check the FOC status*/ + rslt = get_foc_status(&foc_status, dev); + timeout++; + } + if ((rslt == BMI160_OK) && (foc_status == BMI160_ENABLE)) + { + /* Get offset values from sensor */ + rslt = bmi160_get_offsets(offset, dev); + } + else + { + /* FOC failure case */ + rslt = BMI160_FOC_FAILURE; + } + } + if (rslt == BMI160_OK) + { + /* Read registers 0x04-0x17 */ + rslt = bmi160_get_regs(BMI160_GYRO_DATA_ADDR, data_array, 20, dev); + } + } + + return rslt; +} + +/** @}*/ diff --git a/bmi160.h b/bmi160.h index ee279e6..8d9d49d 100644 --- a/bmi160.h +++ b/bmi160.h @@ -1,693 +1,683 @@ -/** - * Copyright (C) 2018 - 2019 Bosch Sensortec GmbH - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * - * 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. - * - * Neither the name of the copyright holder nor the names of the - * 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 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 - * - * The information provided is believed to be accurate and reliable. - * The copyright holder assumes no responsibility - * for the consequences of use - * of such information nor for any infringement of patents or - * other rights of third parties which may result from its use. - * No license is granted by implication or otherwise under any patent or - * patent rights of the copyright holder. - * - * @file bmi160.h - * @date 13 Mar 2019 - * @version 3.7.7 - * @brief - * - */ - -/*! - * @defgroup bmi160 - * @brief - * @{*/ - -#ifndef BMI160_H_ -#define BMI160_H_ - -/*************************** C++ guard macro *****************************/ -#ifdef __cplusplus -extern "C" { -#endif - -#include "bmi160_defs.h" -#ifdef __KERNEL__ -#include -#else -#include -#include -#include -#endif - -/*********************** User function prototypes ************************/ - -/*! - * @brief This API is the entry point for sensor.It performs - * the selection of I2C/SPI read mechanism according to the - * selected interface and reads the chip-id of bmi160 sensor. - * - * @param[in,out] dev : Structure instance of bmi160_dev - * @note : Refer user guide for detailed info. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -int8_t bmi160_init(struct bmi160_dev *dev); - -/*! - * @brief This API reads the data from the given register address of sensor. - * - * @param[in] reg_addr : Register address from where the data to be read - * @param[out] data : Pointer to data buffer to store the read data. - * @param[in] len : No of bytes of data to be read. - * @param[in] dev : Structure instance of bmi160_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 - 0x24(BMI160_FIFO_DATA_ADDR) - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -int8_t bmi160_get_regs(uint8_t reg_addr, uint8_t *data, uint16_t len, const struct bmi160_dev *dev); - -/*! - * @brief This API writes the given data to the register address - * of sensor. - * - * @param[in] reg_addr : Register address from where the data to be written. - * @param[in] data : Pointer to data buffer which is to be written - * in the sensor. - * @param[in] len : No of bytes of data to write.. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -int8_t bmi160_set_regs(uint8_t reg_addr, uint8_t *data, uint16_t len, const struct bmi160_dev *dev); - -/*! - * @brief This API resets and restarts the device. - * All register values are overwritten with default parameters. - * - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error. - */ -int8_t bmi160_soft_reset(struct bmi160_dev *dev); - -/*! - * @brief This API configures the power mode, range and bandwidth - * of sensor. - * - * @param[in] dev : Structure instance of bmi160_dev. - * @note : Refer user guide for detailed info. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error. - */ -int8_t bmi160_set_sens_conf(struct bmi160_dev *dev); - -/*! - * @brief This API sets the power mode of the sensor. - * - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error. - */ -int8_t bmi160_set_power_mode(struct bmi160_dev *dev); - -/*! - * @brief This API gets the power mode of the sensor. - * - * @param[in] power_mode : Power mode of the sensor - * @param[in] dev : Structure instance of bmi160_dev - * - * power_mode Macros possible values for pmu_status->aux_pmu_status : - * - BMI160_AUX_PMU_SUSPEND - * - BMI160_AUX_PMU_NORMAL - * - BMI160_AUX_PMU_LOW_POWER - * - * power_mode Macros possible values for pmu_status->gyro_pmu_status : - * - BMI160_GYRO_PMU_SUSPEND - * - BMI160_GYRO_PMU_NORMAL - * - BMI160_GYRO_PMU_FSU - * - * power_mode Macros possible values for pmu_status->accel_pmu_status : - * - BMI160_ACCEL_PMU_SUSPEND - * - BMI160_ACCEL_PMU_NORMAL - * - BMI160_ACCEL_PMU_LOW_POWER - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error. - */ -int8_t bmi160_get_power_mode(struct bmi160_pmu_status *pmu_status, const struct bmi160_dev *dev); - -/*! - * @brief This API reads sensor data, stores it in - * the bmi160_sensor_data structure pointer passed by the user. - * The user can ask for accel data ,gyro data or both sensor - * data using bmi160_select_sensor enum - * - * @param[in] select_sensor : enum to choose accel,gyro or both sensor data - * @param[out] accel : Structure pointer to store accel data - * @param[out] gyro : Structure pointer to store gyro data - * @param[in] dev : Structure instance of bmi160_dev. - * @note : Refer user guide for detailed info. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -int8_t bmi160_get_sensor_data(uint8_t select_sensor, - struct bmi160_sensor_data *accel, - struct bmi160_sensor_data *gyro, - const struct bmi160_dev *dev); - -/*! - * @brief This API configures the necessary interrupt based on - * the user settings in the bmi160_int_settg structure instance. - * - * @param[in] int_config : Structure instance of bmi160_int_settg. - * @param[in] dev : Structure instance of bmi160_dev. - * @note : Refer user guide for detailed info. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -int8_t bmi160_set_int_config(struct bmi160_int_settg *int_config, struct bmi160_dev *dev); - -/*! - * @brief This API enables the step counter feature. - * - * @param[in] step_cnt_enable : value to enable or disable - * @param[in] dev : Structure instance of bmi160_dev. - * @note : Refer user guide for detailed info. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -int8_t bmi160_set_step_counter(uint8_t step_cnt_enable, const struct bmi160_dev *dev); - -/*! - * @brief This API reads the step counter value. - * - * @param[in] step_val : Pointer to store the step counter value. - * @param[in] dev : Structure instance of bmi160_dev. - * @note : Refer user guide for detailed info. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -int8_t bmi160_read_step_counter(uint16_t *step_val, const struct bmi160_dev *dev); - -/*! - * @brief This API reads the mention no of byte of data from the given - * register address of auxiliary sensor. - * - * @param[in] reg_addr : Address of register to read. - * @param[in] aux_data : Pointer to store the read data. - * @param[in] len : No of bytes to read. - * @param[in] dev : Structure instance of bmi160_dev. - * @note : Refer user guide for detailed info. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -int8_t bmi160_aux_read(uint8_t reg_addr, uint8_t *aux_data, uint16_t len, const struct bmi160_dev *dev); - -/*! - * @brief This API writes the mention no of byte of data to the given - * register address of auxiliary sensor. - * - * @param[in] reg_addr : Address of register to write. - * @param[in] aux_data : Pointer to write data. - * @param[in] len : No of bytes to write. - * @param[in] dev : Structure instance of bmi160_dev. - * @note : Refer user guide for detailed info. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -int8_t bmi160_aux_write(uint8_t reg_addr, uint8_t *aux_data, uint16_t len, const struct bmi160_dev *dev); - -/*! - * @brief This API initialize the auxiliary sensor - * in order to access it. - * - * @param[in] dev : Structure instance of bmi160_dev. - * @note : Refer user guide for detailed info. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -int8_t bmi160_aux_init(const struct bmi160_dev *dev); - -/*! - * @brief This API is used to setup the auxiliary sensor of bmi160 in auto mode - * Thus enabling the auto update of 8 bytes of data from auxiliary sensor - * to BMI160 register address 0x04 to 0x0B - * - * @param[in] data_addr : Starting address of aux. sensor's data register - * (BMI160 registers 0x04 to 0x0B will be updated - * with 8 bytes of data from auxiliary sensor - * starting from this register address.) - * @param[in] dev : Structure instance of bmi160_dev. - * - * @note : Set the value of auxiliary polling rate by setting - * dev->aux_cfg.aux_odr to the required value from the table - * before calling this API - * - * dev->aux_cfg.aux_odr | Auxiliary ODR (Hz) - * -----------------------|----------------------- - * BMI160_AUX_ODR_0_78HZ | 25/32 - * BMI160_AUX_ODR_1_56HZ | 25/16 - * BMI160_AUX_ODR_3_12HZ | 25/8 - * BMI160_AUX_ODR_6_25HZ | 25/4 - * BMI160_AUX_ODR_12_5HZ | 25/2 - * BMI160_AUX_ODR_25HZ | 25 - * BMI160_AUX_ODR_50HZ | 50 - * BMI160_AUX_ODR_100HZ | 100 - * BMI160_AUX_ODR_200HZ | 200 - * BMI160_AUX_ODR_400HZ | 400 - * BMI160_AUX_ODR_800HZ | 800 - * - * @note : Other values of dev->aux_cfg.aux_odr are reserved and not for use - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -int8_t bmi160_set_aux_auto_mode(uint8_t *data_addr, struct bmi160_dev *dev); - -/*! - * @brief This API configures the 0x4C register and settings like - * Auxiliary sensor manual enable/ disable and aux burst read length. - * - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -int8_t bmi160_config_aux_mode(const struct bmi160_dev *dev); - -/*! - * @brief This API is used to read the raw uncompensated auxiliary sensor - * data of 8 bytes from BMI160 register address 0x04 to 0x0B - * - * @param[in] aux_data : Pointer to user array of length 8 bytes - * Ensure that the aux_data array is of - * length 8 bytes - * @param[in] dev : Structure instance of bmi160_dev - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - */ -int8_t bmi160_read_aux_data_auto_mode(uint8_t *aux_data, const struct bmi160_dev *dev); - -/*! - * @brief This is used to perform self test of accel/gyro of the BMI160 sensor - * - * @param[in] select_sensor : enum to choose accel or gyro for self test - * @param[in] dev : Structure instance of bmi160_dev - * - * @note self test can be performed either for accel/gyro at any instant. - * - * value of select_sensor | Inference - *----------------------------------|-------------------------------- - * BMI160_ACCEL_ONLY | Accel self test enabled - * BMI160_GYRO_ONLY | Gyro self test enabled - * BMI160_BOTH_ACCEL_AND_GYRO | NOT TO BE USED - * - * @note The return value of this API gives us the result of self test. - * - * @note Performing self test does soft reset of the sensor, User can - * set the desired settings after performing the self test. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error / +ve value -> Self-test fail - * - * Return value | Result of self test - * --------------------------------|--------------------------------- - * BMI160_OK | Self test success - * BMI160_W_GYRO_SELF_TEST_FAIL | Gyro self test fail - * BMI160_W_ACCEl_SELF_TEST_FAIL | Accel self test fail - */ -int8_t bmi160_perform_self_test(uint8_t select_sensor, struct bmi160_dev *dev); - -/*! - * @brief This API reads data from the fifo buffer. - * - * @note User has to allocate the FIFO buffer along with - * corresponding fifo length from his side before calling this API - * as mentioned in the readme.md - * - * @note User must specify the number of bytes to read from the FIFO in - * dev->fifo->length , It will be updated by the number of bytes actually - * read from FIFO after calling this API - * - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval zero -> Success / -ve value -> Error - * - */ -int8_t bmi160_get_fifo_data(struct bmi160_dev const *dev); - -/*! - * @brief This API writes fifo_flush command to command register.This - * action clears all data in the Fifo without changing fifo configuration - * settings. - * - * @param[in] dev : Structure instance of bmi160_dev - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval Any non zero value -> Fail - * - */ -int8_t bmi160_set_fifo_flush(const struct bmi160_dev *dev); - -/*! @brief This API sets the FIFO configuration in the sensor. - * - * @param[in] config : variable used to specify the FIFO - * configurations which are to be enabled or disabled in the sensor. - * - * @note : User can set either set one or more or all FIFO configurations - * by ORing the below mentioned macros. - * config | Value - * ------------------------|--------------------------- - * BMI160_FIFO_TIME | 0x02 - * BMI160_FIFO_TAG_INT2 | 0x04 - * BMI160_FIFO_TAG_INT1 | 0x08 - * BMI160_FIFO_HEADER | 0x10 - * BMI160_FIFO_AUX | 0x20 - * BMI160_FIFO_ACCEL | 0x40 - * BMI160_FIFO_GYRO | 0x80 - * - * @param[in] enable : Parameter used to enable or disable the above - * FIFO configuration - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return status of bus communication result - * @retval 0 -> Success - * @retval Any non zero value -> Fail - * - */ -int8_t bmi160_set_fifo_config(uint8_t config, uint8_t enable, struct bmi160_dev const *dev); - -/*! @brief This API is used to configure the down sampling ratios of - * the accel and gyro data for FIFO.Also, it configures filtered or - * pre-filtered data for the fifo for accel and gyro. - * - * @param[in] fifo_down : variable used to specify the FIFO down - * configurations which are to be enabled or disabled in the sensor. - * - * @note The user must select one among the following macros to - * select down-sampling ratio for accel - * config | Value - * -------------------------------------|--------------------------- - * BMI160_ACCEL_FIFO_DOWN_ZERO | 0x00 - * BMI160_ACCEL_FIFO_DOWN_ONE | 0x10 - * BMI160_ACCEL_FIFO_DOWN_TWO | 0x20 - * BMI160_ACCEL_FIFO_DOWN_THREE | 0x30 - * BMI160_ACCEL_FIFO_DOWN_FOUR | 0x40 - * BMI160_ACCEL_FIFO_DOWN_FIVE | 0x50 - * BMI160_ACCEL_FIFO_DOWN_SIX | 0x60 - * BMI160_ACCEL_FIFO_DOWN_SEVEN | 0x70 - * - * @note The user must select one among the following macros to - * select down-sampling ratio for gyro - * - * config | Value - * -------------------------------------|--------------------------- - * BMI160_GYRO_FIFO_DOWN_ZERO | 0x00 - * BMI160_GYRO_FIFO_DOWN_ONE | 0x01 - * BMI160_GYRO_FIFO_DOWN_TWO | 0x02 - * BMI160_GYRO_FIFO_DOWN_THREE | 0x03 - * BMI160_GYRO_FIFO_DOWN_FOUR | 0x04 - * BMI160_GYRO_FIFO_DOWN_FIVE | 0x05 - * BMI160_GYRO_FIFO_DOWN_SIX | 0x06 - * BMI160_GYRO_FIFO_DOWN_SEVEN | 0x07 - * - * @note The user can enable filtered accel data by the following macro - * config | Value - * -------------------------------------|--------------------------- - * BMI160_ACCEL_FIFO_FILT_EN | 0x80 - * - * @note The user can enable filtered gyro data by the following macro - * config | Value - * -------------------------------------|--------------------------- - * BMI160_GYRO_FIFO_FILT_EN | 0x08 - * - * @note : By ORing the above mentioned macros, the user can select - * the required FIFO down config settings - * - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return status of bus communication result - * @retval 0 -> Success - * @retval Any non zero value -> Fail - * - */ -int8_t bmi160_set_fifo_down(uint8_t fifo_down, const struct bmi160_dev *dev); - -/*! - * @brief This API sets the FIFO watermark level in the sensor. - * - * @note The FIFO watermark is issued when the FIFO fill level is - * equal or above the watermark level and units of watermark is 4 bytes. - * - * @param[in] fifo_wm : Variable used to set the FIFO water mark level - * @param[in] dev : Structure instance of bmi160_dev - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval Any non zero value -> Fail - * - */ -int8_t bmi160_set_fifo_wm(uint8_t fifo_wm, const struct bmi160_dev *dev); - -/*! - * @brief This API parses and extracts the accelerometer frames from - * FIFO data read by the "bmi160_get_fifo_data" API and stores it in - * the "accel_data" structure instance. - * - * @note The bmi160_extract_accel API should be called only after - * reading the FIFO data by calling the bmi160_get_fifo_data() API. - * - * @param[out] accel_data : Structure instance of bmi160_sensor_data - * where the accelerometer data in FIFO is stored. - * @param[in,out] accel_length : Number of valid accelerometer frames - * (x,y,z axes data) read out from fifo. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @note accel_length is updated with the number of valid accelerometer - * frames extracted from fifo (1 accel frame = 6 bytes) at the end of - * execution of this API. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval Any non zero value -> Fail - * - */ -int8_t bmi160_extract_accel(struct bmi160_sensor_data *accel_data, uint8_t *accel_length, struct bmi160_dev const *dev); - -/*! - * @brief This API parses and extracts the gyro frames from - * FIFO data read by the "bmi160_get_fifo_data" API and stores it in - * the "gyro_data" structure instance. - * - * @note The bmi160_extract_gyro API should be called only after - * reading the FIFO data by calling the bmi160_get_fifo_data() API. - * - * @param[out] gyro_data : Structure instance of bmi160_sensor_data - * where the gyro data in FIFO is stored. - * @param[in,out] gyro_length : Number of valid gyro frames - * (x,y,z axes data) read out from fifo. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @note gyro_length is updated with the number of valid gyro - * frames extracted from fifo (1 gyro frame = 6 bytes) at the end of - * execution of this API. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval Any non zero value -> Fail - * - */ -int8_t bmi160_extract_gyro(struct bmi160_sensor_data *gyro_data, uint8_t *gyro_length, struct bmi160_dev const *dev); - -/*! - * @brief This API parses and extracts the aux frames from - * FIFO data read by the "bmi160_get_fifo_data" API and stores it in - * the bmi160_aux_data structure instance. - * - * @note The bmi160_extract_aux API should be called only after - * reading the FIFO data by calling the bmi160_get_fifo_data() API. - * - * @param[out] aux_data : Structure instance of bmi160_aux_data - * where the aux data in FIFO is stored. - * @param[in,out] aux_len : Number of valid aux frames (8bytes) - * read out from FIFO. - * @param[in] dev : Structure instance of bmi160_dev. - * - * @note aux_len is updated with the number of valid aux - * frames extracted from fifo (1 aux frame = 8 bytes) at the end of - * execution of this API. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval Any non zero value -> Fail - * - */ -int8_t bmi160_extract_aux(struct bmi160_aux_data *aux_data, uint8_t *aux_len, struct bmi160_dev const *dev); - -/*! - * @brief This API starts the FOC of accel and gyro - * - * @note FOC should not be used in low-power mode of sensor - * - * @note Accel FOC targets values of +1g , 0g , -1g - * Gyro FOC always targets value of 0 dps - * - * @param[in] foc_conf : Structure instance of bmi160_foc_conf which - * has the FOC configuration - * @param[in,out] offset : Structure instance to store Offset - * values read from sensor - * @param[in] dev : Structure instance of bmi160_dev. - * - * @note Pre-requisites for triggering FOC in accel , Set the following, - * Enable the acc_off_en - * Ex : foc_conf.acc_off_en = BMI160_ENABLE; - * - * Set the desired target values of FOC to each axes (x,y,z) by using the - * following macros - * - BMI160_FOC_ACCEL_DISABLED - * - BMI160_FOC_ACCEL_POSITIVE_G - * - BMI160_FOC_ACCEL_NEGATIVE_G - * - BMI160_FOC_ACCEL_0G - * - * Ex : foc_conf.foc_acc_x = BMI160_FOC_ACCEL_0G; - * foc_conf.foc_acc_y = BMI160_FOC_ACCEL_0G; - * foc_conf.foc_acc_z = BMI160_FOC_ACCEL_POSITIVE_G; - * - * @note Pre-requisites for triggering FOC in gyro , - * Set the following parameters, - * - * Ex : foc_conf.foc_gyr_en = BMI160_ENABLE; - * foc_conf.gyro_off_en = BMI160_ENABLE; - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval Any non zero value -> Fail - */ -int8_t bmi160_start_foc(const struct bmi160_foc_conf *foc_conf, - struct bmi160_offsets *offset, - struct bmi160_dev const *dev); - -/*! - * @brief This API reads and stores the offset values of accel and gyro - * - * @param[in,out] offset : Structure instance of bmi160_offsets in which - * the offset values are read and stored - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval Any non zero value -> Fail - */ -int8_t bmi160_get_offsets(struct bmi160_offsets *offset, const struct bmi160_dev *dev); - -/*! - * @brief This API writes the offset values of accel and gyro to - * the sensor but these values will be reset on POR or soft reset. - * - * @param[in] foc_conf : Structure instance of bmi160_foc_conf which - * has the FOC configuration - * @param[in] offset : Structure instance in which user updates offset - * values which are to be written in the sensor - * @param[in] dev : Structure instance of bmi160_dev. - * - * @note Offsets can be set by user like offset->off_acc_x = 10; - * where 1LSB = 3.9mg and for gyro 1LSB = 0.061degrees/second - * - * @note BMI160 offset values for xyz axes of accel should be within range of - * BMI160_ACCEL_MIN_OFFSET (-128) to BMI160_ACCEL_MAX_OFFSET (127) - * - * @note BMI160 offset values for xyz axes of gyro should be within range of - * BMI160_GYRO_MIN_OFFSET (-512) to BMI160_GYRO_MAX_OFFSET (511) - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval Any non zero value -> Fail - */ -int8_t bmi160_set_offsets(const struct bmi160_foc_conf *foc_conf, - const struct bmi160_offsets *offset, - struct bmi160_dev const *dev); - -/*! - * @brief This API writes the image registers values to NVM which is - * stored even after POR or soft reset - * - * @param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval Any non zero value -> Fail - */ -int8_t bmi160_update_nvm(struct bmi160_dev const *dev); - -/*! - * @brief This API gets the interrupt status from the sensor. - * - * @param[in] int_status_sel : Enum variable to select either individual or all the - * interrupt status bits. - * @param[in] int_status : pointer variable to get the interrupt status - * from the sensor. - * param[in] dev : Structure instance of bmi160_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval Any non zero value -> Fail - */ -int8_t bmi160_get_int_status(enum bmi160_int_status_sel int_status_sel, - union bmi160_int_status *int_status, - struct bmi160_dev const *dev); - -/*************************** C++ guard macro *****************************/ -#ifdef __cplusplus -} -#endif - -#endif /* BMI160_H_ */ -/** @}*/ +/** +* 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 bmi160.h +* @date 10/01/2020 +* @version 3.8.1 +* +*/ + +/*! + * @defgroup bmi160 + * @brief + * @{*/ + +#ifndef BMI160_H_ +#define BMI160_H_ + +/*************************** C++ guard macro *****************************/ +#ifdef __cplusplus +extern "C" { +#endif + +#include "bmi160_defs.h" +#ifdef __KERNEL__ +#include +#else +#include +#include +#include +#endif + +/*********************** User function prototypes ************************/ + +/*! + * @brief This API is the entry point for sensor.It performs + * the selection of I2C/SPI read mechanism according to the + * selected interface and reads the chip-id of bmi160 sensor. + * + * @param[in,out] dev : Structure instance of bmi160_dev + * @note : Refer user guide for detailed info. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +int8_t bmi160_init(struct bmi160_dev *dev); + +/*! + * @brief This API reads the data from the given register address of sensor. + * + * @param[in] reg_addr : Register address from where the data to be read + * @param[out] data : Pointer to data buffer to store the read data. + * @param[in] len : No of bytes of data to be read. + * @param[in] dev : Structure instance of bmi160_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 - 0x24(BMI160_FIFO_DATA_ADDR) + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +int8_t bmi160_get_regs(uint8_t reg_addr, uint8_t *data, uint16_t len, const struct bmi160_dev *dev); + +/*! + * @brief This API writes the given data to the register address + * of sensor. + * + * @param[in] reg_addr : Register address from where the data to be written. + * @param[in] data : Pointer to data buffer which is to be written + * in the sensor. + * @param[in] len : No of bytes of data to write.. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +int8_t bmi160_set_regs(uint8_t reg_addr, uint8_t *data, uint16_t len, const struct bmi160_dev *dev); + +/*! + * @brief This API resets and restarts the device. + * All register values are overwritten with default parameters. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +int8_t bmi160_soft_reset(struct bmi160_dev *dev); + +/*! + * @brief This API configures the power mode, range and bandwidth + * of sensor. + * + * @param[in] dev : Structure instance of bmi160_dev. + * @note : Refer user guide for detailed info. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +int8_t bmi160_set_sens_conf(struct bmi160_dev *dev); + +/*! + * @brief This API sets the power mode of the sensor. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +int8_t bmi160_set_power_mode(struct bmi160_dev *dev); + +/*! + * @brief This API gets the power mode of the sensor. + * + * @param[in] power_mode : Power mode of the sensor + * @param[in] dev : Structure instance of bmi160_dev + * + * power_mode Macros possible values for pmu_status->aux_pmu_status : + * - BMI160_AUX_PMU_SUSPEND + * - BMI160_AUX_PMU_NORMAL + * - BMI160_AUX_PMU_LOW_POWER + * + * power_mode Macros possible values for pmu_status->gyro_pmu_status : + * - BMI160_GYRO_PMU_SUSPEND + * - BMI160_GYRO_PMU_NORMAL + * - BMI160_GYRO_PMU_FSU + * + * power_mode Macros possible values for pmu_status->accel_pmu_status : + * - BMI160_ACCEL_PMU_SUSPEND + * - BMI160_ACCEL_PMU_NORMAL + * - BMI160_ACCEL_PMU_LOW_POWER + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error. + */ +int8_t bmi160_get_power_mode(struct bmi160_pmu_status *pmu_status, const struct bmi160_dev *dev); + +/*! + * @brief This API reads sensor data, stores it in + * the bmi160_sensor_data structure pointer passed by the user. + * The user can ask for accel data ,gyro data or both sensor + * data using bmi160_select_sensor enum + * + * @param[in] select_sensor : enum to choose accel,gyro or both sensor data + * @param[out] accel : Structure pointer to store accel data + * @param[out] gyro : Structure pointer to store gyro data + * @param[in] dev : Structure instance of bmi160_dev. + * @note : Refer user guide for detailed info. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +int8_t bmi160_get_sensor_data(uint8_t select_sensor, + struct bmi160_sensor_data *accel, + struct bmi160_sensor_data *gyro, + const struct bmi160_dev *dev); + +/*! + * @brief This API configures the necessary interrupt based on + * the user settings in the bmi160_int_settg structure instance. + * + * @param[in] int_config : Structure instance of bmi160_int_settg. + * @param[in] dev : Structure instance of bmi160_dev. + * @note : Refer user guide for detailed info. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +int8_t bmi160_set_int_config(struct bmi160_int_settg *int_config, struct bmi160_dev *dev); + +/*! + * @brief This API enables the step counter feature. + * + * @param[in] step_cnt_enable : value to enable or disable + * @param[in] dev : Structure instance of bmi160_dev. + * @note : Refer user guide for detailed info. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +int8_t bmi160_set_step_counter(uint8_t step_cnt_enable, const struct bmi160_dev *dev); + +/*! + * @brief This API reads the step counter value. + * + * @param[in] step_val : Pointer to store the step counter value. + * @param[in] dev : Structure instance of bmi160_dev. + * @note : Refer user guide for detailed info. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +int8_t bmi160_read_step_counter(uint16_t *step_val, const struct bmi160_dev *dev); + +/*! + * @brief This API reads the mention no of byte of data from the given + * register address of auxiliary sensor. + * + * @param[in] reg_addr : Address of register to read. + * @param[in] aux_data : Pointer to store the read data. + * @param[in] len : No of bytes to read. + * @param[in] dev : Structure instance of bmi160_dev. + * @note : Refer user guide for detailed info. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +int8_t bmi160_aux_read(uint8_t reg_addr, uint8_t *aux_data, uint16_t len, const struct bmi160_dev *dev); + +/*! + * @brief This API writes the mention no of byte of data to the given + * register address of auxiliary sensor. + * + * @param[in] reg_addr : Address of register to write. + * @param[in] aux_data : Pointer to write data. + * @param[in] len : No of bytes to write. + * @param[in] dev : Structure instance of bmi160_dev. + * @note : Refer user guide for detailed info. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +int8_t bmi160_aux_write(uint8_t reg_addr, uint8_t *aux_data, uint16_t len, const struct bmi160_dev *dev); + +/*! + * @brief This API initialize the auxiliary sensor + * in order to access it. + * + * @param[in] dev : Structure instance of bmi160_dev. + * @note : Refer user guide for detailed info. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +int8_t bmi160_aux_init(const struct bmi160_dev *dev); + +/*! + * @brief This API is used to setup the auxiliary sensor of bmi160 in auto mode + * Thus enabling the auto update of 8 bytes of data from auxiliary sensor + * to BMI160 register address 0x04 to 0x0B + * + * @param[in] data_addr : Starting address of aux. sensor's data register + * (BMI160 registers 0x04 to 0x0B will be updated + * with 8 bytes of data from auxiliary sensor + * starting from this register address.) + * @param[in] dev : Structure instance of bmi160_dev. + * + * @note : Set the value of auxiliary polling rate by setting + * dev->aux_cfg.aux_odr to the required value from the table + * before calling this API + * + * dev->aux_cfg.aux_odr | Auxiliary ODR (Hz) + * -----------------------|----------------------- + * BMI160_AUX_ODR_0_78HZ | 25/32 + * BMI160_AUX_ODR_1_56HZ | 25/16 + * BMI160_AUX_ODR_3_12HZ | 25/8 + * BMI160_AUX_ODR_6_25HZ | 25/4 + * BMI160_AUX_ODR_12_5HZ | 25/2 + * BMI160_AUX_ODR_25HZ | 25 + * BMI160_AUX_ODR_50HZ | 50 + * BMI160_AUX_ODR_100HZ | 100 + * BMI160_AUX_ODR_200HZ | 200 + * BMI160_AUX_ODR_400HZ | 400 + * BMI160_AUX_ODR_800HZ | 800 + * + * @note : Other values of dev->aux_cfg.aux_odr are reserved and not for use + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +int8_t bmi160_set_aux_auto_mode(uint8_t *data_addr, struct bmi160_dev *dev); + +/*! + * @brief This API configures the 0x4C register and settings like + * Auxiliary sensor manual enable/ disable and aux burst read length. + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +int8_t bmi160_config_aux_mode(const struct bmi160_dev *dev); + +/*! + * @brief This API is used to read the raw uncompensated auxiliary sensor + * data of 8 bytes from BMI160 register address 0x04 to 0x0B + * + * @param[in] aux_data : Pointer to user array of length 8 bytes + * Ensure that the aux_data array is of + * length 8 bytes + * @param[in] dev : Structure instance of bmi160_dev + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + */ +int8_t bmi160_read_aux_data_auto_mode(uint8_t *aux_data, const struct bmi160_dev *dev); + +/*! + * @brief This is used to perform self test of accel/gyro of the BMI160 sensor + * + * @param[in] select_sensor : enum to choose accel or gyro for self test + * @param[in] dev : Structure instance of bmi160_dev + * + * @note self test can be performed either for accel/gyro at any instant. + * + * value of select_sensor | Inference + *----------------------------------|-------------------------------- + * BMI160_ACCEL_ONLY | Accel self test enabled + * BMI160_GYRO_ONLY | Gyro self test enabled + * BMI160_BOTH_ACCEL_AND_GYRO | NOT TO BE USED + * + * @note The return value of this API gives us the result of self test. + * + * @note Performing self test does soft reset of the sensor, User can + * set the desired settings after performing the self test. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error / +ve value -> Self-test fail + * + * Return value | Result of self test + * --------------------------------|--------------------------------- + * BMI160_OK | Self test success + * BMI160_W_GYRO_SELF_TEST_FAIL | Gyro self test fail + * BMI160_W_ACCEl_SELF_TEST_FAIL | Accel self test fail + */ +int8_t bmi160_perform_self_test(uint8_t select_sensor, struct bmi160_dev *dev); + +/*! + * @brief This API reads data from the fifo buffer. + * + * @note User has to allocate the FIFO buffer along with + * corresponding fifo length from his side before calling this API + * as mentioned in the readme.md + * + * @note User must specify the number of bytes to read from the FIFO in + * dev->fifo->length , It will be updated by the number of bytes actually + * read from FIFO after calling this API + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval zero -> Success / -ve value -> Error + * + */ +int8_t bmi160_get_fifo_data(struct bmi160_dev const *dev); + +/*! + * @brief This API writes fifo_flush command to command register.This + * action clears all data in the Fifo without changing fifo configuration + * settings. + * + * @param[in] dev : Structure instance of bmi160_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +int8_t bmi160_set_fifo_flush(const struct bmi160_dev *dev); + +/*! @brief This API sets the FIFO configuration in the sensor. + * + * @param[in] config : variable used to specify the FIFO + * configurations which are to be enabled or disabled in the sensor. + * + * @note : User can set either set one or more or all FIFO configurations + * by ORing the below mentioned macros. + * config | Value + * ------------------------|--------------------------- + * BMI160_FIFO_TIME | 0x02 + * BMI160_FIFO_TAG_INT2 | 0x04 + * BMI160_FIFO_TAG_INT1 | 0x08 + * BMI160_FIFO_HEADER | 0x10 + * BMI160_FIFO_AUX | 0x20 + * BMI160_FIFO_ACCEL | 0x40 + * BMI160_FIFO_GYRO | 0x80 + * + * @param[in] enable : Parameter used to enable or disable the above + * FIFO configuration + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return status of bus communication result + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +int8_t bmi160_set_fifo_config(uint8_t config, uint8_t enable, struct bmi160_dev const *dev); + +/*! @brief This API is used to configure the down sampling ratios of + * the accel and gyro data for FIFO.Also, it configures filtered or + * pre-filtered data for the fifo for accel and gyro. + * + * @param[in] fifo_down : variable used to specify the FIFO down + * configurations which are to be enabled or disabled in the sensor. + * + * @note The user must select one among the following macros to + * select down-sampling ratio for accel + * config | Value + * -------------------------------------|--------------------------- + * BMI160_ACCEL_FIFO_DOWN_ZERO | 0x00 + * BMI160_ACCEL_FIFO_DOWN_ONE | 0x10 + * BMI160_ACCEL_FIFO_DOWN_TWO | 0x20 + * BMI160_ACCEL_FIFO_DOWN_THREE | 0x30 + * BMI160_ACCEL_FIFO_DOWN_FOUR | 0x40 + * BMI160_ACCEL_FIFO_DOWN_FIVE | 0x50 + * BMI160_ACCEL_FIFO_DOWN_SIX | 0x60 + * BMI160_ACCEL_FIFO_DOWN_SEVEN | 0x70 + * + * @note The user must select one among the following macros to + * select down-sampling ratio for gyro + * + * config | Value + * -------------------------------------|--------------------------- + * BMI160_GYRO_FIFO_DOWN_ZERO | 0x00 + * BMI160_GYRO_FIFO_DOWN_ONE | 0x01 + * BMI160_GYRO_FIFO_DOWN_TWO | 0x02 + * BMI160_GYRO_FIFO_DOWN_THREE | 0x03 + * BMI160_GYRO_FIFO_DOWN_FOUR | 0x04 + * BMI160_GYRO_FIFO_DOWN_FIVE | 0x05 + * BMI160_GYRO_FIFO_DOWN_SIX | 0x06 + * BMI160_GYRO_FIFO_DOWN_SEVEN | 0x07 + * + * @note The user can enable filtered accel data by the following macro + * config | Value + * -------------------------------------|--------------------------- + * BMI160_ACCEL_FIFO_FILT_EN | 0x80 + * + * @note The user can enable filtered gyro data by the following macro + * config | Value + * -------------------------------------|--------------------------- + * BMI160_GYRO_FIFO_FILT_EN | 0x08 + * + * @note : By ORing the above mentioned macros, the user can select + * the required FIFO down config settings + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return status of bus communication result + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +int8_t bmi160_set_fifo_down(uint8_t fifo_down, const struct bmi160_dev *dev); + +/*! + * @brief This API sets the FIFO watermark level in the sensor. + * + * @note The FIFO watermark is issued when the FIFO fill level is + * equal or above the watermark level and units of watermark is 4 bytes. + * + * @param[in] fifo_wm : Variable used to set the FIFO water mark level + * @param[in] dev : Structure instance of bmi160_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +int8_t bmi160_set_fifo_wm(uint8_t fifo_wm, const struct bmi160_dev *dev); + +/*! + * @brief This API parses and extracts the accelerometer frames from + * FIFO data read by the "bmi160_get_fifo_data" API and stores it in + * the "accel_data" structure instance. + * + * @note The bmi160_extract_accel API should be called only after + * reading the FIFO data by calling the bmi160_get_fifo_data() API. + * + * @param[out] accel_data : Structure instance of bmi160_sensor_data + * where the accelerometer data in FIFO is stored. + * @param[in,out] accel_length : Number of valid accelerometer frames + * (x,y,z axes data) read out from fifo. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @note accel_length is updated with the number of valid accelerometer + * frames extracted from fifo (1 accel frame = 6 bytes) at the end of + * execution of this API. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +int8_t bmi160_extract_accel(struct bmi160_sensor_data *accel_data, uint8_t *accel_length, struct bmi160_dev const *dev); + +/*! + * @brief This API parses and extracts the gyro frames from + * FIFO data read by the "bmi160_get_fifo_data" API and stores it in + * the "gyro_data" structure instance. + * + * @note The bmi160_extract_gyro API should be called only after + * reading the FIFO data by calling the bmi160_get_fifo_data() API. + * + * @param[out] gyro_data : Structure instance of bmi160_sensor_data + * where the gyro data in FIFO is stored. + * @param[in,out] gyro_length : Number of valid gyro frames + * (x,y,z axes data) read out from fifo. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @note gyro_length is updated with the number of valid gyro + * frames extracted from fifo (1 gyro frame = 6 bytes) at the end of + * execution of this API. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +int8_t bmi160_extract_gyro(struct bmi160_sensor_data *gyro_data, uint8_t *gyro_length, struct bmi160_dev const *dev); + +/*! + * @brief This API parses and extracts the aux frames from + * FIFO data read by the "bmi160_get_fifo_data" API and stores it in + * the bmi160_aux_data structure instance. + * + * @note The bmi160_extract_aux API should be called only after + * reading the FIFO data by calling the bmi160_get_fifo_data() API. + * + * @param[out] aux_data : Structure instance of bmi160_aux_data + * where the aux data in FIFO is stored. + * @param[in,out] aux_len : Number of valid aux frames (8bytes) + * read out from FIFO. + * @param[in] dev : Structure instance of bmi160_dev. + * + * @note aux_len is updated with the number of valid aux + * frames extracted from fifo (1 aux frame = 8 bytes) at the end of + * execution of this API. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +int8_t bmi160_extract_aux(struct bmi160_aux_data *aux_data, uint8_t *aux_len, struct bmi160_dev const *dev); + +/*! + * @brief This API starts the FOC of accel and gyro + * + * @note FOC should not be used in low-power mode of sensor + * + * @note Accel FOC targets values of +1g , 0g , -1g + * Gyro FOC always targets value of 0 dps + * + * @param[in] foc_conf : Structure instance of bmi160_foc_conf which + * has the FOC configuration + * @param[in,out] offset : Structure instance to store Offset + * values read from sensor + * @param[in] dev : Structure instance of bmi160_dev. + * + * @note Pre-requisites for triggering FOC in accel , Set the following, + * Enable the acc_off_en + * Ex : foc_conf.acc_off_en = BMI160_ENABLE; + * + * Set the desired target values of FOC to each axes (x,y,z) by using the + * following macros + * - BMI160_FOC_ACCEL_DISABLED + * - BMI160_FOC_ACCEL_POSITIVE_G + * - BMI160_FOC_ACCEL_NEGATIVE_G + * - BMI160_FOC_ACCEL_0G + * + * Ex : foc_conf.foc_acc_x = BMI160_FOC_ACCEL_0G; + * foc_conf.foc_acc_y = BMI160_FOC_ACCEL_0G; + * foc_conf.foc_acc_z = BMI160_FOC_ACCEL_POSITIVE_G; + * + * @note Pre-requisites for triggering FOC in gyro , + * Set the following parameters, + * + * Ex : foc_conf.foc_gyr_en = BMI160_ENABLE; + * foc_conf.gyro_off_en = BMI160_ENABLE; + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + */ +int8_t bmi160_start_foc(const struct bmi160_foc_conf *foc_conf, + struct bmi160_offsets *offset, + struct bmi160_dev const *dev); + +/*! + * @brief This API reads and stores the offset values of accel and gyro + * + * @param[in,out] offset : Structure instance of bmi160_offsets in which + * the offset values are read and stored + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + */ +int8_t bmi160_get_offsets(struct bmi160_offsets *offset, const struct bmi160_dev *dev); + +/*! + * @brief This API writes the offset values of accel and gyro to + * the sensor but these values will be reset on POR or soft reset. + * + * @param[in] foc_conf : Structure instance of bmi160_foc_conf which + * has the FOC configuration + * @param[in] offset : Structure instance in which user updates offset + * values which are to be written in the sensor + * @param[in] dev : Structure instance of bmi160_dev. + * + * @note Offsets can be set by user like offset->off_acc_x = 10; + * where 1LSB = 3.9mg and for gyro 1LSB = 0.061degrees/second + * + * @note BMI160 offset values for xyz axes of accel should be within range of + * BMI160_ACCEL_MIN_OFFSET (-128) to BMI160_ACCEL_MAX_OFFSET (127) + * + * @note BMI160 offset values for xyz axes of gyro should be within range of + * BMI160_GYRO_MIN_OFFSET (-512) to BMI160_GYRO_MAX_OFFSET (511) + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + */ +int8_t bmi160_set_offsets(const struct bmi160_foc_conf *foc_conf, + const struct bmi160_offsets *offset, + struct bmi160_dev const *dev); + +/*! + * @brief This API writes the image registers values to NVM which is + * stored even after POR or soft reset + * + * @param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + */ +int8_t bmi160_update_nvm(struct bmi160_dev const *dev); + +/*! + * @brief This API gets the interrupt status from the sensor. + * + * @param[in] int_status_sel : Enum variable to select either individual or all the + * interrupt status bits. + * @param[in] int_status : pointer variable to get the interrupt status + * from the sensor. + * param[in] dev : Structure instance of bmi160_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + */ +int8_t bmi160_get_int_status(enum bmi160_int_status_sel int_status_sel, + union bmi160_int_status *int_status, + struct bmi160_dev const *dev); + +/*************************** C++ guard macro *****************************/ +#ifdef __cplusplus +} +#endif + +#endif /* BMI160_H_ */ +/** @}*/ diff --git a/bmi160_defs.h b/bmi160_defs.h index 73afb4b..cc66c56 100644 --- a/bmi160_defs.h +++ b/bmi160_defs.h @@ -1,1674 +1,1675 @@ -/** - * Copyright (C) 2018 - 2019 Bosch Sensortec GmbH - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * - * 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. - * - * Neither the name of the copyright holder nor the names of the - * 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 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 - * - * The information provided is believed to be accurate and reliable. - * The copyright holder assumes no responsibility - * for the consequences of use - * of such information nor for any infringement of patents or - * other rights of third parties which may result from its use. - * No license is granted by implication or otherwise under any patent or - * patent rights of the copyright holder. - * - * @file bmi160_defs.h - * @date 13 Mar 2019 - * @version 3.7.7 - * @brief - * - */ - -/*! - * @defgroup bmi160_defs - * @brief - * @{*/ - -#ifndef BMI160_DEFS_H_ -#define BMI160_DEFS_H_ - -/*************************** C types headers *****************************/ -#ifdef __KERNEL__ -#include -#include -#else -#include -#include -#endif - -/*************************** Common macros *****************************/ - -#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 - -/**@}*/ -/**\name C standard macros */ -#ifndef NULL -#ifdef __cplusplus -#define NULL 0 -#else -#define NULL ((void *) 0) -#endif -#endif - -/*************************** Sensor macros *****************************/ -/* Test for an endian machine */ -#ifndef __ORDER_LITTLE_ENDIAN__ -#define __ORDER_LITTLE_ENDIAN__ 0 -#endif - -#ifndef __BYTE_ORDER__ -#define __BYTE_ORDER__ __ORDER_LITTLE_ENDIAN__ -#endif - -#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__ -#define LITTLE_ENDIAN 1 -#elif __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__ -#define BIG_ENDIAN 1 -#else -#error "Code does not support Endian format of the processor" -#endif - -/** Mask definitions */ -#define BMI160_ACCEL_BW_MASK UINT8_C(0x70) -#define BMI160_ACCEL_ODR_MASK UINT8_C(0x0F) -#define BMI160_ACCEL_UNDERSAMPLING_MASK UINT8_C(0x80) -#define BMI160_ACCEL_RANGE_MASK UINT8_C(0x0F) -#define BMI160_GYRO_BW_MASK UINT8_C(0x30) -#define BMI160_GYRO_ODR_MASK UINT8_C(0x0F) -#define BMI160_GYRO_RANGE_MSK UINT8_C(0x07) - -/** Mask definitions for INT_EN registers */ -#define BMI160_ANY_MOTION_X_INT_EN_MASK UINT8_C(0x01) -#define BMI160_HIGH_G_X_INT_EN_MASK UINT8_C(0x01) -#define BMI160_NO_MOTION_X_INT_EN_MASK UINT8_C(0x01) -#define BMI160_ANY_MOTION_Y_INT_EN_MASK UINT8_C(0x02) -#define BMI160_HIGH_G_Y_INT_EN_MASK UINT8_C(0x02) -#define BMI160_NO_MOTION_Y_INT_EN_MASK UINT8_C(0x02) -#define BMI160_ANY_MOTION_Z_INT_EN_MASK UINT8_C(0x04) -#define BMI160_HIGH_G_Z_INT_EN_MASK UINT8_C(0x04) -#define BMI160_NO_MOTION_Z_INT_EN_MASK UINT8_C(0x04) -#define BMI160_SIG_MOTION_INT_EN_MASK UINT8_C(0x07) -#define BMI160_ANY_MOTION_ALL_INT_EN_MASK UINT8_C(0x07) -#define BMI160_STEP_DETECT_INT_EN_MASK UINT8_C(0x08) -#define BMI160_DOUBLE_TAP_INT_EN_MASK UINT8_C(0x10) -#define BMI160_SINGLE_TAP_INT_EN_MASK UINT8_C(0x20) -#define BMI160_FIFO_FULL_INT_EN_MASK UINT8_C(0x20) -#define BMI160_ORIENT_INT_EN_MASK UINT8_C(0x40) -#define BMI160_FIFO_WATERMARK_INT_EN_MASK UINT8_C(0x40) -#define BMI160_LOW_G_INT_EN_MASK UINT8_C(0x08) -#define BMI160_STEP_DETECT_EN_MASK UINT8_C(0x08) -#define BMI160_FLAT_INT_EN_MASK UINT8_C(0x80) -#define BMI160_DATA_RDY_INT_EN_MASK UINT8_C(0x10) - -/** PMU status Macros */ -#define BMI160_AUX_PMU_SUSPEND UINT8_C(0x00) -#define BMI160_AUX_PMU_NORMAL UINT8_C(0x01) -#define BMI160_AUX_PMU_LOW_POWER UINT8_C(0x02) - -#define BMI160_GYRO_PMU_SUSPEND UINT8_C(0x00) -#define BMI160_GYRO_PMU_NORMAL UINT8_C(0x01) -#define BMI160_GYRO_PMU_FSU UINT8_C(0x03) - -#define BMI160_ACCEL_PMU_SUSPEND UINT8_C(0x00) -#define BMI160_ACCEL_PMU_NORMAL UINT8_C(0x01) -#define BMI160_ACCEL_PMU_LOW_POWER UINT8_C(0x02) - -/** Mask definitions for INT_OUT_CTRL register */ -#define BMI160_INT1_EDGE_CTRL_MASK UINT8_C(0x01) -#define BMI160_INT1_OUTPUT_MODE_MASK UINT8_C(0x04) -#define BMI160_INT1_OUTPUT_TYPE_MASK UINT8_C(0x02) -#define BMI160_INT1_OUTPUT_EN_MASK UINT8_C(0x08) -#define BMI160_INT2_EDGE_CTRL_MASK UINT8_C(0x10) -#define BMI160_INT2_OUTPUT_MODE_MASK UINT8_C(0x40) -#define BMI160_INT2_OUTPUT_TYPE_MASK UINT8_C(0x20) -#define BMI160_INT2_OUTPUT_EN_MASK UINT8_C(0x80) - -/** Mask definitions for INT_LATCH register */ -#define BMI160_INT1_INPUT_EN_MASK UINT8_C(0x10) -#define BMI160_INT2_INPUT_EN_MASK UINT8_C(0x20) -#define BMI160_INT_LATCH_MASK UINT8_C(0x0F) - -/** Mask definitions for INT_MAP register */ -#define BMI160_INT1_LOW_G_MASK UINT8_C(0x01) -#define BMI160_INT1_HIGH_G_MASK UINT8_C(0x02) -#define BMI160_INT1_SLOPE_MASK UINT8_C(0x04) -#define BMI160_INT1_NO_MOTION_MASK UINT8_C(0x08) -#define BMI160_INT1_DOUBLE_TAP_MASK UINT8_C(0x10) -#define BMI160_INT1_SINGLE_TAP_MASK UINT8_C(0x20) -#define BMI160_INT1_FIFO_FULL_MASK UINT8_C(0x20) -#define BMI160_INT1_FIFO_WM_MASK UINT8_C(0x40) -#define BMI160_INT1_ORIENT_MASK UINT8_C(0x40) -#define BMI160_INT1_FLAT_MASK UINT8_C(0x80) -#define BMI160_INT1_DATA_READY_MASK UINT8_C(0x80) -#define BMI160_INT2_LOW_G_MASK UINT8_C(0x01) -#define BMI160_INT1_LOW_STEP_DETECT_MASK UINT8_C(0x01) -#define BMI160_INT2_LOW_STEP_DETECT_MASK UINT8_C(0x01) -#define BMI160_INT2_HIGH_G_MASK UINT8_C(0x02) -#define BMI160_INT2_FIFO_FULL_MASK UINT8_C(0x02) -#define BMI160_INT2_FIFO_WM_MASK UINT8_C(0x04) -#define BMI160_INT2_SLOPE_MASK UINT8_C(0x04) -#define BMI160_INT2_DATA_READY_MASK UINT8_C(0x08) -#define BMI160_INT2_NO_MOTION_MASK UINT8_C(0x08) -#define BMI160_INT2_DOUBLE_TAP_MASK UINT8_C(0x10) -#define BMI160_INT2_SINGLE_TAP_MASK UINT8_C(0x20) -#define BMI160_INT2_ORIENT_MASK UINT8_C(0x40) -#define BMI160_INT2_FLAT_MASK UINT8_C(0x80) - -/** Mask definitions for INT_DATA register */ -#define BMI160_TAP_SRC_INT_MASK UINT8_C(0x08) -#define BMI160_LOW_HIGH_SRC_INT_MASK UINT8_C(0x80) -#define BMI160_MOTION_SRC_INT_MASK UINT8_C(0x80) - -/** Mask definitions for INT_MOTION register */ -#define BMI160_SLOPE_INT_DUR_MASK UINT8_C(0x03) -#define BMI160_NO_MOTION_INT_DUR_MASK UINT8_C(0xFC) -#define BMI160_NO_MOTION_SEL_BIT_MASK UINT8_C(0x01) - -/** Mask definitions for INT_TAP register */ -#define BMI160_TAP_DUR_MASK UINT8_C(0x07) -#define BMI160_TAP_SHOCK_DUR_MASK UINT8_C(0x40) -#define BMI160_TAP_QUIET_DUR_MASK UINT8_C(0x80) -#define BMI160_TAP_THRES_MASK UINT8_C(0x1F) - -/** Mask definitions for INT_FLAT register */ -#define BMI160_FLAT_THRES_MASK UINT8_C(0x3F) -#define BMI160_FLAT_HOLD_TIME_MASK UINT8_C(0x30) -#define BMI160_FLAT_HYST_MASK UINT8_C(0x07) - -/** Mask definitions for INT_LOWHIGH register */ -#define BMI160_LOW_G_HYST_MASK UINT8_C(0x03) -#define BMI160_LOW_G_LOW_MODE_MASK UINT8_C(0x04) -#define BMI160_HIGH_G_HYST_MASK UINT8_C(0xC0) - -/** Mask definitions for INT_SIG_MOTION register */ -#define BMI160_SIG_MOTION_SEL_MASK UINT8_C(0x02) -#define BMI160_SIG_MOTION_SKIP_MASK UINT8_C(0x0C) -#define BMI160_SIG_MOTION_PROOF_MASK UINT8_C(0x30) - -/** Mask definitions for INT_ORIENT register */ -#define BMI160_ORIENT_MODE_MASK UINT8_C(0x03) -#define BMI160_ORIENT_BLOCK_MASK UINT8_C(0x0C) -#define BMI160_ORIENT_HYST_MASK UINT8_C(0xF0) -#define BMI160_ORIENT_THETA_MASK UINT8_C(0x3F) -#define BMI160_ORIENT_UD_ENABLE UINT8_C(0x40) -#define BMI160_AXES_EN_MASK UINT8_C(0x80) - -/** Mask definitions for FIFO_CONFIG register */ -#define BMI160_FIFO_GYRO UINT8_C(0x80) -#define BMI160_FIFO_ACCEL UINT8_C(0x40) -#define BMI160_FIFO_AUX UINT8_C(0x20) -#define BMI160_FIFO_TAG_INT1 UINT8_C(0x08) -#define BMI160_FIFO_TAG_INT2 UINT8_C(0x04) -#define BMI160_FIFO_TIME UINT8_C(0x02) -#define BMI160_FIFO_HEADER UINT8_C(0x10) -#define BMI160_FIFO_CONFIG_1_MASK UINT8_C(0xFE) - -/** Mask definitions for STEP_CONF register */ -#define BMI160_STEP_COUNT_EN_BIT_MASK UINT8_C(0x08) -#define BMI160_STEP_DETECT_MIN_THRES_MASK UINT8_C(0x18) -#define BMI160_STEP_DETECT_STEPTIME_MIN_MASK UINT8_C(0x07) -#define BMI160_STEP_MIN_BUF_MASK UINT8_C(0x07) - -/** Mask definition for FIFO Header Data Tag */ -#define BMI160_FIFO_TAG_INTR_MASK UINT8_C(0xFC) - -/** Fifo byte counter mask definitions */ -#define BMI160_FIFO_BYTE_COUNTER_MASK UINT8_C(0x07) - -/** Enable/disable bit value */ -#define BMI160_ENABLE UINT8_C(0x01) -#define BMI160_DISABLE UINT8_C(0x00) - -/** Latch Duration */ -#define BMI160_LATCH_DUR_NONE UINT8_C(0x00) -#define BMI160_LATCH_DUR_312_5_MICRO_SEC UINT8_C(0x01) -#define BMI160_LATCH_DUR_625_MICRO_SEC UINT8_C(0x02) -#define BMI160_LATCH_DUR_1_25_MILLI_SEC UINT8_C(0x03) -#define BMI160_LATCH_DUR_2_5_MILLI_SEC UINT8_C(0x04) -#define BMI160_LATCH_DUR_5_MILLI_SEC UINT8_C(0x05) -#define BMI160_LATCH_DUR_10_MILLI_SEC UINT8_C(0x06) -#define BMI160_LATCH_DUR_20_MILLI_SEC UINT8_C(0x07) -#define BMI160_LATCH_DUR_40_MILLI_SEC UINT8_C(0x08) -#define BMI160_LATCH_DUR_80_MILLI_SEC UINT8_C(0x09) -#define BMI160_LATCH_DUR_160_MILLI_SEC UINT8_C(0x0A) -#define BMI160_LATCH_DUR_320_MILLI_SEC UINT8_C(0x0B) -#define BMI160_LATCH_DUR_640_MILLI_SEC UINT8_C(0x0C) -#define BMI160_LATCH_DUR_1_28_SEC UINT8_C(0x0D) -#define BMI160_LATCH_DUR_2_56_SEC UINT8_C(0x0E) -#define BMI160_LATCHED UINT8_C(0x0F) - -/** BMI160 Register map */ -#define BMI160_CHIP_ID_ADDR UINT8_C(0x00) -#define BMI160_ERROR_REG_ADDR UINT8_C(0x02) -#define BMI160_PMU_STATUS_ADDR UINT8_C(0x03) -#define BMI160_AUX_DATA_ADDR UINT8_C(0x04) -#define BMI160_GYRO_DATA_ADDR UINT8_C(0x0C) -#define BMI160_ACCEL_DATA_ADDR UINT8_C(0x12) -#define BMI160_STATUS_ADDR UINT8_C(0x1B) -#define BMI160_INT_STATUS_ADDR UINT8_C(0x1C) -#define BMI160_FIFO_LENGTH_ADDR UINT8_C(0x22) -#define BMI160_FIFO_DATA_ADDR UINT8_C(0x24) -#define BMI160_ACCEL_CONFIG_ADDR UINT8_C(0x40) -#define BMI160_ACCEL_RANGE_ADDR UINT8_C(0x41) -#define BMI160_GYRO_CONFIG_ADDR UINT8_C(0x42) -#define BMI160_GYRO_RANGE_ADDR UINT8_C(0x43) -#define BMI160_AUX_ODR_ADDR UINT8_C(0x44) -#define BMI160_FIFO_DOWN_ADDR UINT8_C(0x45) -#define BMI160_FIFO_CONFIG_0_ADDR UINT8_C(0x46) -#define BMI160_FIFO_CONFIG_1_ADDR UINT8_C(0x47) -#define BMI160_AUX_IF_0_ADDR UINT8_C(0x4B) -#define BMI160_AUX_IF_1_ADDR UINT8_C(0x4C) -#define BMI160_AUX_IF_2_ADDR UINT8_C(0x4D) -#define BMI160_AUX_IF_3_ADDR UINT8_C(0x4E) -#define BMI160_AUX_IF_4_ADDR UINT8_C(0x4F) -#define BMI160_INT_ENABLE_0_ADDR UINT8_C(0x50) -#define BMI160_INT_ENABLE_1_ADDR UINT8_C(0x51) -#define BMI160_INT_ENABLE_2_ADDR UINT8_C(0x52) -#define BMI160_INT_OUT_CTRL_ADDR UINT8_C(0x53) -#define BMI160_INT_LATCH_ADDR UINT8_C(0x54) -#define BMI160_INT_MAP_0_ADDR UINT8_C(0x55) -#define BMI160_INT_MAP_1_ADDR UINT8_C(0x56) -#define BMI160_INT_MAP_2_ADDR UINT8_C(0x57) -#define BMI160_INT_DATA_0_ADDR UINT8_C(0x58) -#define BMI160_INT_DATA_1_ADDR UINT8_C(0x59) -#define BMI160_INT_LOWHIGH_0_ADDR UINT8_C(0x5A) -#define BMI160_INT_LOWHIGH_1_ADDR UINT8_C(0x5B) -#define BMI160_INT_LOWHIGH_2_ADDR UINT8_C(0x5C) -#define BMI160_INT_LOWHIGH_3_ADDR UINT8_C(0x5D) -#define BMI160_INT_LOWHIGH_4_ADDR UINT8_C(0x5E) -#define BMI160_INT_MOTION_0_ADDR UINT8_C(0x5F) -#define BMI160_INT_MOTION_1_ADDR UINT8_C(0x60) -#define BMI160_INT_MOTION_2_ADDR UINT8_C(0x61) -#define BMI160_INT_MOTION_3_ADDR UINT8_C(0x62) -#define BMI160_INT_TAP_0_ADDR UINT8_C(0x63) -#define BMI160_INT_TAP_1_ADDR UINT8_C(0x64) -#define BMI160_INT_ORIENT_0_ADDR UINT8_C(0x65) -#define BMI160_INT_ORIENT_1_ADDR UINT8_C(0x66) -#define BMI160_INT_FLAT_0_ADDR UINT8_C(0x67) -#define BMI160_INT_FLAT_1_ADDR UINT8_C(0x68) -#define BMI160_FOC_CONF_ADDR UINT8_C(0x69) -#define BMI160_CONF_ADDR UINT8_C(0x6A) - -#define BMI160_IF_CONF_ADDR UINT8_C(0x6B) -#define BMI160_SELF_TEST_ADDR UINT8_C(0x6D) -#define BMI160_OFFSET_ADDR UINT8_C(0x71) -#define BMI160_OFFSET_CONF_ADDR UINT8_C(0x77) -#define BMI160_INT_STEP_CNT_0_ADDR UINT8_C(0x78) -#define BMI160_INT_STEP_CONFIG_0_ADDR UINT8_C(0x7A) -#define BMI160_INT_STEP_CONFIG_1_ADDR UINT8_C(0x7B) -#define BMI160_COMMAND_REG_ADDR UINT8_C(0x7E) -#define BMI160_SPI_COMM_TEST_ADDR UINT8_C(0x7F) -#define BMI160_INTL_PULLUP_CONF_ADDR UINT8_C(0x85) - -/** Error code definitions */ -#define BMI160_OK INT8_C(0) -#define BMI160_E_NULL_PTR INT8_C(-1) -#define BMI160_E_COM_FAIL INT8_C(-2) -#define BMI160_E_DEV_NOT_FOUND INT8_C(-3) -#define BMI160_E_OUT_OF_RANGE INT8_C(-4) -#define BMI160_E_INVALID_INPUT INT8_C(-5) -#define BMI160_E_ACCEL_ODR_BW_INVALID INT8_C(-6) -#define BMI160_E_GYRO_ODR_BW_INVALID INT8_C(-7) -#define BMI160_E_LWP_PRE_FLTR_INT_INVALID INT8_C(-8) -#define BMI160_E_LWP_PRE_FLTR_INVALID INT8_C(-9) -#define BMI160_E_AUX_NOT_FOUND INT8_C(-10) -#define BMI160_FOC_FAILURE INT8_C(-11) - -/**\name API warning codes */ -#define BMI160_W_GYRO_SELF_TEST_FAIL INT8_C(1) -#define BMI160_W_ACCEl_SELF_TEST_FAIL INT8_C(2) - -/** BMI160 unique chip identifier */ -#define BMI160_CHIP_ID UINT8_C(0xD1) - -/** Soft reset command */ -#define BMI160_SOFT_RESET_CMD UINT8_C(0xb6) -#define BMI160_SOFT_RESET_DELAY_MS UINT8_C(1) - -/** Start FOC command */ -#define BMI160_START_FOC_CMD UINT8_C(0x03) - -/** NVM backup enabling command */ -#define BMI160_NVM_BACKUP_EN UINT8_C(0xA0) - -/* Delay in ms settings */ -#define BMI160_ACCEL_DELAY_MS UINT8_C(5) -#define BMI160_GYRO_DELAY_MS UINT8_C(81) -#define BMI160_ONE_MS_DELAY UINT8_C(1) -#define BMI160_AUX_COM_DELAY UINT8_C(10) -#define BMI160_GYRO_SELF_TEST_DELAY UINT8_C(20) -#define BMI160_ACCEL_SELF_TEST_DELAY UINT8_C(50) - -/** Self test configurations */ -#define BMI160_ACCEL_SELF_TEST_CONFIG UINT8_C(0x2C) -#define BMI160_ACCEL_SELF_TEST_POSITIVE_EN UINT8_C(0x0D) -#define BMI160_ACCEL_SELF_TEST_NEGATIVE_EN UINT8_C(0x09) -#define BMI160_ACCEL_SELF_TEST_LIMIT UINT16_C(8192) - -/** Power mode settings */ -/* Accel power mode */ -#define BMI160_ACCEL_NORMAL_MODE UINT8_C(0x11) -#define BMI160_ACCEL_LOWPOWER_MODE UINT8_C(0x12) -#define BMI160_ACCEL_SUSPEND_MODE UINT8_C(0x10) - -/* Gyro power mode */ -#define BMI160_GYRO_SUSPEND_MODE UINT8_C(0x14) -#define BMI160_GYRO_NORMAL_MODE UINT8_C(0x15) -#define BMI160_GYRO_FASTSTARTUP_MODE UINT8_C(0x17) - -/* Aux power mode */ -#define BMI160_AUX_SUSPEND_MODE UINT8_C(0x18) -#define BMI160_AUX_NORMAL_MODE UINT8_C(0x19) -#define BMI160_AUX_LOWPOWER_MODE UINT8_C(0x1A) - -/** Range settings */ -/* Accel Range */ -#define BMI160_ACCEL_RANGE_2G UINT8_C(0x03) -#define BMI160_ACCEL_RANGE_4G UINT8_C(0x05) -#define BMI160_ACCEL_RANGE_8G UINT8_C(0x08) -#define BMI160_ACCEL_RANGE_16G UINT8_C(0x0C) - -/* Gyro Range */ -#define BMI160_GYRO_RANGE_2000_DPS UINT8_C(0x00) -#define BMI160_GYRO_RANGE_1000_DPS UINT8_C(0x01) -#define BMI160_GYRO_RANGE_500_DPS UINT8_C(0x02) -#define BMI160_GYRO_RANGE_250_DPS UINT8_C(0x03) -#define BMI160_GYRO_RANGE_125_DPS UINT8_C(0x04) - -/** Bandwidth settings */ -/* Accel Bandwidth */ -#define BMI160_ACCEL_BW_OSR4_AVG1 UINT8_C(0x00) -#define BMI160_ACCEL_BW_OSR2_AVG2 UINT8_C(0x01) -#define BMI160_ACCEL_BW_NORMAL_AVG4 UINT8_C(0x02) -#define BMI160_ACCEL_BW_RES_AVG8 UINT8_C(0x03) -#define BMI160_ACCEL_BW_RES_AVG16 UINT8_C(0x04) -#define BMI160_ACCEL_BW_RES_AVG32 UINT8_C(0x05) -#define BMI160_ACCEL_BW_RES_AVG64 UINT8_C(0x06) -#define BMI160_ACCEL_BW_RES_AVG128 UINT8_C(0x07) - -#define BMI160_GYRO_BW_OSR4_MODE UINT8_C(0x00) -#define BMI160_GYRO_BW_OSR2_MODE UINT8_C(0x01) -#define BMI160_GYRO_BW_NORMAL_MODE UINT8_C(0x02) - -/* Output Data Rate settings */ -/* Accel Output data rate */ -#define BMI160_ACCEL_ODR_RESERVED UINT8_C(0x00) -#define BMI160_ACCEL_ODR_0_78HZ UINT8_C(0x01) -#define BMI160_ACCEL_ODR_1_56HZ UINT8_C(0x02) -#define BMI160_ACCEL_ODR_3_12HZ UINT8_C(0x03) -#define BMI160_ACCEL_ODR_6_25HZ UINT8_C(0x04) -#define BMI160_ACCEL_ODR_12_5HZ UINT8_C(0x05) -#define BMI160_ACCEL_ODR_25HZ UINT8_C(0x06) -#define BMI160_ACCEL_ODR_50HZ UINT8_C(0x07) -#define BMI160_ACCEL_ODR_100HZ UINT8_C(0x08) -#define BMI160_ACCEL_ODR_200HZ UINT8_C(0x09) -#define BMI160_ACCEL_ODR_400HZ UINT8_C(0x0A) -#define BMI160_ACCEL_ODR_800HZ UINT8_C(0x0B) -#define BMI160_ACCEL_ODR_1600HZ UINT8_C(0x0C) -#define BMI160_ACCEL_ODR_RESERVED0 UINT8_C(0x0D) -#define BMI160_ACCEL_ODR_RESERVED1 UINT8_C(0x0E) -#define BMI160_ACCEL_ODR_RESERVED2 UINT8_C(0x0F) - -/* Gyro Output data rate */ -#define BMI160_GYRO_ODR_RESERVED UINT8_C(0x00) -#define BMI160_GYRO_ODR_25HZ UINT8_C(0x06) -#define BMI160_GYRO_ODR_50HZ UINT8_C(0x07) -#define BMI160_GYRO_ODR_100HZ UINT8_C(0x08) -#define BMI160_GYRO_ODR_200HZ UINT8_C(0x09) -#define BMI160_GYRO_ODR_400HZ UINT8_C(0x0A) -#define BMI160_GYRO_ODR_800HZ UINT8_C(0x0B) -#define BMI160_GYRO_ODR_1600HZ UINT8_C(0x0C) -#define BMI160_GYRO_ODR_3200HZ UINT8_C(0x0D) - -/* Auxiliary sensor Output data rate */ -#define BMI160_AUX_ODR_RESERVED UINT8_C(0x00) -#define BMI160_AUX_ODR_0_78HZ UINT8_C(0x01) -#define BMI160_AUX_ODR_1_56HZ UINT8_C(0x02) -#define BMI160_AUX_ODR_3_12HZ UINT8_C(0x03) -#define BMI160_AUX_ODR_6_25HZ UINT8_C(0x04) -#define BMI160_AUX_ODR_12_5HZ UINT8_C(0x05) -#define BMI160_AUX_ODR_25HZ UINT8_C(0x06) -#define BMI160_AUX_ODR_50HZ UINT8_C(0x07) -#define BMI160_AUX_ODR_100HZ UINT8_C(0x08) -#define BMI160_AUX_ODR_200HZ UINT8_C(0x09) -#define BMI160_AUX_ODR_400HZ UINT8_C(0x0A) -#define BMI160_AUX_ODR_800HZ UINT8_C(0x0B) - -/* Maximum limits definition */ -#define BMI160_ACCEL_ODR_MAX UINT8_C(15) -#define BMI160_ACCEL_BW_MAX UINT8_C(2) -#define BMI160_ACCEL_RANGE_MAX UINT8_C(12) -#define BMI160_GYRO_ODR_MAX UINT8_C(13) -#define BMI160_GYRO_BW_MAX UINT8_C(2) -#define BMI160_GYRO_RANGE_MAX UINT8_C(4) - -/** FIFO_CONFIG Definitions */ -#define BMI160_FIFO_TIME_ENABLE UINT8_C(0x02) -#define BMI160_FIFO_TAG_INT2_ENABLE UINT8_C(0x04) -#define BMI160_FIFO_TAG_INT1_ENABLE UINT8_C(0x08) -#define BMI160_FIFO_HEAD_ENABLE UINT8_C(0x10) -#define BMI160_FIFO_M_ENABLE UINT8_C(0x20) -#define BMI160_FIFO_A_ENABLE UINT8_C(0x40) -#define BMI160_FIFO_M_A_ENABLE UINT8_C(0x60) -#define BMI160_FIFO_G_ENABLE UINT8_C(0x80) -#define BMI160_FIFO_M_G_ENABLE UINT8_C(0xA0) -#define BMI160_FIFO_G_A_ENABLE UINT8_C(0xC0) -#define BMI160_FIFO_M_G_A_ENABLE UINT8_C(0xE0) - -/* Macro to specify the number of bytes over-read from the - * FIFO in order to get the sensor time at the end of FIFO */ -#ifndef BMI160_FIFO_BYTES_OVERREAD -#define BMI160_FIFO_BYTES_OVERREAD UINT8_C(25) -#endif - -/* Accel, gyro and aux. sensor length and also their combined - * length definitions in FIFO */ -#define BMI160_FIFO_G_LENGTH UINT8_C(6) -#define BMI160_FIFO_A_LENGTH UINT8_C(6) -#define BMI160_FIFO_M_LENGTH UINT8_C(8) -#define BMI160_FIFO_GA_LENGTH UINT8_C(12) -#define BMI160_FIFO_MA_LENGTH UINT8_C(14) -#define BMI160_FIFO_MG_LENGTH UINT8_C(14) -#define BMI160_FIFO_MGA_LENGTH UINT8_C(20) - -/** FIFO Header Data definitions */ -#define BMI160_FIFO_HEAD_SKIP_FRAME UINT8_C(0x40) -#define BMI160_FIFO_HEAD_SENSOR_TIME UINT8_C(0x44) -#define BMI160_FIFO_HEAD_INPUT_CONFIG UINT8_C(0x48) -#define BMI160_FIFO_HEAD_OVER_READ UINT8_C(0x80) -#define BMI160_FIFO_HEAD_A UINT8_C(0x84) -#define BMI160_FIFO_HEAD_G UINT8_C(0x88) -#define BMI160_FIFO_HEAD_G_A UINT8_C(0x8C) -#define BMI160_FIFO_HEAD_M UINT8_C(0x90) -#define BMI160_FIFO_HEAD_M_A UINT8_C(0x94) -#define BMI160_FIFO_HEAD_M_G UINT8_C(0x98) -#define BMI160_FIFO_HEAD_M_G_A UINT8_C(0x9C) - -/** FIFO sensor time length definitions */ -#define BMI160_SENSOR_TIME_LENGTH UINT8_C(3) - -/** FIFO DOWN selection */ -/* Accel fifo down-sampling values*/ -#define BMI160_ACCEL_FIFO_DOWN_ZERO UINT8_C(0x00) -#define BMI160_ACCEL_FIFO_DOWN_ONE UINT8_C(0x10) -#define BMI160_ACCEL_FIFO_DOWN_TWO UINT8_C(0x20) -#define BMI160_ACCEL_FIFO_DOWN_THREE UINT8_C(0x30) -#define BMI160_ACCEL_FIFO_DOWN_FOUR UINT8_C(0x40) -#define BMI160_ACCEL_FIFO_DOWN_FIVE UINT8_C(0x50) -#define BMI160_ACCEL_FIFO_DOWN_SIX UINT8_C(0x60) -#define BMI160_ACCEL_FIFO_DOWN_SEVEN UINT8_C(0x70) - -/* Gyro fifo down-smapling values*/ -#define BMI160_GYRO_FIFO_DOWN_ZERO UINT8_C(0x00) -#define BMI160_GYRO_FIFO_DOWN_ONE UINT8_C(0x01) -#define BMI160_GYRO_FIFO_DOWN_TWO UINT8_C(0x02) -#define BMI160_GYRO_FIFO_DOWN_THREE UINT8_C(0x03) -#define BMI160_GYRO_FIFO_DOWN_FOUR UINT8_C(0x04) -#define BMI160_GYRO_FIFO_DOWN_FIVE UINT8_C(0x05) -#define BMI160_GYRO_FIFO_DOWN_SIX UINT8_C(0x06) -#define BMI160_GYRO_FIFO_DOWN_SEVEN UINT8_C(0x07) - -/* Accel Fifo filter enable*/ -#define BMI160_ACCEL_FIFO_FILT_EN UINT8_C(0x80) - -/* Gyro Fifo filter enable*/ -#define BMI160_GYRO_FIFO_FILT_EN UINT8_C(0x08) - -/** Definitions to check validity of FIFO frames */ -#define FIFO_CONFIG_MSB_CHECK UINT8_C(0x80) -#define FIFO_CONFIG_LSB_CHECK UINT8_C(0x00) - -/*! BMI160 accel FOC configurations */ -#define BMI160_FOC_ACCEL_DISABLED UINT8_C(0x00) -#define BMI160_FOC_ACCEL_POSITIVE_G UINT8_C(0x01) -#define BMI160_FOC_ACCEL_NEGATIVE_G UINT8_C(0x02) -#define BMI160_FOC_ACCEL_0G UINT8_C(0x03) - -/** Array Parameter DefinItions */ -#define BMI160_SENSOR_TIME_LSB_BYTE UINT8_C(0) -#define BMI160_SENSOR_TIME_XLSB_BYTE UINT8_C(1) -#define BMI160_SENSOR_TIME_MSB_BYTE UINT8_C(2) - -/** Interface settings */ -#define BMI160_SPI_INTF UINT8_C(1) -#define BMI160_I2C_INTF UINT8_C(0) -#define BMI160_SPI_RD_MASK UINT8_C(0x80) -#define BMI160_SPI_WR_MASK UINT8_C(0x7F) - -/* Sensor & time select definition*/ -#define BMI160_ACCEL_SEL UINT8_C(0x01) -#define BMI160_GYRO_SEL UINT8_C(0x02) -#define BMI160_TIME_SEL UINT8_C(0x04) - -/* Sensor select mask*/ -#define BMI160_SEN_SEL_MASK UINT8_C(0x07) - -/* Error code mask */ -#define BMI160_ERR_REG_MASK UINT8_C(0x0F) - -/* BMI160 I2C address */ -#define BMI160_I2C_ADDR UINT8_C(0x68) - -/* BMI160 secondary IF address */ -#define BMI160_AUX_BMM150_I2C_ADDR UINT8_C(0x10) - -/** BMI160 Length definitions */ -#define BMI160_ONE UINT8_C(1) -#define BMI160_TWO UINT8_C(2) -#define BMI160_THREE UINT8_C(3) -#define BMI160_FOUR UINT8_C(4) -#define BMI160_FIVE UINT8_C(5) - -/** BMI160 fifo level Margin */ -#define BMI160_FIFO_LEVEL_MARGIN UINT8_C(16) - -/** BMI160 fifo flush Command */ -#define BMI160_FIFO_FLUSH_VALUE UINT8_C(0xB0) - -/** BMI160 offset values for xyz axes of accel */ -#define BMI160_ACCEL_MIN_OFFSET INT8_C(-128) -#define BMI160_ACCEL_MAX_OFFSET INT8_C(127) - -/** BMI160 offset values for xyz axes of gyro */ -#define BMI160_GYRO_MIN_OFFSET INT16_C(-512) -#define BMI160_GYRO_MAX_OFFSET INT16_C(511) - -/** BMI160 fifo full interrupt position and mask */ -#define BMI160_FIFO_FULL_INT_POS UINT8_C(5) -#define BMI160_FIFO_FULL_INT_MSK UINT8_C(0x20) -#define BMI160_FIFO_WTM_INT_POS UINT8_C(6) -#define BMI160_FIFO_WTM_INT_MSK UINT8_C(0x40) - -#define BMI160_FIFO_FULL_INT_PIN1_POS UINT8_C(5) -#define BMI160_FIFO_FULL_INT_PIN1_MSK UINT8_C(0x20) -#define BMI160_FIFO_FULL_INT_PIN2_POS UINT8_C(1) -#define BMI160_FIFO_FULL_INT_PIN2_MSK UINT8_C(0x02) - -#define BMI160_FIFO_WTM_INT_PIN1_POS UINT8_C(6) -#define BMI160_FIFO_WTM_INT_PIN1_MSK UINT8_C(0x40) -#define BMI160_FIFO_WTM_INT_PIN2_POS UINT8_C(2) -#define BMI160_FIFO_WTM_INT_PIN2_MSK UINT8_C(0x04) - -#define BMI160_MANUAL_MODE_EN_POS UINT8_C(7) -#define BMI160_MANUAL_MODE_EN_MSK UINT8_C(0x80) -#define BMI160_AUX_READ_BURST_POS UINT8_C(0) -#define BMI160_AUX_READ_BURST_MSK UINT8_C(0x03) - -#define BMI160_GYRO_SELF_TEST_POS UINT8_C(4) -#define BMI160_GYRO_SELF_TEST_MSK UINT8_C(0x10) -#define BMI160_GYRO_SELF_TEST_STATUS_POS UINT8_C(1) -#define BMI160_GYRO_SELF_TEST_STATUS_MSK UINT8_C(0x02) - -#define BMI160_GYRO_FOC_EN_POS UINT8_C(6) -#define BMI160_GYRO_FOC_EN_MSK UINT8_C(0x40) - -#define BMI160_ACCEL_FOC_X_CONF_POS UINT8_C(4) -#define BMI160_ACCEL_FOC_X_CONF_MSK UINT8_C(0x30) - -#define BMI160_ACCEL_FOC_Y_CONF_POS UINT8_C(2) -#define BMI160_ACCEL_FOC_Y_CONF_MSK UINT8_C(0x0C) - -#define BMI160_ACCEL_FOC_Z_CONF_MSK UINT8_C(0x03) - -#define BMI160_FOC_STATUS_POS UINT8_C(3) -#define BMI160_FOC_STATUS_MSK UINT8_C(0x08) - -#define BMI160_GYRO_OFFSET_X_MSK UINT8_C(0x03) - -#define BMI160_GYRO_OFFSET_Y_POS UINT8_C(2) -#define BMI160_GYRO_OFFSET_Y_MSK UINT8_C(0x0C) - -#define BMI160_GYRO_OFFSET_Z_POS UINT8_C(4) -#define BMI160_GYRO_OFFSET_Z_MSK UINT8_C(0x30) - -#define BMI160_GYRO_OFFSET_EN_POS UINT8_C(7) -#define BMI160_GYRO_OFFSET_EN_MSK UINT8_C(0x80) - -#define BMI160_ACCEL_OFFSET_EN_POS UINT8_C(6) -#define BMI160_ACCEL_OFFSET_EN_MSK UINT8_C(0x40) - -#define BMI160_GYRO_OFFSET_POS UINT16_C(8) -#define BMI160_GYRO_OFFSET_MSK UINT16_C(0x0300) - -#define BMI160_NVM_UPDATE_POS UINT8_C(1) -#define BMI160_NVM_UPDATE_MSK UINT8_C(0x02) - -#define BMI160_NVM_STATUS_POS UINT8_C(4) -#define BMI160_NVM_STATUS_MSK UINT8_C(0x10) - -#define BMI160_MAG_POWER_MODE_MSK UINT8_C(0x03) - -#define BMI160_ACCEL_POWER_MODE_MSK UINT8_C(0x30) -#define BMI160_ACCEL_POWER_MODE_POS UINT8_C(4) - -#define BMI160_GYRO_POWER_MODE_MSK UINT8_C(0x0C) -#define BMI160_GYRO_POWER_MODE_POS UINT8_C(2) - -/* BIT SLICE GET AND SET FUNCTIONS */ -#define BMI160_GET_BITS(regvar, bitname) \ - ((regvar & bitname##_MSK) >> bitname##_POS) -#define BMI160_SET_BITS(regvar, bitname, val) \ - ((regvar & ~bitname##_MSK) | \ - ((val << bitname##_POS) & bitname##_MSK)) - -#define BMI160_SET_BITS_POS_0(reg_data, bitname, data) \ - ((reg_data & ~(bitname##_MSK)) | \ - (data & bitname##_MSK)) - -#define BMI160_GET_BITS_POS_0(reg_data, bitname) (reg_data & (bitname##_MSK)) - -/**\name UTILITY MACROS */ -#define BMI160_SET_LOW_BYTE UINT16_C(0x00FF) -#define BMI160_SET_HIGH_BYTE UINT16_C(0xFF00) - -#define BMI160_GET_LSB(var) (uint8_t)(var & BMI160_SET_LOW_BYTE) -#define BMI160_GET_MSB(var) (uint8_t)((var & BMI160_SET_HIGH_BYTE) >> 8) - -/*****************************************************************************/ -/* type definitions */ -typedef int8_t (*bmi160_com_fptr_t)(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len); -typedef void (*bmi160_delay_fptr_t)(uint32_t period); - -/*************************** Data structures *********************************/ -struct bmi160_pmu_status -{ - /*! Power mode status of Accel - * Possible values : - * - BMI160_ACCEL_PMU_SUSPEND - * - BMI160_ACCEL_PMU_NORMAL - * - BMI160_ACCEL_PMU_LOW_POWER - */ - uint8_t accel_pmu_status; - - /*! Power mode status of Gyro - * Possible values : - * - BMI160_GYRO_PMU_SUSPEND - * - BMI160_GYRO_PMU_NORMAL - * - BMI160_GYRO_PMU_FSU - */ - uint8_t gyro_pmu_status; - - /*! Power mode status of 'Auxiliary sensor interface' whereas the actual - * power mode of the aux. sensor should be configured - * according to the connected sensor specifications - * Possible values : - * - BMI160_AUX_PMU_SUSPEND - * - BMI160_AUX_PMU_NORMAL - * - BMI160_AUX_PMU_LOW_POWER - */ - uint8_t aux_pmu_status; -}; - -/*! - * @brief bmi160 interrupt status selection enum. - */ -enum bmi160_int_status_sel { - BMI160_INT_STATUS_0 = 1, - BMI160_INT_STATUS_1 = 2, - BMI160_INT_STATUS_2 = 4, - BMI160_INT_STATUS_3 = 8, - BMI160_INT_STATUS_ALL = 15 -}; - -/*! - * @brief bmi160 interrupt status bits structure - */ -struct bmi160_int_status_bits -{ -#if LITTLE_ENDIAN == 1 - uint32_t step : 1; - uint32_t sigmot : 1; - uint32_t anym : 1; - - /* pmu trigger will be handled later */ - uint32_t pmu_trigger_reserved : 1; - uint32_t d_tap : 1; - uint32_t s_tap : 1; - uint32_t orient : 1; - uint32_t flat_int : 1; - uint32_t reserved : 2; - uint32_t high_g : 1; - uint32_t low_g : 1; - uint32_t drdy : 1; - uint32_t ffull : 1; - uint32_t fwm : 1; - uint32_t nomo : 1; - uint32_t anym_first_x : 1; - uint32_t anym_first_y : 1; - uint32_t anym_first_z : 1; - uint32_t anym_sign : 1; - uint32_t tap_first_x : 1; - uint32_t tap_first_y : 1; - uint32_t tap_first_z : 1; - uint32_t tap_sign : 1; - uint32_t high_first_x : 1; - uint32_t high_first_y : 1; - uint32_t high_first_z : 1; - uint32_t high_sign : 1; - uint32_t orient_1_0 : 2; - uint32_t orient_2 : 1; - uint32_t flat : 1; -#elif BIG_ENDIAN == 1 - uint32_t high_first_x : 1; - uint32_t high_first_y : 1; - uint32_t high_first_z : 1; - uint32_t high_sign : 1; - uint32_t orient_1_0 : 2; - uint32_t orient_2 : 1; - uint32_t flat : 1; - uint32_t anym_first_x : 1; - uint32_t anym_first_y : 1; - uint32_t anym_first_z : 1; - uint32_t anym_sign : 1; - uint32_t tap_first_x : 1; - uint32_t tap_first_y : 1; - uint32_t tap_first_z : 1; - uint32_t tap_sign : 1; - uint32_t reserved : 2; - uint32_t high_g : 1; - uint32_t low_g : 1; - uint32_t drdy : 1; - uint32_t ffull : 1; - uint32_t fwm : 1; - uint32_t nomo : 1; - uint32_t step : 1; - uint32_t sigmot : 1; - uint32_t anym : 1; - - /* pmu trigger will be handled later */ - uint32_t pmu_trigger_reserved : 1; - uint32_t d_tap : 1; - uint32_t s_tap : 1; - uint32_t orient : 1; - uint32_t flat_int : 1; -#endif -}; - -/*! - * @brief bmi160 interrupt status structure - */ -union bmi160_int_status -{ - uint8_t data[4]; - struct bmi160_int_status_bits bit; -}; - -/*! - * @brief bmi160 sensor data structure which comprises of accel data - */ -struct bmi160_sensor_data -{ - /*! X-axis sensor data */ - int16_t x; - - /*! Y-axis sensor data */ - int16_t y; - - /*! Z-axis sensor data */ - int16_t z; - - /*! sensor time */ - uint32_t sensortime; -}; - -/*! - * @brief bmi160 aux data structure which comprises of 8 bytes of accel data - */ -struct bmi160_aux_data -{ - /*! Auxiliary data */ - uint8_t data[8]; -}; - -/*! - * @brief bmi160 FOC configuration structure - */ -struct bmi160_foc_conf -{ - /*! Enabling FOC in gyro - * Assignable macros : - * - BMI160_ENABLE - * - BMI160_DISABLE - */ - uint8_t foc_gyr_en; - - /*! Accel FOC configurations - * Assignable macros : - * - BMI160_FOC_ACCEL_DISABLED - * - BMI160_FOC_ACCEL_POSITIVE_G - * - BMI160_FOC_ACCEL_NEGATIVE_G - * - BMI160_FOC_ACCEL_0G - */ - uint8_t foc_acc_x; - uint8_t foc_acc_y; - uint8_t foc_acc_z; - - /*! Enabling offset compensation for accel in data registers - * Assignable macros : - * - BMI160_ENABLE - * - BMI160_DISABLE - */ - uint8_t acc_off_en; - - /*! Enabling offset compensation for gyro in data registers - * Assignable macros : - * - BMI160_ENABLE - * - BMI160_DISABLE - */ - uint8_t gyro_off_en; -}; - -/*! - * @brief bmi160 accel gyro offsets - */ -struct bmi160_offsets -{ - /*! Accel offset for x axis */ - int8_t off_acc_x; - - /*! Accel offset for y axis */ - int8_t off_acc_y; - - /*! Accel offset for z axis */ - int8_t off_acc_z; - - /*! Gyro offset for x axis */ - int16_t off_gyro_x; - - /*! Gyro offset for y axis */ - int16_t off_gyro_y; - - /*! Gyro offset for z axis */ - int16_t off_gyro_z; -}; - -/*! - * @brief FIFO aux. sensor data structure - */ -struct bmi160_aux_fifo_data -{ - /*! The value of aux. sensor x LSB data */ - uint8_t aux_x_lsb; - - /*! The value of aux. sensor x MSB data */ - uint8_t aux_x_msb; - - /*! The value of aux. sensor y LSB data */ - uint8_t aux_y_lsb; - - /*! The value of aux. sensor y MSB data */ - uint8_t aux_y_msb; - - /*! The value of aux. sensor z LSB data */ - uint8_t aux_z_lsb; - - /*! The value of aux. sensor z MSB data */ - uint8_t aux_z_msb; - - /*! The value of aux. sensor r for BMM150 LSB data */ - uint8_t aux_r_y2_lsb; - - /*! The value of aux. sensor r for BMM150 MSB data */ - uint8_t aux_r_y2_msb; -}; - -/*! - * @brief bmi160 sensor select structure - */ -enum bmi160_select_sensor { - BMI160_ACCEL_ONLY = 1, - BMI160_GYRO_ONLY, - BMI160_BOTH_ACCEL_AND_GYRO -}; - -/*! - * @brief bmi160 sensor step detector mode structure - */ -enum bmi160_step_detect_mode { - BMI160_STEP_DETECT_NORMAL, - BMI160_STEP_DETECT_SENSITIVE, - BMI160_STEP_DETECT_ROBUST, - - /*! Non recommended User defined setting */ - BMI160_STEP_DETECT_USER_DEFINE -}; - -/*! - * @brief enum for auxiliary burst read selection - */ -enum bm160_aux_read_len { - BMI160_AUX_READ_LEN_0, - BMI160_AUX_READ_LEN_1, - BMI160_AUX_READ_LEN_2, - BMI160_AUX_READ_LEN_3 -}; - -/*! - * @brief bmi160 sensor configuration structure - */ -struct bmi160_cfg -{ - /*! power mode */ - uint8_t power; - - /*! output data rate */ - uint8_t odr; - - /*! range */ - uint8_t range; - - /*! bandwidth */ - uint8_t bw; -}; - -/*! - * @brief Aux sensor configuration structure - */ -struct bmi160_aux_cfg -{ - /*! Aux sensor, 1 - enable 0 - disable */ - uint8_t aux_sensor_enable : 1; - - /*! Aux manual/auto mode status */ - uint8_t manual_enable : 1; - - /*! Aux read burst length */ - uint8_t aux_rd_burst_len : 2; - - /*! output data rate */ - uint8_t aux_odr : 4; - - /*! i2c addr of auxiliary sensor */ - uint8_t aux_i2c_addr; -}; - -/*! - * @brief bmi160 interrupt channel selection structure - */ -enum bmi160_int_channel { - /*! Un-map both channels */ - BMI160_INT_CHANNEL_NONE, - - /*! interrupt Channel 1 */ - BMI160_INT_CHANNEL_1, - - /*! interrupt Channel 2 */ - BMI160_INT_CHANNEL_2, - - /*! Map both channels */ - BMI160_INT_CHANNEL_BOTH -}; -enum bmi160_int_types { - /*! Slope/Any-motion interrupt */ - BMI160_ACC_ANY_MOTION_INT, - - /*! Significant motion interrupt */ - BMI160_ACC_SIG_MOTION_INT, - - /*! Step detector interrupt */ - BMI160_STEP_DETECT_INT, - - /*! double tap interrupt */ - BMI160_ACC_DOUBLE_TAP_INT, - - /*! single tap interrupt */ - BMI160_ACC_SINGLE_TAP_INT, - - /*! orientation interrupt */ - BMI160_ACC_ORIENT_INT, - - /*! flat interrupt */ - BMI160_ACC_FLAT_INT, - - /*! high-g interrupt */ - BMI160_ACC_HIGH_G_INT, - - /*! low-g interrupt */ - BMI160_ACC_LOW_G_INT, - - /*! slow/no-motion interrupt */ - BMI160_ACC_SLOW_NO_MOTION_INT, - - /*! data ready interrupt */ - BMI160_ACC_GYRO_DATA_RDY_INT, - - /*! fifo full interrupt */ - BMI160_ACC_GYRO_FIFO_FULL_INT, - - /*! fifo watermark interrupt */ - BMI160_ACC_GYRO_FIFO_WATERMARK_INT, - - /*! fifo tagging feature support */ - BMI160_FIFO_TAG_INT_PIN -}; - -/*! - * @brief bmi160 active state of any & sig motion interrupt. - */ -enum bmi160_any_sig_motion_active_interrupt_state { - /*! Both any & sig motion are disabled */ - BMI160_BOTH_ANY_SIG_MOTION_DISABLED = -1, - - /*! Any-motion selected */ - BMI160_ANY_MOTION_ENABLED, - - /*! Sig-motion selected */ - BMI160_SIG_MOTION_ENABLED -}; -struct bmi160_acc_tap_int_cfg -{ -#if LITTLE_ENDIAN == 1 - - /*! tap threshold */ - uint16_t tap_thr : 5; - - /*! tap shock */ - uint16_t tap_shock : 1; - - /*! tap quiet */ - uint16_t tap_quiet : 1; - - /*! tap duration */ - uint16_t tap_dur : 3; - - /*! data source 0- filter & 1 pre-filter*/ - uint16_t tap_data_src : 1; - - /*! tap enable, 1 - enable, 0 - disable */ - uint16_t tap_en : 1; -#elif BIG_ENDIAN == 1 - - /*! tap enable, 1 - enable, 0 - disable */ - uint16_t tap_en : 1; - - /*! data source 0- filter & 1 pre-filter*/ - uint16_t tap_data_src : 1; - - /*! tap duration */ - uint16_t tap_dur : 3; - - /*! tap quiet */ - uint16_t tap_quiet : 1; - - /*! tap shock */ - uint16_t tap_shock : 1; - - /*! tap threshold */ - uint16_t tap_thr : 5; -#endif -}; -struct bmi160_acc_any_mot_int_cfg -{ -#if LITTLE_ENDIAN == 1 - - /*! 1 any-motion enable, 0 - any-motion disable */ - uint8_t anymotion_en : 1; - - /*! slope interrupt x, 1 - enable, 0 - disable */ - uint8_t anymotion_x : 1; - - /*! slope interrupt y, 1 - enable, 0 - disable */ - uint8_t anymotion_y : 1; - - /*! slope interrupt z, 1 - enable, 0 - disable */ - uint8_t anymotion_z : 1; - - /*! slope duration */ - uint8_t anymotion_dur : 2; - - /*! data source 0- filter & 1 pre-filter*/ - uint8_t anymotion_data_src : 1; - - /*! slope threshold */ - uint8_t anymotion_thr; -#elif BIG_ENDIAN == 1 - - /*! slope threshold */ - uint8_t anymotion_thr; - - /*! data source 0- filter & 1 pre-filter*/ - uint8_t anymotion_data_src : 1; - - /*! slope duration */ - uint8_t anymotion_dur : 2; - - /*! slope interrupt z, 1 - enable, 0 - disable */ - uint8_t anymotion_z : 1; - - /*! slope interrupt y, 1 - enable, 0 - disable */ - uint8_t anymotion_y : 1; - - /*! slope interrupt x, 1 - enable, 0 - disable */ - uint8_t anymotion_x : 1; - - /*! 1 any-motion enable, 0 - any-motion disable */ - uint8_t anymotion_en : 1; -#endif -}; -struct bmi160_acc_sig_mot_int_cfg -{ -#if LITTLE_ENDIAN == 1 - - /*! skip time of sig-motion interrupt */ - uint8_t sig_mot_skip : 2; - - /*! proof time of sig-motion interrupt */ - uint8_t sig_mot_proof : 2; - - /*! data source 0- filter & 1 pre-filter*/ - uint8_t sig_data_src : 1; - - /*! 1 - enable sig, 0 - disable sig & enable anymotion */ - uint8_t sig_en : 1; - - /*! sig-motion threshold */ - uint8_t sig_mot_thres; -#elif BIG_ENDIAN == 1 - - /*! sig-motion threshold */ - uint8_t sig_mot_thres; - - /*! 1 - enable sig, 0 - disable sig & enable anymotion */ - uint8_t sig_en : 1; - - /*! data source 0- filter & 1 pre-filter*/ - uint8_t sig_data_src : 1; - - /*! proof time of sig-motion interrupt */ - uint8_t sig_mot_proof : 2; - - /*! skip time of sig-motion interrupt */ - uint8_t sig_mot_skip : 2; -#endif -}; -struct bmi160_acc_step_detect_int_cfg -{ -#if LITTLE_ENDIAN == 1 - - /*! 1- step detector enable, 0- step detector disable */ - uint16_t step_detector_en : 1; - - /*! minimum threshold */ - uint16_t min_threshold : 2; - - /*! minimal detectable step time */ - uint16_t steptime_min : 3; - - /*! enable step counter mode setting */ - uint16_t step_detector_mode : 2; - - /*! minimum step buffer size*/ - uint16_t step_min_buf : 3; -#elif BIG_ENDIAN == 1 - - /*! minimum step buffer size*/ - uint16_t step_min_buf : 3; - - /*! enable step counter mode setting */ - uint16_t step_detector_mode : 2; - - /*! minimal detectable step time */ - uint16_t steptime_min : 3; - - /*! minimum threshold */ - uint16_t min_threshold : 2; - - /*! 1- step detector enable, 0- step detector disable */ - uint16_t step_detector_en : 1; -#endif -}; -struct bmi160_acc_no_motion_int_cfg -{ -#if LITTLE_ENDIAN == 1 - - /*! no motion interrupt x */ - uint16_t no_motion_x : 1; - - /*! no motion interrupt y */ - uint16_t no_motion_y : 1; - - /*! no motion interrupt z */ - uint16_t no_motion_z : 1; - - /*! no motion duration */ - uint16_t no_motion_dur : 6; - - /*! no motion sel , 1 - enable no-motion ,0- enable slow-motion */ - uint16_t no_motion_sel : 1; - - /*! data source 0- filter & 1 pre-filter*/ - uint16_t no_motion_src : 1; - - /*! no motion threshold */ - uint8_t no_motion_thres; -#elif BIG_ENDIAN == 1 - - /*! no motion threshold */ - uint8_t no_motion_thres; - - /*! data source 0- filter & 1 pre-filter*/ - uint16_t no_motion_src : 1; - - /*! no motion sel , 1 - enable no-motion ,0- enable slow-motion */ - uint16_t no_motion_sel : 1; - - /*! no motion duration */ - uint16_t no_motion_dur : 6; - - /* no motion interrupt z */ - uint16_t no_motion_z : 1; - - /*! no motion interrupt y */ - uint16_t no_motion_y : 1; - - /*! no motion interrupt x */ - uint16_t no_motion_x : 1; -#endif -}; -struct bmi160_acc_orient_int_cfg -{ -#if LITTLE_ENDIAN == 1 - - /*! thresholds for switching between the different orientations */ - uint16_t orient_mode : 2; - - /*! blocking_mode */ - uint16_t orient_blocking : 2; - - /*! Orientation interrupt hysteresis */ - uint16_t orient_hyst : 4; - - /*! Orientation interrupt theta */ - uint16_t orient_theta : 6; - - /*! Enable/disable Orientation interrupt */ - uint16_t orient_ud_en : 1; - - /*! exchange x- and z-axis in algorithm ,0 - z, 1 - x */ - uint16_t axes_ex : 1; - - /*! 1 - orient enable, 0 - orient disable */ - uint8_t orient_en : 1; -#elif BIG_ENDIAN == 1 - - /*! 1 - orient enable, 0 - orient disable */ - uint8_t orient_en : 1; - - /*! exchange x- and z-axis in algorithm ,0 - z, 1 - x */ - uint16_t axes_ex : 1; - - /*! Enable/disable Orientation interrupt */ - uint16_t orient_ud_en : 1; - - /*! Orientation interrupt theta */ - uint16_t orient_theta : 6; - - /*! Orientation interrupt hysteresis */ - uint16_t orient_hyst : 4; - - /*! blocking_mode */ - uint16_t orient_blocking : 2; - - /*! thresholds for switching between the different orientations */ - uint16_t orient_mode : 2; -#endif -}; -struct bmi160_acc_flat_detect_int_cfg -{ -#if LITTLE_ENDIAN == 1 - - /*! flat threshold */ - uint16_t flat_theta : 6; - - /*! flat interrupt hysteresis */ - uint16_t flat_hy : 3; - - /*! delay time for which the flat value must remain stable for the - * flat interrupt to be generated */ - uint16_t flat_hold_time : 2; - - /*! 1 - flat enable, 0 - flat disable */ - uint16_t flat_en : 1; -#elif BIG_ENDIAN == 1 - - /*! 1 - flat enable, 0 - flat disable */ - uint16_t flat_en : 1; - - /*! delay time for which the flat value must remain stable for the - * flat interrupt to be generated */ - uint16_t flat_hold_time : 2; - - /*! flat interrupt hysteresis */ - uint16_t flat_hy : 3; - - /*! flat threshold */ - uint16_t flat_theta : 6; -#endif -}; -struct bmi160_acc_low_g_int_cfg -{ -#if LITTLE_ENDIAN == 1 - - /*! low-g interrupt trigger delay */ - uint8_t low_dur; - - /*! low-g interrupt trigger threshold */ - uint8_t low_thres; - - /*! hysteresis of low-g interrupt */ - uint8_t low_hyst : 2; - - /*! 0 - single-axis mode ,1 - axis-summing mode */ - uint8_t low_mode : 1; - - /*! data source 0- filter & 1 pre-filter */ - uint8_t low_data_src : 1; - - /*! 1 - enable low-g, 0 - disable low-g */ - uint8_t low_en : 1; -#elif BIG_ENDIAN == 1 - - /*! 1 - enable low-g, 0 - disable low-g */ - uint8_t low_en : 1; - - /*! data source 0- filter & 1 pre-filter */ - uint8_t low_data_src : 1; - - /*! 0 - single-axis mode ,1 - axis-summing mode */ - uint8_t low_mode : 1; - - /*! hysteresis of low-g interrupt */ - uint8_t low_hyst : 2; - - /*! low-g interrupt trigger threshold */ - uint8_t low_thres; - - /*! low-g interrupt trigger delay */ - uint8_t low_dur; -#endif -}; -struct bmi160_acc_high_g_int_cfg -{ -#if LITTLE_ENDIAN == 1 - - /*! High-g interrupt x, 1 - enable, 0 - disable */ - uint8_t high_g_x : 1; - - /*! High-g interrupt y, 1 - enable, 0 - disable */ - uint8_t high_g_y : 1; - - /*! High-g interrupt z, 1 - enable, 0 - disable */ - uint8_t high_g_z : 1; - - /*! High-g hysteresis */ - uint8_t high_hy : 2; - - /*! data source 0- filter & 1 pre-filter */ - uint8_t high_data_src : 1; - - /*! High-g threshold */ - uint8_t high_thres; - - /*! High-g duration */ - uint8_t high_dur; -#elif BIG_ENDIAN == 1 - - /*! High-g duration */ - uint8_t high_dur; - - /*! High-g threshold */ - uint8_t high_thres; - - /*! data source 0- filter & 1 pre-filter */ - uint8_t high_data_src : 1; - - /*! High-g hysteresis */ - uint8_t high_hy : 2; - - /*! High-g interrupt z, 1 - enable, 0 - disable */ - uint8_t high_g_z : 1; - - /*! High-g interrupt y, 1 - enable, 0 - disable */ - uint8_t high_g_y : 1; - - /*! High-g interrupt x, 1 - enable, 0 - disable */ - uint8_t high_g_x : 1; -#endif -}; -struct bmi160_int_pin_settg -{ -#if LITTLE_ENDIAN == 1 - - /*! To enable either INT1 or INT2 pin as output. - * 0- output disabled ,1- output enabled */ - uint16_t output_en : 1; - - /*! 0 - push-pull 1- open drain,only valid if output_en is set 1 */ - uint16_t output_mode : 1; - - /*! 0 - active low , 1 - active high level. - * if output_en is 1,this applies to interrupts,else PMU_trigger */ - uint16_t output_type : 1; - - /*! 0 - level trigger , 1 - edge trigger */ - uint16_t edge_ctrl : 1; - - /*! To enable either INT1 or INT2 pin as input. - * 0 - input disabled ,1 - input enabled */ - uint16_t input_en : 1; - - /*! latch duration*/ - uint16_t latch_dur : 4; -#elif BIG_ENDIAN == 1 - - /*! latch duration*/ - uint16_t latch_dur : 4; - - /*! Latched,non-latched or temporary interrupt modes */ - uint16_t input_en : 1; - - /*! 1 - edge trigger, 0 - level trigger */ - uint16_t edge_ctrl : 1; - - /*! 0 - active low , 1 - active high level. - * if output_en is 1,this applies to interrupts,else PMU_trigger */ - uint16_t output_type : 1; - - /*! 0 - push-pull , 1 - open drain,only valid if output_en is set 1 */ - uint16_t output_mode : 1; - - /*! To enable either INT1 or INT2 pin as output. - * 0 - output disabled , 1 - output enabled */ - uint16_t output_en : 1; -#endif -}; -union bmi160_int_type_cfg -{ - /*! Tap interrupt structure */ - struct bmi160_acc_tap_int_cfg acc_tap_int; - - /*! Slope interrupt structure */ - struct bmi160_acc_any_mot_int_cfg acc_any_motion_int; - - /*! Significant motion interrupt structure */ - struct bmi160_acc_sig_mot_int_cfg acc_sig_motion_int; - - /*! Step detector interrupt structure */ - struct bmi160_acc_step_detect_int_cfg acc_step_detect_int; - - /*! No motion interrupt structure */ - struct bmi160_acc_no_motion_int_cfg acc_no_motion_int; - - /*! Orientation interrupt structure */ - struct bmi160_acc_orient_int_cfg acc_orient_int; - - /*! Flat interrupt structure */ - struct bmi160_acc_flat_detect_int_cfg acc_flat_int; - - /*! Low-g interrupt structure */ - struct bmi160_acc_low_g_int_cfg acc_low_g_int; - - /*! High-g interrupt structure */ - struct bmi160_acc_high_g_int_cfg acc_high_g_int; -}; -struct bmi160_int_settg -{ - /*! Interrupt channel */ - enum bmi160_int_channel int_channel; - - /*! Select Interrupt */ - enum bmi160_int_types int_type; - - /*! Structure configuring Interrupt pins */ - struct bmi160_int_pin_settg int_pin_settg; - - /*! Union configures required interrupt */ - union bmi160_int_type_cfg int_type_cfg; - - /*! FIFO FULL INT 1-enable, 0-disable */ - uint8_t fifo_full_int_en : 1; - - /*! FIFO WTM INT 1-enable, 0-disable */ - uint8_t fifo_WTM_int_en : 1; -}; - -/*! - * @brief This structure holds the information for usage of - * FIFO by the user. - */ -struct bmi160_fifo_frame -{ - /*! Data buffer of user defined length is to be mapped here */ - uint8_t *data; - - /*! While calling the API "bmi160_get_fifo_data" , length stores - * number of bytes in FIFO to be read (specified by user as input) - * and after execution of the API ,number of FIFO data bytes - * available is provided as an output to user - */ - uint16_t length; - - /*! FIFO time enable */ - uint8_t fifo_time_enable; - - /*! Enabling of the FIFO header to stream in header mode */ - uint8_t fifo_header_enable; - - /*! Streaming of the Accelerometer, Gyroscope - * sensor data or both in FIFO */ - uint8_t fifo_data_enable; - - /*! Will be equal to length when no more frames are there to parse */ - uint16_t accel_byte_start_idx; - - /*! Will be equal to length when no more frames are there to parse */ - uint16_t gyro_byte_start_idx; - - /*! Will be equal to length when no more frames are there to parse */ - uint16_t aux_byte_start_idx; - - /*! Value of FIFO sensor time time */ - uint32_t sensor_time; - - /*! Value of Skipped frame counts */ - uint8_t skipped_frame_count; -}; -struct bmi160_dev -{ - /*! Chip Id */ - uint8_t chip_id; - - /*! Device Id */ - uint8_t id; - - /*! 0 - I2C , 1 - SPI Interface */ - uint8_t interface; - - /*! Hold active interrupts status for any and sig motion - * 0 - Any-motion enable, 1 - Sig-motion enable, - * -1 neither any-motion nor sig-motion selected */ - enum bmi160_any_sig_motion_active_interrupt_state any_sig_sel; - - /*! Structure to configure Accel sensor */ - struct bmi160_cfg accel_cfg; - - /*! Structure to hold previous/old accel config parameters. - * This is used at driver level to prevent overwriting of same - * data, hence user does not change it in the code */ - struct bmi160_cfg prev_accel_cfg; - - /*! Structure to configure Gyro sensor */ - struct bmi160_cfg gyro_cfg; - - /*! Structure to hold previous/old gyro config parameters. - * This is used at driver level to prevent overwriting of same - * data, hence user does not change it in the code */ - struct bmi160_cfg prev_gyro_cfg; - - /*! Structure to configure the auxiliary sensor */ - struct bmi160_aux_cfg aux_cfg; - - /*! Structure to hold previous/old aux config parameters. - * This is used at driver level to prevent overwriting of same - * data, hence user does not change it in the code */ - struct bmi160_aux_cfg prev_aux_cfg; - - /*! FIFO related configurations */ - struct bmi160_fifo_frame *fifo; - - /*! Read function pointer */ - bmi160_com_fptr_t read; - - /*! Write function pointer */ - bmi160_com_fptr_t write; - - /*! Delay function pointer */ - bmi160_delay_fptr_t delay_ms; -}; - -#endif /* BMI160_DEFS_H_ */ +/** +* 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 bmi160_defs.h +* @date 10/01/2020 +* @version 3.8.1 +* +*/ + +/*! + * @defgroup bmi160_defs + * @brief + * @{*/ + +#ifndef BMI160_DEFS_H_ +#define BMI160_DEFS_H_ + +/*************************** C types headers *****************************/ +#ifdef __KERNEL__ +#include +#include +#else +#include +#include +#endif + +/*************************** Common macros *****************************/ + +#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 + +/**@}*/ +/**\name C standard macros */ +#ifndef NULL +#ifdef __cplusplus +#define NULL 0 +#else +#define NULL ((void *) 0) +#endif +#endif + +/*************************** Sensor macros *****************************/ +/* Test for an endian machine */ +#ifndef __ORDER_LITTLE_ENDIAN__ +#define __ORDER_LITTLE_ENDIAN__ 0 +#endif + +#ifndef __BYTE_ORDER__ +#define __BYTE_ORDER__ __ORDER_LITTLE_ENDIAN__ +#endif + +#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__ +#ifndef LITTLE_ENDIAN +#define LITTLE_ENDIAN 1 +#endif +#elif __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__ +#ifndef BIG_ENDIAN +#define BIG_ENDIAN 1 +#endif +#else +#error "Code does not support Endian format of the processor" +#endif + +/** Mask definitions */ +#define BMI160_ACCEL_BW_MASK UINT8_C(0x70) +#define BMI160_ACCEL_ODR_MASK UINT8_C(0x0F) +#define BMI160_ACCEL_UNDERSAMPLING_MASK UINT8_C(0x80) +#define BMI160_ACCEL_RANGE_MASK UINT8_C(0x0F) +#define BMI160_GYRO_BW_MASK UINT8_C(0x30) +#define BMI160_GYRO_ODR_MASK UINT8_C(0x0F) +#define BMI160_GYRO_RANGE_MSK UINT8_C(0x07) + +/** Mask definitions for INT_EN registers */ +#define BMI160_ANY_MOTION_X_INT_EN_MASK UINT8_C(0x01) +#define BMI160_HIGH_G_X_INT_EN_MASK UINT8_C(0x01) +#define BMI160_NO_MOTION_X_INT_EN_MASK UINT8_C(0x01) +#define BMI160_ANY_MOTION_Y_INT_EN_MASK UINT8_C(0x02) +#define BMI160_HIGH_G_Y_INT_EN_MASK UINT8_C(0x02) +#define BMI160_NO_MOTION_Y_INT_EN_MASK UINT8_C(0x02) +#define BMI160_ANY_MOTION_Z_INT_EN_MASK UINT8_C(0x04) +#define BMI160_HIGH_G_Z_INT_EN_MASK UINT8_C(0x04) +#define BMI160_NO_MOTION_Z_INT_EN_MASK UINT8_C(0x04) +#define BMI160_SIG_MOTION_INT_EN_MASK UINT8_C(0x07) +#define BMI160_ANY_MOTION_ALL_INT_EN_MASK UINT8_C(0x07) +#define BMI160_STEP_DETECT_INT_EN_MASK UINT8_C(0x08) +#define BMI160_DOUBLE_TAP_INT_EN_MASK UINT8_C(0x10) +#define BMI160_SINGLE_TAP_INT_EN_MASK UINT8_C(0x20) +#define BMI160_FIFO_FULL_INT_EN_MASK UINT8_C(0x20) +#define BMI160_ORIENT_INT_EN_MASK UINT8_C(0x40) +#define BMI160_FIFO_WATERMARK_INT_EN_MASK UINT8_C(0x40) +#define BMI160_LOW_G_INT_EN_MASK UINT8_C(0x08) +#define BMI160_STEP_DETECT_EN_MASK UINT8_C(0x08) +#define BMI160_FLAT_INT_EN_MASK UINT8_C(0x80) +#define BMI160_DATA_RDY_INT_EN_MASK UINT8_C(0x10) + +/** PMU status Macros */ +#define BMI160_AUX_PMU_SUSPEND UINT8_C(0x00) +#define BMI160_AUX_PMU_NORMAL UINT8_C(0x01) +#define BMI160_AUX_PMU_LOW_POWER UINT8_C(0x02) + +#define BMI160_GYRO_PMU_SUSPEND UINT8_C(0x00) +#define BMI160_GYRO_PMU_NORMAL UINT8_C(0x01) +#define BMI160_GYRO_PMU_FSU UINT8_C(0x03) + +#define BMI160_ACCEL_PMU_SUSPEND UINT8_C(0x00) +#define BMI160_ACCEL_PMU_NORMAL UINT8_C(0x01) +#define BMI160_ACCEL_PMU_LOW_POWER UINT8_C(0x02) + +/** Mask definitions for INT_OUT_CTRL register */ +#define BMI160_INT1_EDGE_CTRL_MASK UINT8_C(0x01) +#define BMI160_INT1_OUTPUT_MODE_MASK UINT8_C(0x04) +#define BMI160_INT1_OUTPUT_TYPE_MASK UINT8_C(0x02) +#define BMI160_INT1_OUTPUT_EN_MASK UINT8_C(0x08) +#define BMI160_INT2_EDGE_CTRL_MASK UINT8_C(0x10) +#define BMI160_INT2_OUTPUT_MODE_MASK UINT8_C(0x40) +#define BMI160_INT2_OUTPUT_TYPE_MASK UINT8_C(0x20) +#define BMI160_INT2_OUTPUT_EN_MASK UINT8_C(0x80) + +/** Mask definitions for INT_LATCH register */ +#define BMI160_INT1_INPUT_EN_MASK UINT8_C(0x10) +#define BMI160_INT2_INPUT_EN_MASK UINT8_C(0x20) +#define BMI160_INT_LATCH_MASK UINT8_C(0x0F) + +/** Mask definitions for INT_MAP register */ +#define BMI160_INT1_LOW_G_MASK UINT8_C(0x01) +#define BMI160_INT1_HIGH_G_MASK UINT8_C(0x02) +#define BMI160_INT1_SLOPE_MASK UINT8_C(0x04) +#define BMI160_INT1_NO_MOTION_MASK UINT8_C(0x08) +#define BMI160_INT1_DOUBLE_TAP_MASK UINT8_C(0x10) +#define BMI160_INT1_SINGLE_TAP_MASK UINT8_C(0x20) +#define BMI160_INT1_FIFO_FULL_MASK UINT8_C(0x20) +#define BMI160_INT1_FIFO_WM_MASK UINT8_C(0x40) +#define BMI160_INT1_ORIENT_MASK UINT8_C(0x40) +#define BMI160_INT1_FLAT_MASK UINT8_C(0x80) +#define BMI160_INT1_DATA_READY_MASK UINT8_C(0x80) +#define BMI160_INT2_LOW_G_MASK UINT8_C(0x01) +#define BMI160_INT1_LOW_STEP_DETECT_MASK UINT8_C(0x01) +#define BMI160_INT2_LOW_STEP_DETECT_MASK UINT8_C(0x01) +#define BMI160_INT2_HIGH_G_MASK UINT8_C(0x02) +#define BMI160_INT2_FIFO_FULL_MASK UINT8_C(0x02) +#define BMI160_INT2_FIFO_WM_MASK UINT8_C(0x04) +#define BMI160_INT2_SLOPE_MASK UINT8_C(0x04) +#define BMI160_INT2_DATA_READY_MASK UINT8_C(0x08) +#define BMI160_INT2_NO_MOTION_MASK UINT8_C(0x08) +#define BMI160_INT2_DOUBLE_TAP_MASK UINT8_C(0x10) +#define BMI160_INT2_SINGLE_TAP_MASK UINT8_C(0x20) +#define BMI160_INT2_ORIENT_MASK UINT8_C(0x40) +#define BMI160_INT2_FLAT_MASK UINT8_C(0x80) + +/** Mask definitions for INT_DATA register */ +#define BMI160_TAP_SRC_INT_MASK UINT8_C(0x08) +#define BMI160_LOW_HIGH_SRC_INT_MASK UINT8_C(0x80) +#define BMI160_MOTION_SRC_INT_MASK UINT8_C(0x80) + +/** Mask definitions for INT_MOTION register */ +#define BMI160_SLOPE_INT_DUR_MASK UINT8_C(0x03) +#define BMI160_NO_MOTION_INT_DUR_MASK UINT8_C(0xFC) +#define BMI160_NO_MOTION_SEL_BIT_MASK UINT8_C(0x01) + +/** Mask definitions for INT_TAP register */ +#define BMI160_TAP_DUR_MASK UINT8_C(0x07) +#define BMI160_TAP_SHOCK_DUR_MASK UINT8_C(0x40) +#define BMI160_TAP_QUIET_DUR_MASK UINT8_C(0x80) +#define BMI160_TAP_THRES_MASK UINT8_C(0x1F) + +/** Mask definitions for INT_FLAT register */ +#define BMI160_FLAT_THRES_MASK UINT8_C(0x3F) +#define BMI160_FLAT_HOLD_TIME_MASK UINT8_C(0x30) +#define BMI160_FLAT_HYST_MASK UINT8_C(0x07) + +/** Mask definitions for INT_LOWHIGH register */ +#define BMI160_LOW_G_HYST_MASK UINT8_C(0x03) +#define BMI160_LOW_G_LOW_MODE_MASK UINT8_C(0x04) +#define BMI160_HIGH_G_HYST_MASK UINT8_C(0xC0) + +/** Mask definitions for INT_SIG_MOTION register */ +#define BMI160_SIG_MOTION_SEL_MASK UINT8_C(0x02) +#define BMI160_SIG_MOTION_SKIP_MASK UINT8_C(0x0C) +#define BMI160_SIG_MOTION_PROOF_MASK UINT8_C(0x30) + +/** Mask definitions for INT_ORIENT register */ +#define BMI160_ORIENT_MODE_MASK UINT8_C(0x03) +#define BMI160_ORIENT_BLOCK_MASK UINT8_C(0x0C) +#define BMI160_ORIENT_HYST_MASK UINT8_C(0xF0) +#define BMI160_ORIENT_THETA_MASK UINT8_C(0x3F) +#define BMI160_ORIENT_UD_ENABLE UINT8_C(0x40) +#define BMI160_AXES_EN_MASK UINT8_C(0x80) + +/** Mask definitions for FIFO_CONFIG register */ +#define BMI160_FIFO_GYRO UINT8_C(0x80) +#define BMI160_FIFO_ACCEL UINT8_C(0x40) +#define BMI160_FIFO_AUX UINT8_C(0x20) +#define BMI160_FIFO_TAG_INT1 UINT8_C(0x08) +#define BMI160_FIFO_TAG_INT2 UINT8_C(0x04) +#define BMI160_FIFO_TIME UINT8_C(0x02) +#define BMI160_FIFO_HEADER UINT8_C(0x10) +#define BMI160_FIFO_CONFIG_1_MASK UINT8_C(0xFE) + +/** Mask definitions for STEP_CONF register */ +#define BMI160_STEP_COUNT_EN_BIT_MASK UINT8_C(0x08) +#define BMI160_STEP_DETECT_MIN_THRES_MASK UINT8_C(0x18) +#define BMI160_STEP_DETECT_STEPTIME_MIN_MASK UINT8_C(0x07) +#define BMI160_STEP_MIN_BUF_MASK UINT8_C(0x07) + +/** Mask definition for FIFO Header Data Tag */ +#define BMI160_FIFO_TAG_INTR_MASK UINT8_C(0xFC) + +/** Fifo byte counter mask definitions */ +#define BMI160_FIFO_BYTE_COUNTER_MASK UINT8_C(0x07) + +/** Enable/disable bit value */ +#define BMI160_ENABLE UINT8_C(0x01) +#define BMI160_DISABLE UINT8_C(0x00) + +/** Latch Duration */ +#define BMI160_LATCH_DUR_NONE UINT8_C(0x00) +#define BMI160_LATCH_DUR_312_5_MICRO_SEC UINT8_C(0x01) +#define BMI160_LATCH_DUR_625_MICRO_SEC UINT8_C(0x02) +#define BMI160_LATCH_DUR_1_25_MILLI_SEC UINT8_C(0x03) +#define BMI160_LATCH_DUR_2_5_MILLI_SEC UINT8_C(0x04) +#define BMI160_LATCH_DUR_5_MILLI_SEC UINT8_C(0x05) +#define BMI160_LATCH_DUR_10_MILLI_SEC UINT8_C(0x06) +#define BMI160_LATCH_DUR_20_MILLI_SEC UINT8_C(0x07) +#define BMI160_LATCH_DUR_40_MILLI_SEC UINT8_C(0x08) +#define BMI160_LATCH_DUR_80_MILLI_SEC UINT8_C(0x09) +#define BMI160_LATCH_DUR_160_MILLI_SEC UINT8_C(0x0A) +#define BMI160_LATCH_DUR_320_MILLI_SEC UINT8_C(0x0B) +#define BMI160_LATCH_DUR_640_MILLI_SEC UINT8_C(0x0C) +#define BMI160_LATCH_DUR_1_28_SEC UINT8_C(0x0D) +#define BMI160_LATCH_DUR_2_56_SEC UINT8_C(0x0E) +#define BMI160_LATCHED UINT8_C(0x0F) + +/** BMI160 Register map */ +#define BMI160_CHIP_ID_ADDR UINT8_C(0x00) +#define BMI160_ERROR_REG_ADDR UINT8_C(0x02) +#define BMI160_PMU_STATUS_ADDR UINT8_C(0x03) +#define BMI160_AUX_DATA_ADDR UINT8_C(0x04) +#define BMI160_GYRO_DATA_ADDR UINT8_C(0x0C) +#define BMI160_ACCEL_DATA_ADDR UINT8_C(0x12) +#define BMI160_STATUS_ADDR UINT8_C(0x1B) +#define BMI160_INT_STATUS_ADDR UINT8_C(0x1C) +#define BMI160_FIFO_LENGTH_ADDR UINT8_C(0x22) +#define BMI160_FIFO_DATA_ADDR UINT8_C(0x24) +#define BMI160_ACCEL_CONFIG_ADDR UINT8_C(0x40) +#define BMI160_ACCEL_RANGE_ADDR UINT8_C(0x41) +#define BMI160_GYRO_CONFIG_ADDR UINT8_C(0x42) +#define BMI160_GYRO_RANGE_ADDR UINT8_C(0x43) +#define BMI160_AUX_ODR_ADDR UINT8_C(0x44) +#define BMI160_FIFO_DOWN_ADDR UINT8_C(0x45) +#define BMI160_FIFO_CONFIG_0_ADDR UINT8_C(0x46) +#define BMI160_FIFO_CONFIG_1_ADDR UINT8_C(0x47) +#define BMI160_AUX_IF_0_ADDR UINT8_C(0x4B) +#define BMI160_AUX_IF_1_ADDR UINT8_C(0x4C) +#define BMI160_AUX_IF_2_ADDR UINT8_C(0x4D) +#define BMI160_AUX_IF_3_ADDR UINT8_C(0x4E) +#define BMI160_AUX_IF_4_ADDR UINT8_C(0x4F) +#define BMI160_INT_ENABLE_0_ADDR UINT8_C(0x50) +#define BMI160_INT_ENABLE_1_ADDR UINT8_C(0x51) +#define BMI160_INT_ENABLE_2_ADDR UINT8_C(0x52) +#define BMI160_INT_OUT_CTRL_ADDR UINT8_C(0x53) +#define BMI160_INT_LATCH_ADDR UINT8_C(0x54) +#define BMI160_INT_MAP_0_ADDR UINT8_C(0x55) +#define BMI160_INT_MAP_1_ADDR UINT8_C(0x56) +#define BMI160_INT_MAP_2_ADDR UINT8_C(0x57) +#define BMI160_INT_DATA_0_ADDR UINT8_C(0x58) +#define BMI160_INT_DATA_1_ADDR UINT8_C(0x59) +#define BMI160_INT_LOWHIGH_0_ADDR UINT8_C(0x5A) +#define BMI160_INT_LOWHIGH_1_ADDR UINT8_C(0x5B) +#define BMI160_INT_LOWHIGH_2_ADDR UINT8_C(0x5C) +#define BMI160_INT_LOWHIGH_3_ADDR UINT8_C(0x5D) +#define BMI160_INT_LOWHIGH_4_ADDR UINT8_C(0x5E) +#define BMI160_INT_MOTION_0_ADDR UINT8_C(0x5F) +#define BMI160_INT_MOTION_1_ADDR UINT8_C(0x60) +#define BMI160_INT_MOTION_2_ADDR UINT8_C(0x61) +#define BMI160_INT_MOTION_3_ADDR UINT8_C(0x62) +#define BMI160_INT_TAP_0_ADDR UINT8_C(0x63) +#define BMI160_INT_TAP_1_ADDR UINT8_C(0x64) +#define BMI160_INT_ORIENT_0_ADDR UINT8_C(0x65) +#define BMI160_INT_ORIENT_1_ADDR UINT8_C(0x66) +#define BMI160_INT_FLAT_0_ADDR UINT8_C(0x67) +#define BMI160_INT_FLAT_1_ADDR UINT8_C(0x68) +#define BMI160_FOC_CONF_ADDR UINT8_C(0x69) +#define BMI160_CONF_ADDR UINT8_C(0x6A) + +#define BMI160_IF_CONF_ADDR UINT8_C(0x6B) +#define BMI160_SELF_TEST_ADDR UINT8_C(0x6D) +#define BMI160_OFFSET_ADDR UINT8_C(0x71) +#define BMI160_OFFSET_CONF_ADDR UINT8_C(0x77) +#define BMI160_INT_STEP_CNT_0_ADDR UINT8_C(0x78) +#define BMI160_INT_STEP_CONFIG_0_ADDR UINT8_C(0x7A) +#define BMI160_INT_STEP_CONFIG_1_ADDR UINT8_C(0x7B) +#define BMI160_COMMAND_REG_ADDR UINT8_C(0x7E) +#define BMI160_SPI_COMM_TEST_ADDR UINT8_C(0x7F) +#define BMI160_INTL_PULLUP_CONF_ADDR UINT8_C(0x85) + +/** Error code definitions */ +#define BMI160_OK INT8_C(0) +#define BMI160_E_NULL_PTR INT8_C(-1) +#define BMI160_E_COM_FAIL INT8_C(-2) +#define BMI160_E_DEV_NOT_FOUND INT8_C(-3) +#define BMI160_E_OUT_OF_RANGE INT8_C(-4) +#define BMI160_E_INVALID_INPUT INT8_C(-5) +#define BMI160_E_ACCEL_ODR_BW_INVALID INT8_C(-6) +#define BMI160_E_GYRO_ODR_BW_INVALID INT8_C(-7) +#define BMI160_E_LWP_PRE_FLTR_INT_INVALID INT8_C(-8) +#define BMI160_E_LWP_PRE_FLTR_INVALID INT8_C(-9) +#define BMI160_E_AUX_NOT_FOUND INT8_C(-10) +#define BMI160_FOC_FAILURE INT8_C(-11) +#define BMI160_READ_WRITE_LENGHT_INVALID INT8_C(-12) + +/**\name API warning codes */ +#define BMI160_W_GYRO_SELF_TEST_FAIL INT8_C(1) +#define BMI160_W_ACCEl_SELF_TEST_FAIL INT8_C(2) + +/** BMI160 unique chip identifier */ +#define BMI160_CHIP_ID UINT8_C(0xD1) + +/** Soft reset command */ +#define BMI160_SOFT_RESET_CMD UINT8_C(0xb6) +#define BMI160_SOFT_RESET_DELAY_MS UINT8_C(1) + +/** Start FOC command */ +#define BMI160_START_FOC_CMD UINT8_C(0x03) + +/** NVM backup enabling command */ +#define BMI160_NVM_BACKUP_EN UINT8_C(0xA0) + +/* Delay in ms settings */ +#define BMI160_ACCEL_DELAY_MS UINT8_C(5) +#define BMI160_GYRO_DELAY_MS UINT8_C(81) +#define BMI160_ONE_MS_DELAY UINT8_C(1) +#define BMI160_AUX_COM_DELAY UINT8_C(10) +#define BMI160_GYRO_SELF_TEST_DELAY UINT8_C(20) +#define BMI160_ACCEL_SELF_TEST_DELAY UINT8_C(50) + +/** Self test configurations */ +#define BMI160_ACCEL_SELF_TEST_CONFIG UINT8_C(0x2C) +#define BMI160_ACCEL_SELF_TEST_POSITIVE_EN UINT8_C(0x0D) +#define BMI160_ACCEL_SELF_TEST_NEGATIVE_EN UINT8_C(0x09) +#define BMI160_ACCEL_SELF_TEST_LIMIT UINT16_C(8192) + +/** Power mode settings */ +/* Accel power mode */ +#define BMI160_ACCEL_NORMAL_MODE UINT8_C(0x11) +#define BMI160_ACCEL_LOWPOWER_MODE UINT8_C(0x12) +#define BMI160_ACCEL_SUSPEND_MODE UINT8_C(0x10) + +/* Gyro power mode */ +#define BMI160_GYRO_SUSPEND_MODE UINT8_C(0x14) +#define BMI160_GYRO_NORMAL_MODE UINT8_C(0x15) +#define BMI160_GYRO_FASTSTARTUP_MODE UINT8_C(0x17) + +/* Aux power mode */ +#define BMI160_AUX_SUSPEND_MODE UINT8_C(0x18) +#define BMI160_AUX_NORMAL_MODE UINT8_C(0x19) +#define BMI160_AUX_LOWPOWER_MODE UINT8_C(0x1A) + +/** Range settings */ +/* Accel Range */ +#define BMI160_ACCEL_RANGE_2G UINT8_C(0x03) +#define BMI160_ACCEL_RANGE_4G UINT8_C(0x05) +#define BMI160_ACCEL_RANGE_8G UINT8_C(0x08) +#define BMI160_ACCEL_RANGE_16G UINT8_C(0x0C) + +/* Gyro Range */ +#define BMI160_GYRO_RANGE_2000_DPS UINT8_C(0x00) +#define BMI160_GYRO_RANGE_1000_DPS UINT8_C(0x01) +#define BMI160_GYRO_RANGE_500_DPS UINT8_C(0x02) +#define BMI160_GYRO_RANGE_250_DPS UINT8_C(0x03) +#define BMI160_GYRO_RANGE_125_DPS UINT8_C(0x04) + +/** Bandwidth settings */ +/* Accel Bandwidth */ +#define BMI160_ACCEL_BW_OSR4_AVG1 UINT8_C(0x00) +#define BMI160_ACCEL_BW_OSR2_AVG2 UINT8_C(0x01) +#define BMI160_ACCEL_BW_NORMAL_AVG4 UINT8_C(0x02) +#define BMI160_ACCEL_BW_RES_AVG8 UINT8_C(0x03) +#define BMI160_ACCEL_BW_RES_AVG16 UINT8_C(0x04) +#define BMI160_ACCEL_BW_RES_AVG32 UINT8_C(0x05) +#define BMI160_ACCEL_BW_RES_AVG64 UINT8_C(0x06) +#define BMI160_ACCEL_BW_RES_AVG128 UINT8_C(0x07) + +#define BMI160_GYRO_BW_OSR4_MODE UINT8_C(0x00) +#define BMI160_GYRO_BW_OSR2_MODE UINT8_C(0x01) +#define BMI160_GYRO_BW_NORMAL_MODE UINT8_C(0x02) + +/* Output Data Rate settings */ +/* Accel Output data rate */ +#define BMI160_ACCEL_ODR_RESERVED UINT8_C(0x00) +#define BMI160_ACCEL_ODR_0_78HZ UINT8_C(0x01) +#define BMI160_ACCEL_ODR_1_56HZ UINT8_C(0x02) +#define BMI160_ACCEL_ODR_3_12HZ UINT8_C(0x03) +#define BMI160_ACCEL_ODR_6_25HZ UINT8_C(0x04) +#define BMI160_ACCEL_ODR_12_5HZ UINT8_C(0x05) +#define BMI160_ACCEL_ODR_25HZ UINT8_C(0x06) +#define BMI160_ACCEL_ODR_50HZ UINT8_C(0x07) +#define BMI160_ACCEL_ODR_100HZ UINT8_C(0x08) +#define BMI160_ACCEL_ODR_200HZ UINT8_C(0x09) +#define BMI160_ACCEL_ODR_400HZ UINT8_C(0x0A) +#define BMI160_ACCEL_ODR_800HZ UINT8_C(0x0B) +#define BMI160_ACCEL_ODR_1600HZ UINT8_C(0x0C) +#define BMI160_ACCEL_ODR_RESERVED0 UINT8_C(0x0D) +#define BMI160_ACCEL_ODR_RESERVED1 UINT8_C(0x0E) +#define BMI160_ACCEL_ODR_RESERVED2 UINT8_C(0x0F) + +/* Gyro Output data rate */ +#define BMI160_GYRO_ODR_RESERVED UINT8_C(0x00) +#define BMI160_GYRO_ODR_25HZ UINT8_C(0x06) +#define BMI160_GYRO_ODR_50HZ UINT8_C(0x07) +#define BMI160_GYRO_ODR_100HZ UINT8_C(0x08) +#define BMI160_GYRO_ODR_200HZ UINT8_C(0x09) +#define BMI160_GYRO_ODR_400HZ UINT8_C(0x0A) +#define BMI160_GYRO_ODR_800HZ UINT8_C(0x0B) +#define BMI160_GYRO_ODR_1600HZ UINT8_C(0x0C) +#define BMI160_GYRO_ODR_3200HZ UINT8_C(0x0D) + +/* Auxiliary sensor Output data rate */ +#define BMI160_AUX_ODR_RESERVED UINT8_C(0x00) +#define BMI160_AUX_ODR_0_78HZ UINT8_C(0x01) +#define BMI160_AUX_ODR_1_56HZ UINT8_C(0x02) +#define BMI160_AUX_ODR_3_12HZ UINT8_C(0x03) +#define BMI160_AUX_ODR_6_25HZ UINT8_C(0x04) +#define BMI160_AUX_ODR_12_5HZ UINT8_C(0x05) +#define BMI160_AUX_ODR_25HZ UINT8_C(0x06) +#define BMI160_AUX_ODR_50HZ UINT8_C(0x07) +#define BMI160_AUX_ODR_100HZ UINT8_C(0x08) +#define BMI160_AUX_ODR_200HZ UINT8_C(0x09) +#define BMI160_AUX_ODR_400HZ UINT8_C(0x0A) +#define BMI160_AUX_ODR_800HZ UINT8_C(0x0B) + +/* Maximum limits definition */ +#define BMI160_ACCEL_ODR_MAX UINT8_C(15) +#define BMI160_ACCEL_BW_MAX UINT8_C(2) +#define BMI160_ACCEL_RANGE_MAX UINT8_C(12) +#define BMI160_GYRO_ODR_MAX UINT8_C(13) +#define BMI160_GYRO_BW_MAX UINT8_C(2) +#define BMI160_GYRO_RANGE_MAX UINT8_C(4) + +/** FIFO_CONFIG Definitions */ +#define BMI160_FIFO_TIME_ENABLE UINT8_C(0x02) +#define BMI160_FIFO_TAG_INT2_ENABLE UINT8_C(0x04) +#define BMI160_FIFO_TAG_INT1_ENABLE UINT8_C(0x08) +#define BMI160_FIFO_HEAD_ENABLE UINT8_C(0x10) +#define BMI160_FIFO_M_ENABLE UINT8_C(0x20) +#define BMI160_FIFO_A_ENABLE UINT8_C(0x40) +#define BMI160_FIFO_M_A_ENABLE UINT8_C(0x60) +#define BMI160_FIFO_G_ENABLE UINT8_C(0x80) +#define BMI160_FIFO_M_G_ENABLE UINT8_C(0xA0) +#define BMI160_FIFO_G_A_ENABLE UINT8_C(0xC0) +#define BMI160_FIFO_M_G_A_ENABLE UINT8_C(0xE0) + +/* Macro to specify the number of bytes over-read from the + * FIFO in order to get the sensor time at the end of FIFO */ +#ifndef BMI160_FIFO_BYTES_OVERREAD +#define BMI160_FIFO_BYTES_OVERREAD UINT8_C(25) +#endif + +/* Accel, gyro and aux. sensor length and also their combined + * length definitions in FIFO */ +#define BMI160_FIFO_G_LENGTH UINT8_C(6) +#define BMI160_FIFO_A_LENGTH UINT8_C(6) +#define BMI160_FIFO_M_LENGTH UINT8_C(8) +#define BMI160_FIFO_GA_LENGTH UINT8_C(12) +#define BMI160_FIFO_MA_LENGTH UINT8_C(14) +#define BMI160_FIFO_MG_LENGTH UINT8_C(14) +#define BMI160_FIFO_MGA_LENGTH UINT8_C(20) + +/** FIFO Header Data definitions */ +#define BMI160_FIFO_HEAD_SKIP_FRAME UINT8_C(0x40) +#define BMI160_FIFO_HEAD_SENSOR_TIME UINT8_C(0x44) +#define BMI160_FIFO_HEAD_INPUT_CONFIG UINT8_C(0x48) +#define BMI160_FIFO_HEAD_OVER_READ UINT8_C(0x80) +#define BMI160_FIFO_HEAD_A UINT8_C(0x84) +#define BMI160_FIFO_HEAD_G UINT8_C(0x88) +#define BMI160_FIFO_HEAD_G_A UINT8_C(0x8C) +#define BMI160_FIFO_HEAD_M UINT8_C(0x90) +#define BMI160_FIFO_HEAD_M_A UINT8_C(0x94) +#define BMI160_FIFO_HEAD_M_G UINT8_C(0x98) +#define BMI160_FIFO_HEAD_M_G_A UINT8_C(0x9C) + +/** FIFO sensor time length definitions */ +#define BMI160_SENSOR_TIME_LENGTH UINT8_C(3) + +/** FIFO DOWN selection */ +/* Accel fifo down-sampling values*/ +#define BMI160_ACCEL_FIFO_DOWN_ZERO UINT8_C(0x00) +#define BMI160_ACCEL_FIFO_DOWN_ONE UINT8_C(0x10) +#define BMI160_ACCEL_FIFO_DOWN_TWO UINT8_C(0x20) +#define BMI160_ACCEL_FIFO_DOWN_THREE UINT8_C(0x30) +#define BMI160_ACCEL_FIFO_DOWN_FOUR UINT8_C(0x40) +#define BMI160_ACCEL_FIFO_DOWN_FIVE UINT8_C(0x50) +#define BMI160_ACCEL_FIFO_DOWN_SIX UINT8_C(0x60) +#define BMI160_ACCEL_FIFO_DOWN_SEVEN UINT8_C(0x70) + +/* Gyro fifo down-smapling values*/ +#define BMI160_GYRO_FIFO_DOWN_ZERO UINT8_C(0x00) +#define BMI160_GYRO_FIFO_DOWN_ONE UINT8_C(0x01) +#define BMI160_GYRO_FIFO_DOWN_TWO UINT8_C(0x02) +#define BMI160_GYRO_FIFO_DOWN_THREE UINT8_C(0x03) +#define BMI160_GYRO_FIFO_DOWN_FOUR UINT8_C(0x04) +#define BMI160_GYRO_FIFO_DOWN_FIVE UINT8_C(0x05) +#define BMI160_GYRO_FIFO_DOWN_SIX UINT8_C(0x06) +#define BMI160_GYRO_FIFO_DOWN_SEVEN UINT8_C(0x07) + +/* Accel Fifo filter enable*/ +#define BMI160_ACCEL_FIFO_FILT_EN UINT8_C(0x80) + +/* Gyro Fifo filter enable*/ +#define BMI160_GYRO_FIFO_FILT_EN UINT8_C(0x08) + +/** Definitions to check validity of FIFO frames */ +#define FIFO_CONFIG_MSB_CHECK UINT8_C(0x80) +#define FIFO_CONFIG_LSB_CHECK UINT8_C(0x00) + +/*! BMI160 accel FOC configurations */ +#define BMI160_FOC_ACCEL_DISABLED UINT8_C(0x00) +#define BMI160_FOC_ACCEL_POSITIVE_G UINT8_C(0x01) +#define BMI160_FOC_ACCEL_NEGATIVE_G UINT8_C(0x02) +#define BMI160_FOC_ACCEL_0G UINT8_C(0x03) + +/** Array Parameter DefinItions */ +#define BMI160_SENSOR_TIME_LSB_BYTE UINT8_C(0) +#define BMI160_SENSOR_TIME_XLSB_BYTE UINT8_C(1) +#define BMI160_SENSOR_TIME_MSB_BYTE UINT8_C(2) + +/** Interface settings */ +#define BMI160_SPI_INTF UINT8_C(1) +#define BMI160_I2C_INTF UINT8_C(0) +#define BMI160_SPI_RD_MASK UINT8_C(0x80) +#define BMI160_SPI_WR_MASK UINT8_C(0x7F) + +/* Sensor & time select definition*/ +#define BMI160_ACCEL_SEL UINT8_C(0x01) +#define BMI160_GYRO_SEL UINT8_C(0x02) +#define BMI160_TIME_SEL UINT8_C(0x04) + +/* Sensor select mask*/ +#define BMI160_SEN_SEL_MASK UINT8_C(0x07) + +/* Error code mask */ +#define BMI160_ERR_REG_MASK UINT8_C(0x0F) + +/* BMI160 I2C address */ +#define BMI160_I2C_ADDR UINT8_C(0x68) + +/* BMI160 secondary IF address */ +#define BMI160_AUX_BMM150_I2C_ADDR UINT8_C(0x10) + +/** BMI160 Length definitions */ +#define BMI160_ONE UINT8_C(1) +#define BMI160_TWO UINT8_C(2) +#define BMI160_THREE UINT8_C(3) +#define BMI160_FOUR UINT8_C(4) +#define BMI160_FIVE UINT8_C(5) + +/** BMI160 fifo level Margin */ +#define BMI160_FIFO_LEVEL_MARGIN UINT8_C(16) + +/** BMI160 fifo flush Command */ +#define BMI160_FIFO_FLUSH_VALUE UINT8_C(0xB0) + +/** BMI160 offset values for xyz axes of accel */ +#define BMI160_ACCEL_MIN_OFFSET INT8_C(-128) +#define BMI160_ACCEL_MAX_OFFSET INT8_C(127) + +/** BMI160 offset values for xyz axes of gyro */ +#define BMI160_GYRO_MIN_OFFSET INT16_C(-512) +#define BMI160_GYRO_MAX_OFFSET INT16_C(511) + +/** BMI160 fifo full interrupt position and mask */ +#define BMI160_FIFO_FULL_INT_POS UINT8_C(5) +#define BMI160_FIFO_FULL_INT_MSK UINT8_C(0x20) +#define BMI160_FIFO_WTM_INT_POS UINT8_C(6) +#define BMI160_FIFO_WTM_INT_MSK UINT8_C(0x40) + +#define BMI160_FIFO_FULL_INT_PIN1_POS UINT8_C(5) +#define BMI160_FIFO_FULL_INT_PIN1_MSK UINT8_C(0x20) +#define BMI160_FIFO_FULL_INT_PIN2_POS UINT8_C(1) +#define BMI160_FIFO_FULL_INT_PIN2_MSK UINT8_C(0x02) + +#define BMI160_FIFO_WTM_INT_PIN1_POS UINT8_C(6) +#define BMI160_FIFO_WTM_INT_PIN1_MSK UINT8_C(0x40) +#define BMI160_FIFO_WTM_INT_PIN2_POS UINT8_C(2) +#define BMI160_FIFO_WTM_INT_PIN2_MSK UINT8_C(0x04) + +#define BMI160_MANUAL_MODE_EN_POS UINT8_C(7) +#define BMI160_MANUAL_MODE_EN_MSK UINT8_C(0x80) +#define BMI160_AUX_READ_BURST_POS UINT8_C(0) +#define BMI160_AUX_READ_BURST_MSK UINT8_C(0x03) + +#define BMI160_GYRO_SELF_TEST_POS UINT8_C(4) +#define BMI160_GYRO_SELF_TEST_MSK UINT8_C(0x10) +#define BMI160_GYRO_SELF_TEST_STATUS_POS UINT8_C(1) +#define BMI160_GYRO_SELF_TEST_STATUS_MSK UINT8_C(0x02) + +#define BMI160_GYRO_FOC_EN_POS UINT8_C(6) +#define BMI160_GYRO_FOC_EN_MSK UINT8_C(0x40) + +#define BMI160_ACCEL_FOC_X_CONF_POS UINT8_C(4) +#define BMI160_ACCEL_FOC_X_CONF_MSK UINT8_C(0x30) + +#define BMI160_ACCEL_FOC_Y_CONF_POS UINT8_C(2) +#define BMI160_ACCEL_FOC_Y_CONF_MSK UINT8_C(0x0C) + +#define BMI160_ACCEL_FOC_Z_CONF_MSK UINT8_C(0x03) + +#define BMI160_FOC_STATUS_POS UINT8_C(3) +#define BMI160_FOC_STATUS_MSK UINT8_C(0x08) + +#define BMI160_GYRO_OFFSET_X_MSK UINT8_C(0x03) + +#define BMI160_GYRO_OFFSET_Y_POS UINT8_C(2) +#define BMI160_GYRO_OFFSET_Y_MSK UINT8_C(0x0C) + +#define BMI160_GYRO_OFFSET_Z_POS UINT8_C(4) +#define BMI160_GYRO_OFFSET_Z_MSK UINT8_C(0x30) + +#define BMI160_GYRO_OFFSET_EN_POS UINT8_C(7) +#define BMI160_GYRO_OFFSET_EN_MSK UINT8_C(0x80) + +#define BMI160_ACCEL_OFFSET_EN_POS UINT8_C(6) +#define BMI160_ACCEL_OFFSET_EN_MSK UINT8_C(0x40) + +#define BMI160_GYRO_OFFSET_POS UINT16_C(8) +#define BMI160_GYRO_OFFSET_MSK UINT16_C(0x0300) + +#define BMI160_NVM_UPDATE_POS UINT8_C(1) +#define BMI160_NVM_UPDATE_MSK UINT8_C(0x02) + +#define BMI160_NVM_STATUS_POS UINT8_C(4) +#define BMI160_NVM_STATUS_MSK UINT8_C(0x10) + +#define BMI160_MAG_POWER_MODE_MSK UINT8_C(0x03) + +#define BMI160_ACCEL_POWER_MODE_MSK UINT8_C(0x30) +#define BMI160_ACCEL_POWER_MODE_POS UINT8_C(4) + +#define BMI160_GYRO_POWER_MODE_MSK UINT8_C(0x0C) +#define BMI160_GYRO_POWER_MODE_POS UINT8_C(2) + +/* BIT SLICE GET AND SET FUNCTIONS */ +#define BMI160_GET_BITS(regvar, bitname) \ + ((regvar & bitname##_MSK) >> bitname##_POS) +#define BMI160_SET_BITS(regvar, bitname, val) \ + ((regvar & ~bitname##_MSK) | \ + ((val << bitname##_POS) & bitname##_MSK)) + +#define BMI160_SET_BITS_POS_0(reg_data, bitname, data) \ + ((reg_data & ~(bitname##_MSK)) | \ + (data & bitname##_MSK)) + +#define BMI160_GET_BITS_POS_0(reg_data, bitname) (reg_data & (bitname##_MSK)) + +/**\name UTILITY MACROS */ +#define BMI160_SET_LOW_BYTE UINT16_C(0x00FF) +#define BMI160_SET_HIGH_BYTE UINT16_C(0xFF00) + +#define BMI160_GET_LSB(var) (uint8_t)(var & BMI160_SET_LOW_BYTE) +#define BMI160_GET_MSB(var) (uint8_t)((var & BMI160_SET_HIGH_BYTE) >> 8) + +/*****************************************************************************/ +/* type definitions */ +typedef int8_t (*bmi160_com_fptr_t)(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len); +typedef void (*bmi160_delay_fptr_t)(uint32_t period); + +/*************************** Data structures *********************************/ +struct bmi160_pmu_status +{ + /*! Power mode status of Accel + * Possible values : + * - BMI160_ACCEL_PMU_SUSPEND + * - BMI160_ACCEL_PMU_NORMAL + * - BMI160_ACCEL_PMU_LOW_POWER + */ + uint8_t accel_pmu_status; + + /*! Power mode status of Gyro + * Possible values : + * - BMI160_GYRO_PMU_SUSPEND + * - BMI160_GYRO_PMU_NORMAL + * - BMI160_GYRO_PMU_FSU + */ + uint8_t gyro_pmu_status; + + /*! Power mode status of 'Auxiliary sensor interface' whereas the actual + * power mode of the aux. sensor should be configured + * according to the connected sensor specifications + * Possible values : + * - BMI160_AUX_PMU_SUSPEND + * - BMI160_AUX_PMU_NORMAL + * - BMI160_AUX_PMU_LOW_POWER + */ + uint8_t aux_pmu_status; +}; + +/*! + * @brief bmi160 interrupt status selection enum. + */ +enum bmi160_int_status_sel { + BMI160_INT_STATUS_0 = 1, + BMI160_INT_STATUS_1 = 2, + BMI160_INT_STATUS_2 = 4, + BMI160_INT_STATUS_3 = 8, + BMI160_INT_STATUS_ALL = 15 +}; + +/*! + * @brief bmi160 interrupt status bits structure + */ +struct bmi160_int_status_bits +{ +#if LITTLE_ENDIAN == 1 + uint32_t step : 1; + uint32_t sigmot : 1; + uint32_t anym : 1; + + /* pmu trigger will be handled later */ + uint32_t pmu_trigger_reserved : 1; + uint32_t d_tap : 1; + uint32_t s_tap : 1; + uint32_t orient : 1; + uint32_t flat_int : 1; + uint32_t reserved : 2; + uint32_t high_g : 1; + uint32_t low_g : 1; + uint32_t drdy : 1; + uint32_t ffull : 1; + uint32_t fwm : 1; + uint32_t nomo : 1; + uint32_t anym_first_x : 1; + uint32_t anym_first_y : 1; + uint32_t anym_first_z : 1; + uint32_t anym_sign : 1; + uint32_t tap_first_x : 1; + uint32_t tap_first_y : 1; + uint32_t tap_first_z : 1; + uint32_t tap_sign : 1; + uint32_t high_first_x : 1; + uint32_t high_first_y : 1; + uint32_t high_first_z : 1; + uint32_t high_sign : 1; + uint32_t orient_1_0 : 2; + uint32_t orient_2 : 1; + uint32_t flat : 1; +#elif BIG_ENDIAN == 1 + uint32_t high_first_x : 1; + uint32_t high_first_y : 1; + uint32_t high_first_z : 1; + uint32_t high_sign : 1; + uint32_t orient_1_0 : 2; + uint32_t orient_2 : 1; + uint32_t flat : 1; + uint32_t anym_first_x : 1; + uint32_t anym_first_y : 1; + uint32_t anym_first_z : 1; + uint32_t anym_sign : 1; + uint32_t tap_first_x : 1; + uint32_t tap_first_y : 1; + uint32_t tap_first_z : 1; + uint32_t tap_sign : 1; + uint32_t reserved : 2; + uint32_t high_g : 1; + uint32_t low_g : 1; + uint32_t drdy : 1; + uint32_t ffull : 1; + uint32_t fwm : 1; + uint32_t nomo : 1; + uint32_t step : 1; + uint32_t sigmot : 1; + uint32_t anym : 1; + + /* pmu trigger will be handled later */ + uint32_t pmu_trigger_reserved : 1; + uint32_t d_tap : 1; + uint32_t s_tap : 1; + uint32_t orient : 1; + uint32_t flat_int : 1; +#endif +}; + +/*! + * @brief bmi160 interrupt status structure + */ +union bmi160_int_status +{ + uint8_t data[4]; + struct bmi160_int_status_bits bit; +}; + +/*! + * @brief bmi160 sensor data structure which comprises of accel data + */ +struct bmi160_sensor_data +{ + /*! X-axis sensor data */ + int16_t x; + + /*! Y-axis sensor data */ + int16_t y; + + /*! Z-axis sensor data */ + int16_t z; + + /*! sensor time */ + uint32_t sensortime; +}; + +/*! + * @brief bmi160 aux data structure which comprises of 8 bytes of accel data + */ +struct bmi160_aux_data +{ + /*! Auxiliary data */ + uint8_t data[8]; +}; + +/*! + * @brief bmi160 FOC configuration structure + */ +struct bmi160_foc_conf +{ + /*! Enabling FOC in gyro + * Assignable macros : + * - BMI160_ENABLE + * - BMI160_DISABLE + */ + uint8_t foc_gyr_en; + + /*! Accel FOC configurations + * Assignable macros : + * - BMI160_FOC_ACCEL_DISABLED + * - BMI160_FOC_ACCEL_POSITIVE_G + * - BMI160_FOC_ACCEL_NEGATIVE_G + * - BMI160_FOC_ACCEL_0G + */ + uint8_t foc_acc_x; + uint8_t foc_acc_y; + uint8_t foc_acc_z; + + /*! Enabling offset compensation for accel in data registers + * Assignable macros : + * - BMI160_ENABLE + * - BMI160_DISABLE + */ + uint8_t acc_off_en; + + /*! Enabling offset compensation for gyro in data registers + * Assignable macros : + * - BMI160_ENABLE + * - BMI160_DISABLE + */ + uint8_t gyro_off_en; +}; + +/*! + * @brief bmi160 accel gyro offsets + */ +struct bmi160_offsets +{ + /*! Accel offset for x axis */ + int8_t off_acc_x; + + /*! Accel offset for y axis */ + int8_t off_acc_y; + + /*! Accel offset for z axis */ + int8_t off_acc_z; + + /*! Gyro offset for x axis */ + int16_t off_gyro_x; + + /*! Gyro offset for y axis */ + int16_t off_gyro_y; + + /*! Gyro offset for z axis */ + int16_t off_gyro_z; +}; + +/*! + * @brief FIFO aux. sensor data structure + */ +struct bmi160_aux_fifo_data +{ + /*! The value of aux. sensor x LSB data */ + uint8_t aux_x_lsb; + + /*! The value of aux. sensor x MSB data */ + uint8_t aux_x_msb; + + /*! The value of aux. sensor y LSB data */ + uint8_t aux_y_lsb; + + /*! The value of aux. sensor y MSB data */ + uint8_t aux_y_msb; + + /*! The value of aux. sensor z LSB data */ + uint8_t aux_z_lsb; + + /*! The value of aux. sensor z MSB data */ + uint8_t aux_z_msb; + + /*! The value of aux. sensor r for BMM150 LSB data */ + uint8_t aux_r_y2_lsb; + + /*! The value of aux. sensor r for BMM150 MSB data */ + uint8_t aux_r_y2_msb; +}; + +/*! + * @brief bmi160 sensor select structure + */ +enum bmi160_select_sensor { + BMI160_ACCEL_ONLY = 1, + BMI160_GYRO_ONLY, + BMI160_BOTH_ACCEL_AND_GYRO +}; + +/*! + * @brief bmi160 sensor step detector mode structure + */ +enum bmi160_step_detect_mode { + BMI160_STEP_DETECT_NORMAL, + BMI160_STEP_DETECT_SENSITIVE, + BMI160_STEP_DETECT_ROBUST, + + /*! Non recommended User defined setting */ + BMI160_STEP_DETECT_USER_DEFINE +}; + +/*! + * @brief enum for auxiliary burst read selection + */ +enum bm160_aux_read_len { + BMI160_AUX_READ_LEN_0, + BMI160_AUX_READ_LEN_1, + BMI160_AUX_READ_LEN_2, + BMI160_AUX_READ_LEN_3 +}; + +/*! + * @brief bmi160 sensor configuration structure + */ +struct bmi160_cfg +{ + /*! power mode */ + uint8_t power; + + /*! output data rate */ + uint8_t odr; + + /*! range */ + uint8_t range; + + /*! bandwidth */ + uint8_t bw; +}; + +/*! + * @brief Aux sensor configuration structure + */ +struct bmi160_aux_cfg +{ + /*! Aux sensor, 1 - enable 0 - disable */ + uint8_t aux_sensor_enable : 1; + + /*! Aux manual/auto mode status */ + uint8_t manual_enable : 1; + + /*! Aux read burst length */ + uint8_t aux_rd_burst_len : 2; + + /*! output data rate */ + uint8_t aux_odr : 4; + + /*! i2c addr of auxiliary sensor */ + uint8_t aux_i2c_addr; +}; + +/*! + * @brief bmi160 interrupt channel selection structure + */ +enum bmi160_int_channel { + /*! Un-map both channels */ + BMI160_INT_CHANNEL_NONE, + + /*! interrupt Channel 1 */ + BMI160_INT_CHANNEL_1, + + /*! interrupt Channel 2 */ + BMI160_INT_CHANNEL_2, + + /*! Map both channels */ + BMI160_INT_CHANNEL_BOTH +}; +enum bmi160_int_types { + /*! Slope/Any-motion interrupt */ + BMI160_ACC_ANY_MOTION_INT, + + /*! Significant motion interrupt */ + BMI160_ACC_SIG_MOTION_INT, + + /*! Step detector interrupt */ + BMI160_STEP_DETECT_INT, + + /*! double tap interrupt */ + BMI160_ACC_DOUBLE_TAP_INT, + + /*! single tap interrupt */ + BMI160_ACC_SINGLE_TAP_INT, + + /*! orientation interrupt */ + BMI160_ACC_ORIENT_INT, + + /*! flat interrupt */ + BMI160_ACC_FLAT_INT, + + /*! high-g interrupt */ + BMI160_ACC_HIGH_G_INT, + + /*! low-g interrupt */ + BMI160_ACC_LOW_G_INT, + + /*! slow/no-motion interrupt */ + BMI160_ACC_SLOW_NO_MOTION_INT, + + /*! data ready interrupt */ + BMI160_ACC_GYRO_DATA_RDY_INT, + + /*! fifo full interrupt */ + BMI160_ACC_GYRO_FIFO_FULL_INT, + + /*! fifo watermark interrupt */ + BMI160_ACC_GYRO_FIFO_WATERMARK_INT, + + /*! fifo tagging feature support */ + BMI160_FIFO_TAG_INT_PIN +}; + +/*! + * @brief bmi160 active state of any & sig motion interrupt. + */ +enum bmi160_any_sig_motion_active_interrupt_state { + /*! Both any & sig motion are disabled */ + BMI160_BOTH_ANY_SIG_MOTION_DISABLED = -1, + + /*! Any-motion selected */ + BMI160_ANY_MOTION_ENABLED, + + /*! Sig-motion selected */ + BMI160_SIG_MOTION_ENABLED +}; +struct bmi160_acc_tap_int_cfg +{ +#if LITTLE_ENDIAN == 1 + + /*! tap threshold */ + uint16_t tap_thr : 5; + + /*! tap shock */ + uint16_t tap_shock : 1; + + /*! tap quiet */ + uint16_t tap_quiet : 1; + + /*! tap duration */ + uint16_t tap_dur : 3; + + /*! data source 0- filter & 1 pre-filter*/ + uint16_t tap_data_src : 1; + + /*! tap enable, 1 - enable, 0 - disable */ + uint16_t tap_en : 1; +#elif BIG_ENDIAN == 1 + + /*! tap enable, 1 - enable, 0 - disable */ + uint16_t tap_en : 1; + + /*! data source 0- filter & 1 pre-filter*/ + uint16_t tap_data_src : 1; + + /*! tap duration */ + uint16_t tap_dur : 3; + + /*! tap quiet */ + uint16_t tap_quiet : 1; + + /*! tap shock */ + uint16_t tap_shock : 1; + + /*! tap threshold */ + uint16_t tap_thr : 5; +#endif +}; +struct bmi160_acc_any_mot_int_cfg +{ +#if LITTLE_ENDIAN == 1 + + /*! 1 any-motion enable, 0 - any-motion disable */ + uint8_t anymotion_en : 1; + + /*! slope interrupt x, 1 - enable, 0 - disable */ + uint8_t anymotion_x : 1; + + /*! slope interrupt y, 1 - enable, 0 - disable */ + uint8_t anymotion_y : 1; + + /*! slope interrupt z, 1 - enable, 0 - disable */ + uint8_t anymotion_z : 1; + + /*! slope duration */ + uint8_t anymotion_dur : 2; + + /*! data source 0- filter & 1 pre-filter*/ + uint8_t anymotion_data_src : 1; + + /*! slope threshold */ + uint8_t anymotion_thr; +#elif BIG_ENDIAN == 1 + + /*! slope threshold */ + uint8_t anymotion_thr; + + /*! data source 0- filter & 1 pre-filter*/ + uint8_t anymotion_data_src : 1; + + /*! slope duration */ + uint8_t anymotion_dur : 2; + + /*! slope interrupt z, 1 - enable, 0 - disable */ + uint8_t anymotion_z : 1; + + /*! slope interrupt y, 1 - enable, 0 - disable */ + uint8_t anymotion_y : 1; + + /*! slope interrupt x, 1 - enable, 0 - disable */ + uint8_t anymotion_x : 1; + + /*! 1 any-motion enable, 0 - any-motion disable */ + uint8_t anymotion_en : 1; +#endif +}; +struct bmi160_acc_sig_mot_int_cfg +{ +#if LITTLE_ENDIAN == 1 + + /*! skip time of sig-motion interrupt */ + uint8_t sig_mot_skip : 2; + + /*! proof time of sig-motion interrupt */ + uint8_t sig_mot_proof : 2; + + /*! data source 0- filter & 1 pre-filter*/ + uint8_t sig_data_src : 1; + + /*! 1 - enable sig, 0 - disable sig & enable anymotion */ + uint8_t sig_en : 1; + + /*! sig-motion threshold */ + uint8_t sig_mot_thres; +#elif BIG_ENDIAN == 1 + + /*! sig-motion threshold */ + uint8_t sig_mot_thres; + + /*! 1 - enable sig, 0 - disable sig & enable anymotion */ + uint8_t sig_en : 1; + + /*! data source 0- filter & 1 pre-filter*/ + uint8_t sig_data_src : 1; + + /*! proof time of sig-motion interrupt */ + uint8_t sig_mot_proof : 2; + + /*! skip time of sig-motion interrupt */ + uint8_t sig_mot_skip : 2; +#endif +}; +struct bmi160_acc_step_detect_int_cfg +{ +#if LITTLE_ENDIAN == 1 + + /*! 1- step detector enable, 0- step detector disable */ + uint16_t step_detector_en : 1; + + /*! minimum threshold */ + uint16_t min_threshold : 2; + + /*! minimal detectable step time */ + uint16_t steptime_min : 3; + + /*! enable step counter mode setting */ + uint16_t step_detector_mode : 2; + + /*! minimum step buffer size*/ + uint16_t step_min_buf : 3; +#elif BIG_ENDIAN == 1 + + /*! minimum step buffer size*/ + uint16_t step_min_buf : 3; + + /*! enable step counter mode setting */ + uint16_t step_detector_mode : 2; + + /*! minimal detectable step time */ + uint16_t steptime_min : 3; + + /*! minimum threshold */ + uint16_t min_threshold : 2; + + /*! 1- step detector enable, 0- step detector disable */ + uint16_t step_detector_en : 1; +#endif +}; +struct bmi160_acc_no_motion_int_cfg +{ +#if LITTLE_ENDIAN == 1 + + /*! no motion interrupt x */ + uint16_t no_motion_x : 1; + + /*! no motion interrupt y */ + uint16_t no_motion_y : 1; + + /*! no motion interrupt z */ + uint16_t no_motion_z : 1; + + /*! no motion duration */ + uint16_t no_motion_dur : 6; + + /*! no motion sel , 1 - enable no-motion ,0- enable slow-motion */ + uint16_t no_motion_sel : 1; + + /*! data source 0- filter & 1 pre-filter*/ + uint16_t no_motion_src : 1; + + /*! no motion threshold */ + uint8_t no_motion_thres; +#elif BIG_ENDIAN == 1 + + /*! no motion threshold */ + uint8_t no_motion_thres; + + /*! data source 0- filter & 1 pre-filter*/ + uint16_t no_motion_src : 1; + + /*! no motion sel , 1 - enable no-motion ,0- enable slow-motion */ + uint16_t no_motion_sel : 1; + + /*! no motion duration */ + uint16_t no_motion_dur : 6; + + /* no motion interrupt z */ + uint16_t no_motion_z : 1; + + /*! no motion interrupt y */ + uint16_t no_motion_y : 1; + + /*! no motion interrupt x */ + uint16_t no_motion_x : 1; +#endif +}; +struct bmi160_acc_orient_int_cfg +{ +#if LITTLE_ENDIAN == 1 + + /*! thresholds for switching between the different orientations */ + uint16_t orient_mode : 2; + + /*! blocking_mode */ + uint16_t orient_blocking : 2; + + /*! Orientation interrupt hysteresis */ + uint16_t orient_hyst : 4; + + /*! Orientation interrupt theta */ + uint16_t orient_theta : 6; + + /*! Enable/disable Orientation interrupt */ + uint16_t orient_ud_en : 1; + + /*! exchange x- and z-axis in algorithm ,0 - z, 1 - x */ + uint16_t axes_ex : 1; + + /*! 1 - orient enable, 0 - orient disable */ + uint8_t orient_en : 1; +#elif BIG_ENDIAN == 1 + + /*! 1 - orient enable, 0 - orient disable */ + uint8_t orient_en : 1; + + /*! exchange x- and z-axis in algorithm ,0 - z, 1 - x */ + uint16_t axes_ex : 1; + + /*! Enable/disable Orientation interrupt */ + uint16_t orient_ud_en : 1; + + /*! Orientation interrupt theta */ + uint16_t orient_theta : 6; + + /*! Orientation interrupt hysteresis */ + uint16_t orient_hyst : 4; + + /*! blocking_mode */ + uint16_t orient_blocking : 2; + + /*! thresholds for switching between the different orientations */ + uint16_t orient_mode : 2; +#endif +}; +struct bmi160_acc_flat_detect_int_cfg +{ +#if LITTLE_ENDIAN == 1 + + /*! flat threshold */ + uint16_t flat_theta : 6; + + /*! flat interrupt hysteresis */ + uint16_t flat_hy : 3; + + /*! delay time for which the flat value must remain stable for the + * flat interrupt to be generated */ + uint16_t flat_hold_time : 2; + + /*! 1 - flat enable, 0 - flat disable */ + uint16_t flat_en : 1; +#elif BIG_ENDIAN == 1 + + /*! 1 - flat enable, 0 - flat disable */ + uint16_t flat_en : 1; + + /*! delay time for which the flat value must remain stable for the + * flat interrupt to be generated */ + uint16_t flat_hold_time : 2; + + /*! flat interrupt hysteresis */ + uint16_t flat_hy : 3; + + /*! flat threshold */ + uint16_t flat_theta : 6; +#endif +}; +struct bmi160_acc_low_g_int_cfg +{ +#if LITTLE_ENDIAN == 1 + + /*! low-g interrupt trigger delay */ + uint8_t low_dur; + + /*! low-g interrupt trigger threshold */ + uint8_t low_thres; + + /*! hysteresis of low-g interrupt */ + uint8_t low_hyst : 2; + + /*! 0 - single-axis mode ,1 - axis-summing mode */ + uint8_t low_mode : 1; + + /*! data source 0- filter & 1 pre-filter */ + uint8_t low_data_src : 1; + + /*! 1 - enable low-g, 0 - disable low-g */ + uint8_t low_en : 1; +#elif BIG_ENDIAN == 1 + + /*! 1 - enable low-g, 0 - disable low-g */ + uint8_t low_en : 1; + + /*! data source 0- filter & 1 pre-filter */ + uint8_t low_data_src : 1; + + /*! 0 - single-axis mode ,1 - axis-summing mode */ + uint8_t low_mode : 1; + + /*! hysteresis of low-g interrupt */ + uint8_t low_hyst : 2; + + /*! low-g interrupt trigger threshold */ + uint8_t low_thres; + + /*! low-g interrupt trigger delay */ + uint8_t low_dur; +#endif +}; +struct bmi160_acc_high_g_int_cfg +{ +#if LITTLE_ENDIAN == 1 + + /*! High-g interrupt x, 1 - enable, 0 - disable */ + uint8_t high_g_x : 1; + + /*! High-g interrupt y, 1 - enable, 0 - disable */ + uint8_t high_g_y : 1; + + /*! High-g interrupt z, 1 - enable, 0 - disable */ + uint8_t high_g_z : 1; + + /*! High-g hysteresis */ + uint8_t high_hy : 2; + + /*! data source 0- filter & 1 pre-filter */ + uint8_t high_data_src : 1; + + /*! High-g threshold */ + uint8_t high_thres; + + /*! High-g duration */ + uint8_t high_dur; +#elif BIG_ENDIAN == 1 + + /*! High-g duration */ + uint8_t high_dur; + + /*! High-g threshold */ + uint8_t high_thres; + + /*! data source 0- filter & 1 pre-filter */ + uint8_t high_data_src : 1; + + /*! High-g hysteresis */ + uint8_t high_hy : 2; + + /*! High-g interrupt z, 1 - enable, 0 - disable */ + uint8_t high_g_z : 1; + + /*! High-g interrupt y, 1 - enable, 0 - disable */ + uint8_t high_g_y : 1; + + /*! High-g interrupt x, 1 - enable, 0 - disable */ + uint8_t high_g_x : 1; +#endif +}; +struct bmi160_int_pin_settg +{ +#if LITTLE_ENDIAN == 1 + + /*! To enable either INT1 or INT2 pin as output. + * 0- output disabled ,1- output enabled */ + uint16_t output_en : 1; + + /*! 0 - push-pull 1- open drain,only valid if output_en is set 1 */ + uint16_t output_mode : 1; + + /*! 0 - active low , 1 - active high level. + * if output_en is 1,this applies to interrupts,else PMU_trigger */ + uint16_t output_type : 1; + + /*! 0 - level trigger , 1 - edge trigger */ + uint16_t edge_ctrl : 1; + + /*! To enable either INT1 or INT2 pin as input. + * 0 - input disabled ,1 - input enabled */ + uint16_t input_en : 1; + + /*! latch duration*/ + uint16_t latch_dur : 4; +#elif BIG_ENDIAN == 1 + + /*! latch duration*/ + uint16_t latch_dur : 4; + + /*! Latched,non-latched or temporary interrupt modes */ + uint16_t input_en : 1; + + /*! 1 - edge trigger, 0 - level trigger */ + uint16_t edge_ctrl : 1; + + /*! 0 - active low , 1 - active high level. + * if output_en is 1,this applies to interrupts,else PMU_trigger */ + uint16_t output_type : 1; + + /*! 0 - push-pull , 1 - open drain,only valid if output_en is set 1 */ + uint16_t output_mode : 1; + + /*! To enable either INT1 or INT2 pin as output. + * 0 - output disabled , 1 - output enabled */ + uint16_t output_en : 1; +#endif +}; +union bmi160_int_type_cfg +{ + /*! Tap interrupt structure */ + struct bmi160_acc_tap_int_cfg acc_tap_int; + + /*! Slope interrupt structure */ + struct bmi160_acc_any_mot_int_cfg acc_any_motion_int; + + /*! Significant motion interrupt structure */ + struct bmi160_acc_sig_mot_int_cfg acc_sig_motion_int; + + /*! Step detector interrupt structure */ + struct bmi160_acc_step_detect_int_cfg acc_step_detect_int; + + /*! No motion interrupt structure */ + struct bmi160_acc_no_motion_int_cfg acc_no_motion_int; + + /*! Orientation interrupt structure */ + struct bmi160_acc_orient_int_cfg acc_orient_int; + + /*! Flat interrupt structure */ + struct bmi160_acc_flat_detect_int_cfg acc_flat_int; + + /*! Low-g interrupt structure */ + struct bmi160_acc_low_g_int_cfg acc_low_g_int; + + /*! High-g interrupt structure */ + struct bmi160_acc_high_g_int_cfg acc_high_g_int; +}; +struct bmi160_int_settg +{ + /*! Interrupt channel */ + enum bmi160_int_channel int_channel; + + /*! Select Interrupt */ + enum bmi160_int_types int_type; + + /*! Structure configuring Interrupt pins */ + struct bmi160_int_pin_settg int_pin_settg; + + /*! Union configures required interrupt */ + union bmi160_int_type_cfg int_type_cfg; + + /*! FIFO FULL INT 1-enable, 0-disable */ + uint8_t fifo_full_int_en : 1; + + /*! FIFO WTM INT 1-enable, 0-disable */ + uint8_t fifo_wtm_int_en : 1; +}; + +/*! + * @brief This structure holds the information for usage of + * FIFO by the user. + */ +struct bmi160_fifo_frame +{ + /*! Data buffer of user defined length is to be mapped here */ + uint8_t *data; + + /*! While calling the API "bmi160_get_fifo_data" , length stores + * number of bytes in FIFO to be read (specified by user as input) + * and after execution of the API ,number of FIFO data bytes + * available is provided as an output to user + */ + uint16_t length; + + /*! FIFO time enable */ + uint8_t fifo_time_enable; + + /*! Enabling of the FIFO header to stream in header mode */ + uint8_t fifo_header_enable; + + /*! Streaming of the Accelerometer, Gyroscope + * sensor data or both in FIFO */ + uint8_t fifo_data_enable; + + /*! Will be equal to length when no more frames are there to parse */ + uint16_t accel_byte_start_idx; + + /*! Will be equal to length when no more frames are there to parse */ + uint16_t gyro_byte_start_idx; + + /*! Will be equal to length when no more frames are there to parse */ + uint16_t aux_byte_start_idx; + + /*! Value of FIFO sensor time time */ + uint32_t sensor_time; + + /*! Value of Skipped frame counts */ + uint8_t skipped_frame_count; +}; +struct bmi160_dev +{ + /*! Chip Id */ + uint8_t chip_id; + + /*! Device Id */ + uint8_t id; + + /*! 0 - I2C , 1 - SPI Interface */ + uint8_t interface; + + /*! Hold active interrupts status for any and sig motion + * 0 - Any-motion enable, 1 - Sig-motion enable, + * -1 neither any-motion nor sig-motion selected */ + enum bmi160_any_sig_motion_active_interrupt_state any_sig_sel; + + /*! Structure to configure Accel sensor */ + struct bmi160_cfg accel_cfg; + + /*! Structure to hold previous/old accel config parameters. + * This is used at driver level to prevent overwriting of same + * data, hence user does not change it in the code */ + struct bmi160_cfg prev_accel_cfg; + + /*! Structure to configure Gyro sensor */ + struct bmi160_cfg gyro_cfg; + + /*! Structure to hold previous/old gyro config parameters. + * This is used at driver level to prevent overwriting of same + * data, hence user does not change it in the code */ + struct bmi160_cfg prev_gyro_cfg; + + /*! Structure to configure the auxiliary sensor */ + struct bmi160_aux_cfg aux_cfg; + + /*! Structure to hold previous/old aux config parameters. + * This is used at driver level to prevent overwriting of same + * data, hence user does not change it in the code */ + struct bmi160_aux_cfg prev_aux_cfg; + + /*! FIFO related configurations */ + struct bmi160_fifo_frame *fifo; + + /*! Read function pointer */ + bmi160_com_fptr_t read; + + /*! Write function pointer */ + bmi160_com_fptr_t write; + + /*! Delay function pointer */ + bmi160_delay_fptr_t delay_ms; + + /*! User set read/write length */ + uint16_t read_write_len; + + /*! For switching from I2C to SPI */ + uint8_t dummy_byte; +}; + +#endif /* BMI160_DEFS_H_ */