Merge pull request #10715 from mikeller/update_bmi270_config
Switched to 'maximum FIFO' version of the BMI270 microcode.
This commit is contained in:
commit
148f9e9d90
|
@ -6,15 +6,15 @@ Redistribution and use in source and binary forms, with or without
|
|||
modification, are permitted provided that the following conditions are met:
|
||||
|
||||
1. Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
|
||||
2. Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
|
||||
3. Neither the name of the copyright holder nor the names of its
|
||||
contributors may be used to endorse or promote products derived from
|
||||
this software without specific prior written permission.
|
||||
contributors may be used to endorse or promote products derived from
|
||||
this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
|
|
|
@ -0,0 +1,170 @@
|
|||
# Sensor API for the BMI2's OIS interface
|
||||
|
||||
## Table of Contents
|
||||
- [Introduction](#Intro)
|
||||
- [Integration details](#Integration)
|
||||
- [Driver files information](#file)
|
||||
- [Sensor interfaces](#interface)
|
||||
- [Integration Examples](#examples)
|
||||
|
||||
### Introduction<a name=Intro></a>
|
||||
This package contains Bosch Sensortec's BMI2 Sensor API.
|
||||
|
||||
### Integration details<a name=Integration></a>
|
||||
- Integrate _bmi2.c_, _bmi2.h_, _bmi2_ois.c_, _bmi2_ois.h_, _bmi2_defs.h_ and the required variant files in your project.
|
||||
- User has to include _bmi2_ois.h_ in the code to call OIS related APIs and a _variant header_ for initialization as
|
||||
well as BMI2 related API calls, as shown below:
|
||||
``` c
|
||||
#include "bmi270.h"
|
||||
#include "bmi2_ois.h"
|
||||
````
|
||||
### Driver files information<a name=file></a>
|
||||
- *_bmi2_ois.c_*
|
||||
* This file has function definitions of OIS related API interfaces.
|
||||
- *_bmi2_ois.h_*
|
||||
* This header file has necessary include files, function declarations, required to make OIS related API calls.
|
||||
|
||||
### Sensor interfaces<a name=interface></a>
|
||||
#### _Host Interface_
|
||||
- I2C interface
|
||||
- SPI interface
|
||||
_Note: By default, the interface is I2C._
|
||||
|
||||
#### _OIS Interface_
|
||||
- SPI interface
|
||||
|
||||
### Integration examples<a name=examples></a>
|
||||
#### Configuring SPI/I2C for host interface.
|
||||
To configure host interface, an instance of the bmi2_dev structure should be
|
||||
created for initializing BMI2 sensor. "_Refer **README** for initializing BMI2
|
||||
sensor._"
|
||||
|
||||
#### Configuring SPI for OIS interface.
|
||||
To configure OIS interface, an instance of the bmi2_ois_dev structure should be
|
||||
created. The following parameters are required to be updated in the structure,
|
||||
by the user.
|
||||
|
||||
Parameters | Details
|
||||
--------------|-----------------------------------
|
||||
_intf_ptr_ | device address reference of SPI interface
|
||||
_ois_read_ | read through SPI interface
|
||||
_ois_write_ | read through SPI interface
|
||||
_ois_delay_us_| delay in micro seconds
|
||||
_acc_en_ | for enabling accelerometer
|
||||
_gyr_en_ | for enabling gyroscope
|
||||
|
||||
``` c
|
||||
int8_t rslt = 0;
|
||||
|
||||
struct bmi2_ois_dev ois_dev = {
|
||||
.intf_ptr = intf_ptr will contain the chip selection info of SPI CS pin,
|
||||
.ois_read = user_spi_reg_read,
|
||||
.ois_write = user_spi_reg_write,
|
||||
.ois_delay_us = user_delay_us
|
||||
};
|
||||
```
|
||||
>**_Important Note_**: For initializing and configuring BMI2 sensors, which is
|
||||
done through host interface, the API's are to be used from bmi2.c file. Rest
|
||||
of the API's, for OIS configurations and the reading of OIS data, which is done
|
||||
through OIS interface, are to be used from bmi2_ois.c file.
|
||||
|
||||
##### Get accelerometer and gyroscope data through OIS interface
|
||||
``` c
|
||||
int8_t rslt;
|
||||
/* Array to enable sensor through host interface */
|
||||
uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO};
|
||||
/* Array to enable sensor through OIS interface */
|
||||
uint8_t sens_sel[2] = {BMI2_OIS_ACCEL, BMI2_OIS_GYRO};
|
||||
/* Initialize the configuration structure */
|
||||
struct bmi2_sens_config sens_cfg = {0};
|
||||
|
||||
/* Initialize BMI2 */
|
||||
rslt = bmi2_init(&dev);
|
||||
if (rslt != BMI2_OK) {
|
||||
printf("Error: %d\n", rslt);
|
||||
return;
|
||||
}
|
||||
|
||||
/* Enable accelerometer and gyroscope through host interface */
|
||||
rslt = bmi270_sensor_enable(sens_list, 2, &dev);
|
||||
if (rslt != BMI2_OK) {
|
||||
printf("Error: %d\n", rslt);
|
||||
return;
|
||||
}
|
||||
|
||||
/* Setting of OIS Range is done through host interface */
|
||||
/* Select the gyroscope sensor for OIS Range configuration */
|
||||
sens_cfg.type = BMI2_GYRO;
|
||||
|
||||
/* Get gyroscope configuration */
|
||||
rslt = bmi2_get_sensor_config(&sens_cfg, 1, &dev);
|
||||
if (rslt != BMI2_OK) {
|
||||
printf("Error: %d\n", rslt);
|
||||
return;
|
||||
}
|
||||
|
||||
/* Set the desired OIS Range */
|
||||
sens_cfg.cfg.gyr.ois_range = BMI2_GYR_OIS_2000;
|
||||
|
||||
/* Set gyroscope configuration for default values */
|
||||
rslt = bmi2_set_sensor_config(&sens_cfg, 1, &dev);
|
||||
if (rslt != BMI2_OK) {
|
||||
printf("Error: %d\n", rslt);
|
||||
return;
|
||||
}
|
||||
|
||||
/* Enable OIS through host interface */
|
||||
rslt = bmi2_set_ois_interface(BMI2_ENABLE, &dev);
|
||||
if (rslt != BMI2_OK) {
|
||||
printf("Error: %d\n", rslt);
|
||||
return;
|
||||
}
|
||||
|
||||
/* Disable Advance Power Save Mode through host interface */
|
||||
rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &dev);
|
||||
if (rslt != BMI2_OK) {
|
||||
printf("Error: %d\n", rslt);
|
||||
return;
|
||||
}
|
||||
|
||||
/* Get configurations for OIS through OIS interface for default values */
|
||||
rslt = bmi2_ois_get_config(&ois_dev);
|
||||
if (rslt != BMI2_OK) {
|
||||
printf("Error: %d\n", rslt);
|
||||
return;
|
||||
}
|
||||
|
||||
/* Enable accelerometer and gyroscope for reading OIS data */
|
||||
ois_dev.acc_en = BMI2_ENABLE;
|
||||
ois_dev.gyr_en = BMI2_ENABLE;
|
||||
|
||||
/* Set configurations for OIS through OIS interface */
|
||||
rslt = bmi2_ois_set_config(&ois_dev);
|
||||
if (rslt == BMI2_OK) {
|
||||
/* Get OIS accelerometer and gyroscope data through OIS interface */
|
||||
rslt = bmi2_ois_read_data(sens_sel, 2, &ois_dev);
|
||||
if (rslt == BMI2_OK) {
|
||||
/* Print accelerometer data */
|
||||
printf("OIS Accel x-axis = %d\t", ois_dev.acc_data.x);
|
||||
printf("OIS Accel y-axis= %d\t", ois_dev.acc_data.y);
|
||||
printf("OIS Accel z-axis = %d\r\n", ois_dev.acc_data.z);
|
||||
|
||||
/* Print gyroscope data */
|
||||
printf("OIS Gyro x-axis = %d\t", ois_dev.gyr_data.x);
|
||||
printf("OIS Gyro y-axis= %d\t", ois_dev.gyr_data.y);
|
||||
printf("OIS Gyro z-axis = %d\r\n", ois_dev.gyr_data.z);
|
||||
}
|
||||
}
|
||||
|
||||
if (rslt != BMI2_OK) {
|
||||
printf("Error code: %d\n", rslt);
|
||||
return;
|
||||
}
|
||||
|
||||
/* Enable Advance Power Save Mode through host interface */
|
||||
rslt = bmi2_set_adv_power_save(BMI2_ENABLE, &dev);
|
||||
if (rslt != BMI2_OK) {
|
||||
printf("Error: %d\n", rslt);
|
||||
return;
|
||||
}
|
||||
```
|
|
@ -2,18 +2,13 @@ BMI2xy Sensor API
|
|||
|
||||
> This package contains BMI2xy sensor API
|
||||
|
||||
**Note : When updating from 2.19.0 to 2.34.0, the gyroscope offset calibration data cannot be reused. In case of a permanently stored gyroscope offset (inside the sensor NVM or on the host), please perform calibration again with API version 2.34.0.
|
||||
|
||||
## Sensor Overview
|
||||
|
||||
The BMI2xy is a small, low power, low noise inertial measurement unit designed for use in mobile applications like augmented reality or indoor navigation which require highly accurate, real-time sensor data.
|
||||
|
||||
### Features
|
||||
|
||||
- Indoor navigation, pedestrian dead-reckoning, step-counting
|
||||
|
||||
For more information refer [product page](https://www.bosch-sensortec.com/bst/products/all_products/bmi270)
|
||||
For more information refer product page [Link](https://www.bosch-sensortec.com/products/motion-sensors/imus/bmi270.html)
|
||||
|
||||
---
|
||||
|
||||
#### Copyright (C) 2019 Bosch Sensortec GmbH
|
||||
---
|
Before Width: | Height: | Size: 777 B After Width: | Height: | Size: 493 B |
|
@ -3,4 +3,7 @@ This library is used to support the Bosch BMI270 gyro sensor (see drivers/accgyr
|
|||
Library download location:
|
||||
https://github.com/BoschSensortec/BMI270-Sensor-API
|
||||
|
||||
The only file that is compiled as part of Betaflight is bmi270.c. This file contains the device microcode that must be uploaded during initialization.
|
||||
Version:
|
||||
2.63.1
|
||||
|
||||
The only file that is compiled as part of Betaflight is bmi270_maximum_fifo.c. This file contains the device microcode that must be uploaded during initialization.
|
||||
|
|
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
|
@ -30,11 +30,19 @@
|
|||
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* @file bmi270.h
|
||||
* @date 2020-01-10
|
||||
* @version v2.46.1
|
||||
* @file bmi270.h
|
||||
* @date 2020-11-04
|
||||
* @version v2.63.1
|
||||
*
|
||||
*/#ifndef BMI270_H_
|
||||
*/
|
||||
|
||||
/**
|
||||
* \ingroup bmi2xy
|
||||
* \defgroup bmi270 BMI270
|
||||
* @brief Sensor driver for BMI270 sensor
|
||||
*/
|
||||
|
||||
#ifndef BMI270_H_
|
||||
#define BMI270_H_
|
||||
|
||||
/*! CPP guard */
|
||||
|
@ -54,57 +62,82 @@ extern "C" {
|
|||
****************************************************************************/
|
||||
|
||||
/*! @name BMI270 Chip identifier */
|
||||
#define BMI270_CHIP_ID UINT8_C(0x24)
|
||||
#define BMI270_CHIP_ID UINT8_C(0x24)
|
||||
|
||||
/*! @name BMI270 feature input start addresses */
|
||||
#define BMI270_MAX_BURST_LEN_STRT_ADDR UINT8_C(0x02)
|
||||
#define BMI270_CRT_GYRO_SELF_TEST_STRT_ADDR UINT8_C(0x03)
|
||||
#define BMI270_ABORT_STRT_ADDR UINT8_C(0x03)
|
||||
#define BMI270_AXIS_MAP_STRT_ADDR UINT8_C(0x04)
|
||||
#define BMI270_GYRO_SELF_OFF_STRT_ADDR UINT8_C(0x05)
|
||||
#define BMI270_NVM_PROG_PREP_STRT_ADDR UINT8_C(0x05)
|
||||
#define BMI270_GYRO_GAIN_UPDATE_STRT_ADDR UINT8_C(0x06)
|
||||
#define BMI270_ANY_MOT_STRT_ADDR UINT8_C(0x0C)
|
||||
#define BMI270_NO_MOT_STRT_ADDR UINT8_C(0x00)
|
||||
#define BMI270_SIG_MOT_STRT_ADDR UINT8_C(0x04)
|
||||
#define BMI270_STEP_CNT_1_STRT_ADDR UINT8_C(0x00)
|
||||
#define BMI270_STEP_CNT_4_STRT_ADDR UINT8_C(0x02)
|
||||
#define BMI270_WRIST_GEST_STRT_ADDR UINT8_C(0x06)
|
||||
#define BMI270_WRIST_WEAR_WAKE_UP_STRT_ADDR UINT8_C(0x00)
|
||||
#define BMI270_CONFIG_ID_STRT_ADDR UINT8_C(0x00)
|
||||
#define BMI270_MAX_BURST_LEN_STRT_ADDR UINT8_C(0x02)
|
||||
#define BMI270_CRT_GYRO_SELF_TEST_STRT_ADDR UINT8_C(0x03)
|
||||
#define BMI270_ABORT_STRT_ADDR UINT8_C(0x03)
|
||||
#define BMI270_AXIS_MAP_STRT_ADDR UINT8_C(0x04)
|
||||
#define BMI270_GYRO_SELF_OFF_STRT_ADDR UINT8_C(0x05)
|
||||
#define BMI270_NVM_PROG_PREP_STRT_ADDR UINT8_C(0x05)
|
||||
#define BMI270_GYRO_GAIN_UPDATE_STRT_ADDR UINT8_C(0x06)
|
||||
#define BMI270_ANY_MOT_STRT_ADDR UINT8_C(0x0C)
|
||||
#define BMI270_NO_MOT_STRT_ADDR UINT8_C(0x00)
|
||||
#define BMI270_SIG_MOT_STRT_ADDR UINT8_C(0x04)
|
||||
#define BMI270_STEP_CNT_1_STRT_ADDR UINT8_C(0x00)
|
||||
#define BMI270_STEP_CNT_4_STRT_ADDR UINT8_C(0x02)
|
||||
#define BMI270_WRIST_GEST_STRT_ADDR UINT8_C(0x06)
|
||||
#define BMI270_WRIST_WEAR_WAKE_UP_STRT_ADDR UINT8_C(0x00)
|
||||
|
||||
/*! @name BMI270 feature output start addresses */
|
||||
#define BMI270_STEP_CNT_OUT_STRT_ADDR UINT8_C(0x00)
|
||||
#define BMI270_STEP_ACT_OUT_STRT_ADDR UINT8_C(0x04)
|
||||
#define BMI270_WRIST_GEST_OUT_STRT_ADDR UINT8_C(0x06)
|
||||
#define BMI270_GYR_USER_GAIN_OUT_STRT_ADDR UINT8_C(0x08)
|
||||
#define BMI270_GYRO_CROSS_SENSE_STRT_ADDR UINT8_C(0x0C)
|
||||
#define BMI270_NVM_VFRM_OUT_STRT_ADDR UINT8_C(0x0E)
|
||||
#define BMI270_STEP_CNT_OUT_STRT_ADDR UINT8_C(0x00)
|
||||
#define BMI270_STEP_ACT_OUT_STRT_ADDR UINT8_C(0x04)
|
||||
#define BMI270_WRIST_GEST_OUT_STRT_ADDR UINT8_C(0x06)
|
||||
#define BMI270_GYR_USER_GAIN_OUT_STRT_ADDR UINT8_C(0x08)
|
||||
#define BMI270_GYRO_CROSS_SENSE_STRT_ADDR UINT8_C(0x0C)
|
||||
#define BMI270_NVM_VFRM_OUT_STRT_ADDR UINT8_C(0x0E)
|
||||
|
||||
/*! @name Defines maximum number of pages */
|
||||
#define BMI270_MAX_PAGE_NUM UINT8_C(8)
|
||||
#define BMI270_MAX_PAGE_NUM UINT8_C(8)
|
||||
|
||||
/*! @name Defines maximum number of feature input configurations */
|
||||
#define BMI270_MAX_FEAT_IN UINT8_C(16)
|
||||
#define BMI270_MAX_FEAT_IN UINT8_C(17)
|
||||
|
||||
/*! @name Defines maximum number of feature outputs */
|
||||
#define BMI270_MAX_FEAT_OUT UINT8_C(7)
|
||||
#define BMI270_MAX_FEAT_OUT UINT8_C(7)
|
||||
|
||||
/*! @name Mask definitions for feature interrupt status bits */
|
||||
#define BMI270_SIG_MOT_STATUS_MASK UINT8_C(0x01)
|
||||
#define BMI270_STEP_CNT_STATUS_MASK UINT8_C(0x02)
|
||||
#define BMI270_STEP_ACT_STATUS_MASK UINT8_C(0x04)
|
||||
#define BMI270_WRIST_WAKE_UP_STATUS_MASK UINT8_C(0x08)
|
||||
#define BMI270_WRIST_GEST_STATUS_MASK UINT8_C(0x10)
|
||||
#define BMI270_NO_MOT_STATUS_MASK UINT8_C(0x20)
|
||||
#define BMI270_ANY_MOT_STATUS_MASK UINT8_C(0x40)
|
||||
#define BMI270_SIG_MOT_STATUS_MASK UINT8_C(0x01)
|
||||
#define BMI270_STEP_CNT_STATUS_MASK UINT8_C(0x02)
|
||||
#define BMI270_STEP_ACT_STATUS_MASK UINT8_C(0x04)
|
||||
#define BMI270_WRIST_WAKE_UP_STATUS_MASK UINT8_C(0x08)
|
||||
#define BMI270_WRIST_GEST_STATUS_MASK UINT8_C(0x10)
|
||||
#define BMI270_NO_MOT_STATUS_MASK UINT8_C(0x20)
|
||||
#define BMI270_ANY_MOT_STATUS_MASK UINT8_C(0x40)
|
||||
|
||||
/*! @name Mask definitions for feature interrupt mapping bits */
|
||||
#define BMI270_INT_SIG_MOT_MASK UINT8_C(0x01)
|
||||
#define BMI270_INT_STEP_COUNTER_MASK UINT8_C(0x02)
|
||||
#define BMI270_INT_STEP_DETECTOR_MASK UINT8_C(0x02)
|
||||
#define BMI270_INT_STEP_ACT_MASK UINT8_C(0x04)
|
||||
#define BMI270_INT_WRIST_WEAR_WAKEUP_MASK UINT8_C(0x08)
|
||||
#define BMI270_INT_WRIST_GEST_MASK UINT8_C(0x10)
|
||||
#define BMI270_INT_NO_MOT_MASK UINT8_C(0x20)
|
||||
#define BMI270_INT_ANY_MOT_MASK UINT8_C(0x40)
|
||||
|
||||
/*! @name Defines maximum number of feature interrupts */
|
||||
#define BMI270_MAX_INT_MAP UINT8_C(8)
|
||||
|
||||
/***************************************************************************/
|
||||
|
||||
/*! BMI270 User Interface function prototypes
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* \ingroup bmi270
|
||||
* \defgroup bmi270ApiInit Initialization
|
||||
* @brief Initialize the sensor and device structure
|
||||
*/
|
||||
|
||||
/*!
|
||||
* @brief This API:
|
||||
* \ingroup bmi270ApiInit
|
||||
* \page bmi270_api_bmi270_init bmi270_init
|
||||
* \code
|
||||
* int8_t bmi270_init(struct bmi2_dev *dev);
|
||||
* \endcode
|
||||
* @details This API:
|
||||
* 1) updates the device structure with address of the configuration file.
|
||||
* 2) Initializes BMI270 sensor.
|
||||
* 3) Writes the configuration file.
|
||||
|
@ -114,14 +147,271 @@ extern "C" {
|
|||
* @param[in, out] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @return Result of API execution status
|
||||
*
|
||||
* @retval BMI2_OK - Success
|
||||
* @retval BMI2_E_NULL_PTR - Error: Null pointer error
|
||||
* @retval BMI2_E_COM_FAIL - Error: Communication fail
|
||||
* @retval BMI2_E_DEV_NOT_FOUND - Invalid device
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_init(struct bmi2_dev *dev);
|
||||
|
||||
/**
|
||||
* \ingroup bmi270
|
||||
* \defgroup bmi270ApiSensor Feature Set
|
||||
* @brief Enable / Disable features of the sensor
|
||||
*/
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270ApiSensor
|
||||
* \page bmi270_api_bmi270_sensor_enable bmi270_sensor_enable
|
||||
* \code
|
||||
* int8_t bmi270_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
* \endcode
|
||||
* @details This API selects the sensors/features to be enabled.
|
||||
*
|
||||
* @param[in] sens_list : Pointer to select the sensor/feature.
|
||||
* @param[in] n_sens : Number of sensors selected.
|
||||
* @param[in, out] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @note Sensors/features that can be enabled.
|
||||
*
|
||||
*@verbatim
|
||||
* sens_list | Values
|
||||
* ----------------------------|-----------
|
||||
* BMI2_ACCEL | 0
|
||||
* BMI2_GYRO | 1
|
||||
* BMI2_AUX | 2
|
||||
* BMI2_SIG_MOTION | 3
|
||||
* BMI2_ANY_MOTION | 4
|
||||
* BMI2_NO_MOTION | 5
|
||||
* BMI2_STEP_DETECTOR | 6
|
||||
* BMI2_STEP_COUNTER | 7
|
||||
* BMI2_STEP_ACTIVITY | 8
|
||||
* BMI2_GYRO_GAIN_UPDATE | 9
|
||||
* BMI2_WRIST_GESTURE | 19
|
||||
* BMI2_WRIST_WEAR_WAKE_UP | 20
|
||||
* BMI2_GYRO_SELF_OFF | 33
|
||||
*@endverbatim
|
||||
*
|
||||
* @note :
|
||||
* example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO};
|
||||
* uint8_t n_sens = 2;
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270ApiSensor
|
||||
* \page bmi270_api_bmi270_sensor_disable bmi270_sensor_disable
|
||||
* \code
|
||||
* int8_t bmi270_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
* \endcode
|
||||
* @details This API selects the sensors/features to be disabled.
|
||||
*
|
||||
* @param[in] sens_list : Pointer to select the sensor/feature.
|
||||
* @param[in] n_sens : Number of sensors selected.
|
||||
* @param[in, out] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @note Sensors/features that can be disabled.
|
||||
*
|
||||
*@verbatim
|
||||
* sens_list | Values
|
||||
* ----------------------------|-----------
|
||||
* BMI2_ACCEL | 0
|
||||
* BMI2_GYRO | 1
|
||||
* BMI2_AUX | 2
|
||||
* BMI2_SIG_MOTION | 3
|
||||
* BMI2_ANY_MOTION | 4
|
||||
* BMI2_NO_MOTION | 5
|
||||
* BMI2_STEP_DETECTOR | 6
|
||||
* BMI2_STEP_COUNTER | 7
|
||||
* BMI2_STEP_ACTIVITY | 8
|
||||
* BMI2_GYRO_GAIN_UPDATE | 9
|
||||
* BMI2_WRIST_GESTURE | 19
|
||||
* BMI2_WRIST_WEAR_WAKE_UP | 20
|
||||
* BMI2_GYRO_SELF_OFF | 33
|
||||
*@endverbatim
|
||||
*
|
||||
* @note :
|
||||
* example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO};
|
||||
* uint8_t n_sens = 2;
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
|
||||
/**
|
||||
* \ingroup bmi270
|
||||
* \defgroup bmi270ApiSensorC Sensor Configuration
|
||||
* @brief Enable / Disable feature configuration of the sensor
|
||||
*/
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270ApiSensorC
|
||||
* \page bmi270_api_bmi270_set_sensor_config bmi270_set_sensor_config
|
||||
* \code
|
||||
* int8_t bmi270_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
* \endcode
|
||||
* @details This API sets the sensor/feature configuration.
|
||||
*
|
||||
* @param[in] sens_cfg : Structure instance of bmi2_sens_config.
|
||||
* @param[in] n_sens : Number of sensors selected.
|
||||
* @param[in, out] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @note Sensors/features that can be configured
|
||||
*
|
||||
*@verbatim
|
||||
* sens_list | Values
|
||||
* ----------------------------|-----------
|
||||
* BMI2_SIG_MOTION | 3
|
||||
* BMI2_ANY_MOTION | 4
|
||||
* BMI2_NO_MOTION | 5
|
||||
* BMI2_STEP_DETECTOR | 6
|
||||
* BMI2_STEP_COUNTER | 7
|
||||
* BMI2_STEP_ACTIVITY | 8
|
||||
* BMI2_WRIST_GESTURE | 19
|
||||
* BMI2_WRIST_WEAR_WAKE_UP | 20
|
||||
* BMI2_STEP_COUNTER_PARAMS | 28
|
||||
*@endverbatim
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270ApiSensorC
|
||||
* \page bmi270_api_bmi270_get_sensor_config bmi270_get_sensor_config
|
||||
* \code
|
||||
* int8_t bmi270_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
* \endcode
|
||||
* @details This API gets the sensor/feature configuration.
|
||||
*
|
||||
* @param[in] sens_cfg : Structure instance of bmi2_sens_config.
|
||||
* @param[in] n_sens : Number of sensors selected.
|
||||
* @param[in, out] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @note Sensors/features whose configurations can be read.
|
||||
*
|
||||
*@verbatim
|
||||
* sens_list | Values
|
||||
* ----------------------------|-----------
|
||||
* BMI2_SIG_MOTION | 3
|
||||
* BMI2_ANY_MOTION | 4
|
||||
* BMI2_NO_MOTION | 5
|
||||
* BMI2_STEP_DETECTOR | 6
|
||||
* BMI2_STEP_COUNTER | 7
|
||||
* BMI2_STEP_ACTIVITY | 8
|
||||
* BMI2_WRIST_GESTURE | 19
|
||||
* BMI2_WRIST_WEAR_WAKE_UP | 20
|
||||
* BMI2_STEP_COUNTER_PARAMS | 28
|
||||
*@endverbatim
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
|
||||
/**
|
||||
* \ingroup bmi270
|
||||
* \defgroup bmi270ApiSensorD Sensor Data
|
||||
* @brief Get sensor data
|
||||
*/
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270ApiSensorD
|
||||
* \page bmi270_api_bmi270_get_sensor_data bmi270_get_sensor_data
|
||||
* \code
|
||||
* int8_t bmi270_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
* \endcode
|
||||
* @details This API gets the sensor/feature data for accelerometer, gyroscope,
|
||||
* auxiliary sensor, step counter, high-g, gyroscope user-gain update,
|
||||
* orientation, gyroscope cross sensitivity and error status for NVM and VFRM.
|
||||
*
|
||||
* @param[out] sensor_data : Structure instance of bmi2_sensor_data.
|
||||
* @param[in] n_sens : Number of sensors selected.
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @note Sensors/features whose data can be read
|
||||
*
|
||||
*@verbatim
|
||||
* sens_list | Values
|
||||
* ----------------------------|-----------
|
||||
* BMI2_STEP_COUNTER | 7
|
||||
* BMI2_STEP_ACTIVITY | 8
|
||||
* BMI2_WRIST_GESTURE | 19
|
||||
* BMI2_NVM_STATUS | 38
|
||||
* BMI2_VFRM_STATUS | 39
|
||||
*@endverbatim
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
|
||||
/**
|
||||
* \ingroup bmi270
|
||||
* \defgroup bmi270ApiGyroUG Gyro User Gain
|
||||
* @brief Set / Get Gyro User Gain of the sensor
|
||||
*/
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270ApiGyroUG
|
||||
* \page bmi270_api_bmi270_update_gyro_user_gain bmi270_update_gyro_user_gain
|
||||
* \code
|
||||
* int8_t bmi270_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev);
|
||||
* \endcode
|
||||
* @details This API updates the gyroscope user-gain.
|
||||
*
|
||||
* @param[in] user_gain : Structure that stores user-gain configurations.
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev);
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270ApiGyroUG
|
||||
* \page bmi270_api_bmi270_read_gyro_user_gain bmi270_read_gyro_user_gain
|
||||
* \code
|
||||
* int8_t bmi270_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, const struct bmi2_dev *dev);
|
||||
* \endcode
|
||||
* @details This API reads the compensated gyroscope user-gain values.
|
||||
*
|
||||
* @param[out] gyr_usr_gain : Structure that stores gain values.
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, struct bmi2_dev *dev);
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270ApiInt
|
||||
* \page bmi270_api_bmi270_map_feat_int bmi270_map_feat_int
|
||||
* \code
|
||||
* int8_t bmi270_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev)
|
||||
* \endcode
|
||||
* @details This API maps/unmaps feature interrupts to that of interrupt pins.
|
||||
*
|
||||
* @param[in] sens_int : Structure instance of bmi2_sens_int_config.
|
||||
* @param[in] n_sens : Number of interrupts to be mapped.
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
|
||||
/******************************************************************************/
|
||||
/*! @name C++ Guard Macros */
|
||||
/******************************************************************************/
|
||||
|
|
File diff suppressed because it is too large
Load Diff
|
@ -0,0 +1,493 @@
|
|||
/**
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* BSD-3-Clause
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
*
|
||||
* 3. Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived from
|
||||
* this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
|
||||
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
|
||||
* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
|
||||
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* @file bmi270_context.h
|
||||
* @date 2020-11-04
|
||||
* @version v2.63.1
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
* \ingroup bmi2xy
|
||||
* \defgroup bmi270_context BMI270_CONTEXT
|
||||
* @brief Sensor driver for BMI270_CONTEXT sensor
|
||||
*/
|
||||
|
||||
#ifndef BMI270_CONTEXT_H_
|
||||
#define BMI270_CONTEXT_H_
|
||||
|
||||
/*! CPP guard */
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/***************************************************************************/
|
||||
|
||||
/*! Header files
|
||||
****************************************************************************/
|
||||
#include "bmi2.h"
|
||||
|
||||
/***************************************************************************/
|
||||
|
||||
/*! Macro definitions
|
||||
****************************************************************************/
|
||||
|
||||
/*! @name BMI270_CONTEXT Chip identifier */
|
||||
#define BMI270_CONTEXT_CHIP_ID UINT8_C(0x24)
|
||||
|
||||
/*! @name BMI270_CONTEXT feature input start addresses */
|
||||
#define BMI270_CONTEXT_CONFIG_ID_STRT_ADDR UINT8_C(0x06)
|
||||
#define BMI270_CONTEXT_STEP_CNT_1_STRT_ADDR UINT8_C(0x00)
|
||||
#define BMI270_CONTEXT_STEP_CNT_4_STRT_ADDR UINT8_C(0x02)
|
||||
#define BMI270_CONTEXT_MAX_BURST_LEN_STRT_ADDR UINT8_C(0x08)
|
||||
#define BMI270_CONTEXT_CRT_GYRO_SELF_TEST_STRT_ADDR UINT8_C(0x09)
|
||||
#define BMI270_CONTEXT_ABORT_STRT_ADDR UINT8_C(0x09)
|
||||
#define BMI270_CONTEXT_NVM_PROG_PREP_STRT_ADDR UINT8_C(0x0A)
|
||||
#define BMI270_CONTEXT_ACT_RGN_SETT_STRT_ADDR UINT8_C(0x00)
|
||||
#define BMI270_CONTEXT_ACT_RGN_STRT_ADDR UINT8_C(0x0A)
|
||||
|
||||
/*! @name BMI270_CONTEXT feature output start addresses */
|
||||
#define BMI270_CONTEXT_STEP_CNT_OUT_STRT_ADDR UINT8_C(0x00)
|
||||
#define BMI270_CONTEXT_GYR_USER_GAIN_OUT_STRT_ADDR UINT8_C(0x04)
|
||||
#define BMI270_CONTEXT_GYRO_CROSS_SENSE_STRT_ADDR UINT8_C(0x0C)
|
||||
#define BMI270_CONTEXT_NVM_VFRM_OUT_STRT_ADDR UINT8_C(0x0E)
|
||||
|
||||
/*! @name Defines maximum number of pages */
|
||||
#define BMI270_CONTEXT_MAX_PAGE_NUM UINT8_C(8)
|
||||
|
||||
/*! @name Defines maximum number of feature input configurations */
|
||||
#define BMI270_CONTEXT_MAX_FEAT_IN UINT8_C(10)
|
||||
|
||||
/*! @name Defines maximum number of feature outputs */
|
||||
#define BMI270_CONTEXT_MAX_FEAT_OUT UINT8_C(5)
|
||||
|
||||
/*! @name Mask definitions for feature interrupt status bits */
|
||||
#define BMI270_CONTEXT_STEP_CNT_STATUS_MASK UINT8_C(0x01)
|
||||
|
||||
/*! @name Mask definitions for feature interrupt mapping bits */
|
||||
#define BMI270_C_INT_STEP_COUNTER_MASK UINT8_C(0x01)
|
||||
#define BMI270_C_INT_STEP_DETECTOR_MASK UINT8_C(0x01)
|
||||
|
||||
/*! @name Defines maximum number of feature interrupts */
|
||||
#define BMI270_C_MAX_INT_MAP UINT8_C(2)
|
||||
|
||||
/***************************************************************************/
|
||||
|
||||
/*! BMI270_CONTEXT User Interface function prototypes
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* \ingroup bmi270_context
|
||||
* \defgroup bmi270_contextApiInit Initialization
|
||||
* @brief Initialize the sensor and device structure
|
||||
*/
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270_contextApiInit
|
||||
* \page bmi270_context_api_bmi270_context_init bmi270_context_init
|
||||
* \code
|
||||
* int8_t bmi270_context_init(struct bmi2_dev *dev);
|
||||
* \endcode
|
||||
* @details This API:
|
||||
* 1) updates the device structure with address of the configuration file.
|
||||
* 2) Initializes BMI270_CONTEXT sensor.
|
||||
* 3) Writes the configuration file.
|
||||
* 4) Updates the feature offset parameters in the device structure.
|
||||
* 5) Updates the maximum number of pages, in the device structure.
|
||||
*
|
||||
* @param[in, out] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_context_init(struct bmi2_dev *dev);
|
||||
|
||||
/**
|
||||
* \ingroup bmi270_context
|
||||
* \defgroup bmi270_contextApiSensor Feature Set
|
||||
* @brief Enable / Disable features of the sensor
|
||||
*/
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270_contextApiSensor
|
||||
* \page bmi270_context_api_bmi270_context_sensor_enable bmi270_context_sensor_enable
|
||||
* \code
|
||||
* int8_t bmi270_context_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
* \endcode
|
||||
* @details This API selects the sensors/features to be enabled.
|
||||
*
|
||||
* @param[in] sens_list : Pointer to select the sensor/feature.
|
||||
* @param[in] n_sens : Number of sensors selected.
|
||||
* @param[in, out] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @note Sensors/features that can be enabled.
|
||||
*
|
||||
*@verbatim
|
||||
* sens_list | Values
|
||||
* ----------------------------|-----------
|
||||
* BMI2_ACCEL | 0
|
||||
* BMI2_GYRO | 1
|
||||
* BMI2_AUX | 2
|
||||
* BMI2_STEP_DETECTOR | 6
|
||||
* BMI2_STEP_COUNTER | 7
|
||||
* BMI2_GYRO_GAIN_UPDATE | 9
|
||||
* BMI2_ACTIVITY_RECOGNITION | 34
|
||||
*@endverbatim
|
||||
*
|
||||
* @note :
|
||||
* example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO};
|
||||
* uint8_t n_sens = 2;
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_context_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270_contextApiSensor
|
||||
* \page bmi270_context_api_bmi270_context_sensor_disable bmi270_context_sensor_disable
|
||||
* \code
|
||||
* int8_t bmi270_context_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
* \endcode
|
||||
* @details This API selects the sensors/features to be disabled.
|
||||
*
|
||||
* @param[in] sens_list : Pointer to select the sensor/feature.
|
||||
* @param[in] n_sens : Number of sensors selected.
|
||||
* @param[in, out] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @note Sensors/features that can be disabled.
|
||||
*
|
||||
*@verbatim
|
||||
* sens_list | Values
|
||||
* ----------------------------|-----------
|
||||
* BMI2_ACCEL | 0
|
||||
* BMI2_GYRO | 1
|
||||
* BMI2_AUX | 2
|
||||
* BMI2_STEP_DETECTOR | 6
|
||||
* BMI2_STEP_COUNTER | 7
|
||||
* BMI2_GYRO_GAIN_UPDATE | 9
|
||||
* BMI2_ACTIVITY_RECOGNITION | 34
|
||||
*@endverbatim
|
||||
*
|
||||
* @note :
|
||||
* example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO};
|
||||
* uint8_t n_sens = 2;
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_context_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
|
||||
/**
|
||||
* \ingroup bmi270_context
|
||||
* \defgroup bmi270_contextApiSensorC Sensor Configuration
|
||||
* @brief Enable / Disable feature configuration of the sensor
|
||||
*/
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270_contextApiSensorC
|
||||
* \page bmi270_context_api_bmi270_context_set_sensor_config bmi270_context_set_sensor_config
|
||||
* \code
|
||||
* int8_t bmi270_context_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
* \endcode
|
||||
* @details This API sets the sensor/feature configuration.
|
||||
*
|
||||
* @param[in] sens_cfg : Structure instance of bmi2_sens_config.
|
||||
* @param[in] n_sens : Number of sensors selected.
|
||||
* @param[in, out] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @note Sensors/features that can be configured
|
||||
*
|
||||
*@verbatim
|
||||
* sens_list | Values
|
||||
* ----------------------------|-----------
|
||||
* BMI2_STEP_DETECTOR | 6
|
||||
* BMI2_STEP_COUNTER | 7
|
||||
* BMI2_STEP_COUNTER_PARAMS | 28
|
||||
*@endverbatim
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_context_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270_contextApiSensorC
|
||||
* \page bmi270_context_api_bmi270_context_get_sensor_config bmi270_context_get_sensor_config
|
||||
* \code
|
||||
* int8_t bmi270_context_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
* \endcode
|
||||
* @details This API gets the sensor/feature configuration.
|
||||
*
|
||||
* @param[in] sens_cfg : Structure instance of bmi2_sens_config.
|
||||
* @param[in] n_sens : Number of sensors selected.
|
||||
* @param[in, out] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @note Sensors/features whose configurations can be read.
|
||||
*
|
||||
*@verbatim
|
||||
* sens_list | Values
|
||||
* ----------------------------|-----------
|
||||
* BMI2_STEP_DETECTOR | 6
|
||||
* BMI2_STEP_COUNTER | 7
|
||||
* BMI2_STEP_COUNTER_PARAMS | 28
|
||||
*@endverbatim
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_context_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
|
||||
/**
|
||||
* \ingroup bmi270_context
|
||||
* \defgroup bmi270_contextApiSensorD Sensor Data
|
||||
* @brief Get sensor data
|
||||
*/
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270_contextApiSensorD
|
||||
* \page bmi270_context_api_bmi270_context_get_sensor_data bmi270_context_get_sensor_data
|
||||
* \code
|
||||
* int8_t bmi270_context_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
* \endcode
|
||||
* @details This API gets the sensor/feature data for accelerometer, gyroscope,
|
||||
* auxiliary sensor, step counter, high-g, gyroscope user-gain update,
|
||||
* orientation, gyroscope cross sensitivity and error status for NVM and VFRM.
|
||||
*
|
||||
* @param[out] sensor_data : Structure instance of bmi2_sensor_data.
|
||||
* @param[in] n_sens : Number of sensors selected.
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @note Sensors/features whose data can be read
|
||||
*
|
||||
*@verbatim
|
||||
* sens_list | Values
|
||||
* ---------------------|-----------
|
||||
* BMI2_STEP_COUNTER | 7
|
||||
* BMI2_NVM_STATUS | 38
|
||||
* BMI2_VFRM_STATUS | 39
|
||||
*@endverbatim
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_context_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
|
||||
/**
|
||||
* \ingroup bmi270_context
|
||||
* \defgroup bmi270_contextApiARecog Activity recognition settings
|
||||
* @brief Set / Get activity recognition settings of the sensor
|
||||
*/
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270_contextApiARecog
|
||||
* \page bmi270_context_api_bmi270_context_get_act_recg_sett bmi270_context_get_act_recg_sett
|
||||
* \code
|
||||
* int8_t bmi270_context_get_act_recg_sett(struct bmi2_act_recg_sett *sett, struct bmi2_dev *dev);
|
||||
* \endcode
|
||||
* @details This api is used for retrieving the following activity recognition settings currently set.
|
||||
* enable/disable post processing(0/1) by default -> 1(enable),
|
||||
* Setting the min & max Gini's diversity index (GDI) threshold. min_GDI_tres(0-0XFFFF) by default ->(0x06e1)
|
||||
* max_GDI_tres(0-0xFFFF) by default ->(0x0A66)
|
||||
* buffer size for post processing. range (1-0x0A) default -> (0x0A)
|
||||
* min segment confidence. range (1-0x0A) default -> (0x0A)
|
||||
*
|
||||
* @param[in] sett : Structure instance of bmi2_act_recg_sett.
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_context_get_act_recg_sett(struct bmi2_act_recg_sett *sett, struct bmi2_dev *dev);
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270_contextApiARecog
|
||||
* \page bmi270_context_api_bmi270_context_set_act_recg_sett bmi270_context_set_act_recg_sett
|
||||
* \code
|
||||
* int8_t bmi270_context_set_act_recg_sett(const struct bmi2_act_recg_sett *sett, struct bmi2_dev *dev);
|
||||
* \endcode
|
||||
* @details This api is used for setting the following activity recognition settings
|
||||
* enable/disable post processing(0/1) by default -> 1(enable),
|
||||
* Setting the min & max Gini's diversity index (GDI) threshold. min_GDI_tres(0-0XFFFF) by default ->(0x06e1)
|
||||
* max_GDI_tres(0-0xFFFF) by default ->(0x0A66)
|
||||
* buffer size for post processing. range (1-0x0A) default -> (0x0A)
|
||||
* min segment confidence. range (1-0x0A) default -> (0x0A)
|
||||
*
|
||||
* @param[in] sett : Structure instance of bmi2_act_recg_sett.
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_context_set_act_recg_sett(const struct bmi2_act_recg_sett *sett, struct bmi2_dev *dev);
|
||||
|
||||
/**
|
||||
* \ingroup bmi270_contex
|
||||
* \defgroup bmi270_contextApiactOut Activity Output
|
||||
* @brief Activity output operations of the sensor
|
||||
*/
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270_contextApiactOut
|
||||
* \page bmi270_context_api_bmi270_context_get_act_recog_output bmi270_context_get_act_recog_output
|
||||
* \code
|
||||
* int8_t bmi270_context_get_act_recog_output(struct bmi2_act_recog_output *act_recog,
|
||||
* uint16_t *act_frm_len,
|
||||
* struct bmi2_fifo_frame *fifo,
|
||||
* const struct bmi2_dev *dev);
|
||||
*
|
||||
* \endcode
|
||||
* @details This internal API is used to parse the activity output from the
|
||||
* FIFO in header mode.
|
||||
*
|
||||
* @param[out] act_recog : Pointer to buffer where the parsed activity data
|
||||
* bytes are stored.
|
||||
* @param[in] act_frm_len : Number of activity frames parsed.
|
||||
* @param[in] fifo : Structure instance of bmi2_fifo_frame.
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @verbatim
|
||||
* ----------------------------------------------------------------------------
|
||||
* bmi2_act_rec_output |
|
||||
* Structure parameters | Description
|
||||
*--------------------------|--------------------------------------------------
|
||||
* time_stamp | time-stamp (expressed in 50Hz ticks)
|
||||
* -------------------------|---------------------------------------------------
|
||||
* type | Type of activity
|
||||
* -------------------------|---------------------------------------------------
|
||||
* stat | Activity status
|
||||
* -------------------------|---------------------------------------------------
|
||||
* @endverbatim
|
||||
*
|
||||
*@verbatim
|
||||
* type | Activities
|
||||
*----------|---------------------
|
||||
* 0 | UNKNOWN
|
||||
* 1 | STILL
|
||||
* 2 | WALK
|
||||
* 3 | RUN
|
||||
* 4 | BIKE
|
||||
* 5 | VEHICLE
|
||||
* 6 | TILTED
|
||||
*@endverbatim
|
||||
*
|
||||
*
|
||||
*@verbatim
|
||||
* stat | Activity status
|
||||
*----------|---------------------
|
||||
* 1 | START
|
||||
* 2 | END
|
||||
*@endverbatim
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_context_get_act_recog_output(struct bmi2_act_recog_output *act_recog,
|
||||
uint16_t *act_frm_len,
|
||||
struct bmi2_fifo_frame *fifo,
|
||||
const struct bmi2_dev *dev);
|
||||
|
||||
/**
|
||||
* \ingroup bmi270_contex
|
||||
* \defgroup bmi270_contextApiGyroUG Gyro User Gain
|
||||
* @brief Set / Get Gyro User Gain of the sensor
|
||||
*/
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270_contextApiGyroUG
|
||||
* \page bmi270_context_api_bmi270_context_update_gyro_user_gain bmi270_context_update_gyro_user_gain
|
||||
* \code
|
||||
* int8_t bmi270_context_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev);
|
||||
* \endcode
|
||||
* @details This API updates the gyroscope user-gain.
|
||||
*
|
||||
* @param[in] user_gain : Structure that stores user-gain configurations.
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_context_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev);
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270_contextApiGyroUG
|
||||
* \page bmi270_context_api_bmi270_context_read_gyro_user_gain bmi270_context_read_gyro_user_gain
|
||||
* \code
|
||||
* int8_t bmi270_context_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, const struct bmi2_dev *dev);
|
||||
* \endcode
|
||||
* @details This API reads the compensated gyroscope user-gain values.
|
||||
*
|
||||
* @param[out] gyr_usr_gain : Structure that stores gain values.
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_context_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, struct bmi2_dev *dev);
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270_contextApiInt
|
||||
* \page bmi270_context_api_bmi270_context_map_feat_int bmi270_context_map_feat_int
|
||||
* \code
|
||||
* int8_t bmi270_context_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev)
|
||||
* \endcode
|
||||
* @details This API maps/unmaps feature interrupts to that of interrupt pins.
|
||||
*
|
||||
* @param[in] sens_int : Structure instance of bmi2_sens_int_config.
|
||||
* @param[in] n_sens : Number of interrupts to be mapped.
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_context_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
|
||||
/******************************************************************************/
|
||||
/*! @name C++ Guard Macros */
|
||||
/******************************************************************************/
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif /* End of CPP guard */
|
||||
|
||||
#endif /* BMI270_CONTEXT_H_ */
|
File diff suppressed because it is too large
Load Diff
|
@ -0,0 +1,483 @@
|
|||
/**
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* BSD-3-Clause
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
*
|
||||
* 3. Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived from
|
||||
* this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
|
||||
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
|
||||
* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
|
||||
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* @file bmi270_hc.h
|
||||
* @date 2020-11-04
|
||||
* @version v2.63.1
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
* \ingroup bmi2xy
|
||||
* \defgroup bmi270_hc BMI270_HC
|
||||
* @brief Sensor driver for BMI270_HC sensor
|
||||
*/
|
||||
|
||||
#ifndef BMI270_HC_H_
|
||||
#define BMI270_HC_H_
|
||||
|
||||
/*! CPP guard */
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/***************************************************************************/
|
||||
|
||||
/*! Header files
|
||||
****************************************************************************/
|
||||
#include "bmi2.h"
|
||||
|
||||
/***************************************************************************/
|
||||
|
||||
/*! Macro definitions
|
||||
****************************************************************************/
|
||||
|
||||
/*! @name BMI270_HC Chip identifier */
|
||||
#define BMI270_HC_CHIP_ID UINT8_C(0x24)
|
||||
|
||||
/*! @name BMI270_HC feature input start addresses */
|
||||
#define BMI270_HC_CONFIG_ID_STRT_ADDR UINT8_C(0x06)
|
||||
#define BMI270_HC_STEP_CNT_1_STRT_ADDR UINT8_C(0x00)
|
||||
#define BMI270_HC_STEP_CNT_4_STRT_ADDR UINT8_C(0x02)
|
||||
#define BMI270_HC_MAX_BURST_LEN_STRT_ADDR UINT8_C(0x08)
|
||||
#define BMI270_HC_CRT_GYRO_SELF_TEST_STRT_ADDR UINT8_C(0x09)
|
||||
#define BMI270_HC_ABORT_STRT_ADDR UINT8_C(0x09)
|
||||
#define BMI270_HC_NVM_PROG_PREP_STRT_ADDR UINT8_C(0x0A)
|
||||
#define BMI270_HC_ACT_RGN_SETT_STRT_ADDR UINT8_C(0x02)
|
||||
#define BMI270_HC_ACT_RGN_STRT_ADDR UINT8_C(0x00)
|
||||
|
||||
/*! @name BMI270_HC feature output start addresses */
|
||||
#define BMI270_HC_STEP_CNT_OUT_STRT_ADDR UINT8_C(0x00)
|
||||
#define BMI270_HC_GYR_USER_GAIN_OUT_STRT_ADDR UINT8_C(0x04)
|
||||
#define BMI270_HC_GYRO_CROSS_SENSE_STRT_ADDR UINT8_C(0x0C)
|
||||
#define BMI270_HC_NVM_VFRM_OUT_STRT_ADDR UINT8_C(0x0E)
|
||||
|
||||
/*! @name Defines maximum number of pages */
|
||||
#define BMI270_HC_MAX_PAGE_NUM UINT8_C(8)
|
||||
|
||||
/*! @name Defines maximum number of feature input configurations */
|
||||
#define BMI270_HC_MAX_FEAT_IN UINT8_C(10)
|
||||
|
||||
/*! @name Defines maximum number of feature outputs */
|
||||
#define BMI270_HC_MAX_FEAT_OUT UINT8_C(5)
|
||||
|
||||
/*! @name Mask definitions for feature interrupt status bits */
|
||||
#define BMI270_HC_STEP_CNT_STATUS_MASK UINT8_C(0x01)
|
||||
|
||||
/*! @name Mask definitions for feature interrupt mapping bits */
|
||||
#define BMI270_HC_INT_STEP_COUNTER_MASK UINT8_C(0x01)
|
||||
#define BMI270_HC_INT_STEP_DETECTOR_MASK UINT8_C(0x01)
|
||||
|
||||
/*! @name Defines maximum number of feature interrupts */
|
||||
#define BMI270_HC_MAX_INT_MAP UINT8_C(2)
|
||||
|
||||
/***************************************************************************/
|
||||
|
||||
/*! BMI270_HC User Interface function prototypes
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* \ingroup bmi270_hc
|
||||
* \defgroup bmi270_hcApiInit Initialization
|
||||
* @brief Initialize the sensor and device structure
|
||||
*/
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270_hcApiInit
|
||||
* \page bmi270_hc_api_bmi270_hc_init bmi270_hc_init
|
||||
* \code
|
||||
* int8_t bmi270_hc_init(struct bmi2_dev *dev);
|
||||
* \endcode
|
||||
* @details This API:
|
||||
* 1) updates the device structure with address of the configuration file.
|
||||
* 2) Initializes BMI270_HC sensor.
|
||||
* 3) Writes the configuration file.
|
||||
* 4) Updates the feature offset parameters in the device structure.
|
||||
* 5) Updates the maximum number of pages, in the device structure.
|
||||
*
|
||||
* @param[in, out] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_hc_init(struct bmi2_dev *dev);
|
||||
|
||||
/**
|
||||
* \ingroup bmi270_hc
|
||||
* \defgroup bmi270_hcApiSensor Feature Set
|
||||
* @brief Enable / Disable features of the sensor
|
||||
*/
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270_hcApiSensor
|
||||
* \page bmi270_hc_api_bmi270_hc_sensor_enable bmi270_hc_sensor_enable
|
||||
* \code
|
||||
* int8_t bmi270_hc_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
* \endcode
|
||||
* @details This API selects the sensors/features to be enabled.
|
||||
*
|
||||
* @param[in] sens_list : Pointer to select the sensor/feature.
|
||||
* @param[in] n_sens : Number of sensors selected.
|
||||
* @param[in, out] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @note Sensors/features that can be enabled.
|
||||
*
|
||||
*@verbatim
|
||||
* sens_list | Values
|
||||
* ----------------------------|-----------
|
||||
* BMI2_ACCEL | 0
|
||||
* BMI2_GYRO | 1
|
||||
* BMI2_AUX | 2
|
||||
* BMI2_STEP_DETECTOR | 6
|
||||
* BMI2_STEP_COUNTER | 7
|
||||
* BMI2_GYRO_GAIN_UPDATE | 9
|
||||
* BMI2_ACTIVITY_RECOGNITION | 34
|
||||
*@endverbatim
|
||||
*
|
||||
* @note :
|
||||
* example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO};
|
||||
* uint8_t n_sens = 2;
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_hc_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270_hcApiSensor
|
||||
* \page bmi270_hc_api_bmi270_hc_sensor_disable bmi270_hc_sensor_disable
|
||||
* \code
|
||||
* int8_t bmi270_hc_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
* \endcode
|
||||
* @details This API selects the sensors/features to be disabled.
|
||||
*
|
||||
* @param[in] sens_list : Pointer to select the sensor/feature.
|
||||
* @param[in] n_sens : Number of sensors selected.
|
||||
* @param[in, out] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @note Sensors/features that can be disabled.
|
||||
*
|
||||
*@verbatim
|
||||
* sens_list | Values
|
||||
* ----------------------------|-----------
|
||||
* BMI2_ACCEL | 0
|
||||
* BMI2_GYRO | 1
|
||||
* BMI2_AUX | 2
|
||||
* BMI2_STEP_DETECTOR | 6
|
||||
* BMI2_STEP_COUNTER | 7
|
||||
* BMI2_GYRO_GAIN_UPDATE | 9
|
||||
* BMI2_ACTIVITY_RECOGNITION | 34
|
||||
*@endverbatim
|
||||
*
|
||||
* @note :
|
||||
* example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO};
|
||||
* uint8_t n_sens = 2;
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_hc_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
|
||||
/**
|
||||
* \ingroup bmi270_hc
|
||||
* \defgroup bmi270_hcApiSensorC Sensor Configuration
|
||||
* @brief Enable / Disable feature configuration of the sensor
|
||||
*/
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270_hcApiSensorC
|
||||
* \page bmi270_hc_api_bmi270_hc_set_sensor_config bmi270_hc_set_sensor_config
|
||||
* \code
|
||||
* int8_t bmi270_hc_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
* \endcode
|
||||
* @details This API sets the sensor/feature configuration.
|
||||
*
|
||||
* @param[in] sens_cfg : Structure instance of bmi2_sens_config.
|
||||
* @param[in] n_sens : Number of sensors selected.
|
||||
* @param[in, out] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @note Sensors/features that can be configured
|
||||
*
|
||||
*@verbatim
|
||||
* sens_list | Values
|
||||
* ----------------------------|-----------
|
||||
* BMI2_STEP_DETECTOR | 6
|
||||
* BMI2_STEP_COUNTER | 7
|
||||
* BMI2_STEP_COUNTER_PARAMS | 28
|
||||
*@endverbatim
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_hc_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270_hcApiSensorC
|
||||
* \page bmi270_hc_api_bmi270_hc_get_sensor_config bmi270_hc_get_sensor_config
|
||||
* \code
|
||||
* int8_t bmi270_hc_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
* \endcode
|
||||
* @details This API gets the sensor/feature configuration.
|
||||
*
|
||||
* @param[in] sens_cfg : Structure instance of bmi2_sens_config.
|
||||
* @param[in] n_sens : Number of sensors selected.
|
||||
* @param[in, out] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @note Sensors/features whose configurations can be read.
|
||||
*
|
||||
*@verbatim
|
||||
* sens_list | Values
|
||||
* ----------------------------|-----------
|
||||
* BMI2_STEP_DETECTOR | 6
|
||||
* BMI2_STEP_COUNTER | 7
|
||||
* BMI2_STEP_COUNTER_PARAMS | 28
|
||||
*@endverbatim
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_hc_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
|
||||
/**
|
||||
* \ingroup bmi270_hc
|
||||
* \defgroup bmi270_hcApiSensorD Sensor Data
|
||||
* @brief Get sensor data
|
||||
*/
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270_hcApiSensorD
|
||||
* \page bmi270_hc_api_bmi270_hc_get_sensor_data bmi270_hc_get_sensor_data
|
||||
* \code
|
||||
* int8_t bmi270_hc_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
* \endcode
|
||||
* @details This API gets the sensor/feature data for accelerometer, gyroscope,
|
||||
* auxiliary sensor, step counter, high-g, gyroscope user-gain update,
|
||||
* orientation, gyroscope cross sensitivity and error status for NVM and VFRM.
|
||||
*
|
||||
* @param[out] sensor_data : Structure instance of bmi2_sensor_data.
|
||||
* @param[in] n_sens : Number of sensors selected.
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @note Sensors/features whose data can be read
|
||||
*
|
||||
*@verbatim
|
||||
* sens_list | Values
|
||||
* ---------------------|-----------
|
||||
* BMI2_STEP_COUNTER | 7
|
||||
* BMI2_NVM_STATUS | 38
|
||||
* BMI2_VFRM_STATUS | 39
|
||||
*@endverbatim
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_hc_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
|
||||
/**
|
||||
* \ingroup bmi270_hc
|
||||
* \defgroup bmi270_hcApiARecoghc Activity recognition settings
|
||||
* @brief Set / Get activity recognition settings of bmi270hc sensor
|
||||
*/
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270_hcApiARecoghc
|
||||
* \page bmi270_hc_api_bmi270_hc_get_act_recg_sett bmi270_hc_get_act_recg_sett
|
||||
* \code
|
||||
* int8_t bmi270_hc_get_act_recg_sett(struct bmi2_hc_act_recg_sett *sett, struct bmi2_dev *dev);
|
||||
* \endcode
|
||||
* @details This api is used for retrieving the following activity recognition settings currently set.
|
||||
*
|
||||
* @param[in] sett : Structure instance of bmi2_hc_act_recg_sett.
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_hc_get_act_recg_sett(struct bmi2_hc_act_recg_sett *sett, struct bmi2_dev *dev);
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270_hcApiARecoghc
|
||||
* \page bmi270_hc_api_bmi270_hc_set_act_recg_sett bmi270_hc_set_act_recg_sett
|
||||
* \code
|
||||
* int8_t bmi270_hc_set_act_recg_sett(const struct bmi2_hc_act_recg_sett *sett, struct bmi2_dev *dev);
|
||||
* \endcode
|
||||
* @details This api is used for setting the following activity recognition settings
|
||||
*
|
||||
* @param[in] sett : Structure instance of bmi2_hc_act_recg_sett.
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_hc_set_act_recg_sett(const struct bmi2_hc_act_recg_sett *sett, struct bmi2_dev *dev);
|
||||
|
||||
/**
|
||||
* \ingroup bmi270_hc
|
||||
* \defgroup bmi270_hcApiactOut Activity Output
|
||||
* @brief Activity output operations of the sensor
|
||||
*/
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270_hcApiactOut
|
||||
* \page bmi270_hc_api_bmi270_hc_get_act_recog_output bmi270_hc_get_act_recog_output
|
||||
* \code
|
||||
* int8_t bmi270_hc_get_act_recog_output(struct bmi2_act_recog_output *act_recog,
|
||||
* uint16_t *act_frm_len,
|
||||
* struct bmi2_fifo_frame *fifo,
|
||||
* const struct bmi2_dev *dev);
|
||||
*
|
||||
* \endcode
|
||||
* @details This internal API is used to parse the activity output from the
|
||||
* FIFO in header mode.
|
||||
*
|
||||
* @param[out] act_recog : Pointer to buffer where the parsed activity data
|
||||
* bytes are stored.
|
||||
* @param[in] act_frm_len : Number of activity frames parsed.
|
||||
* @param[in] fifo : Structure instance of bmi2_fifo_frame.
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @verbatim
|
||||
* ----------------------------------------------------------------------------
|
||||
* bmi2_act_rec_output |
|
||||
* Structure parameters | Description
|
||||
*--------------------------|--------------------------------------------------
|
||||
* time_stamp | time-stamp (expressed in 50Hz ticks)
|
||||
* -------------------------|---------------------------------------------------
|
||||
* type | Type of activity
|
||||
* -------------------------|---------------------------------------------------
|
||||
* stat | Activity status
|
||||
* -------------------------|---------------------------------------------------
|
||||
* @endverbatim
|
||||
*
|
||||
*@verbatim
|
||||
* type | Activities
|
||||
*----------|---------------------
|
||||
* 0 | UNKNOWN
|
||||
* 1 | STILL
|
||||
* 2 | WALK
|
||||
* 3 | RUN
|
||||
* 4 | BIKE
|
||||
* 5 | VEHICLE
|
||||
* 6 | TILTED
|
||||
*@endverbatim
|
||||
*
|
||||
*
|
||||
*@verbatim
|
||||
* stat | Activity status
|
||||
*----------|---------------------
|
||||
* 1 | START
|
||||
* 2 | END
|
||||
*@endverbatim
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_hc_get_act_recog_output(struct bmi2_act_recog_output *act_recog,
|
||||
uint16_t *act_frm_len,
|
||||
struct bmi2_fifo_frame *fifo,
|
||||
const struct bmi2_dev *dev);
|
||||
|
||||
/**
|
||||
* \ingroup bmi270_hc
|
||||
* \defgroup bmi270_hcApiGyroUG Gyro User Gain
|
||||
* @brief Set / Get Gyro User Gain of the sensor
|
||||
*/
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270_hcApiGyroUG
|
||||
* \page bmi270_hc_api_bmi270_hc_update_gyro_user_gain bmi270_hc_update_gyro_user_gain
|
||||
* \code
|
||||
* int8_t bmi270_hc_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev);
|
||||
* \endcode
|
||||
* @details This API updates the gyroscope user-gain.
|
||||
*
|
||||
* @param[in] user_gain : Structure that stores user-gain configurations.
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_hc_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev);
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270_hcApiGyroUG
|
||||
* \page bmi270_hc_api_bmi270_hc_read_gyro_user_gain bmi270_hc_read_gyro_user_gain
|
||||
* \code
|
||||
* int8_t bmi270_hc_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, const struct bmi2_dev *dev);
|
||||
* \endcode
|
||||
* @details This API reads the compensated gyroscope user-gain values.
|
||||
*
|
||||
* @param[out] gyr_usr_gain : Structure that stores gain values.
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_hc_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, struct bmi2_dev *dev);
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270_hcApiInt
|
||||
* \page bmi270_hc_api_bmi270_hc_map_feat_int bmi270_hc_map_feat_int
|
||||
* \code
|
||||
* int8_t bmi270_hc_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev)
|
||||
* \endcode
|
||||
* @details This API maps/unmaps feature interrupts to that of interrupt pins.
|
||||
*
|
||||
* @param[in] sens_int : Structure instance of bmi2_sens_int_config.
|
||||
* @param[in] n_sens : Number of interrupts to be mapped.
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_hc_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
|
||||
/******************************************************************************/
|
||||
/*! @name C++ Guard Macros */
|
||||
/******************************************************************************/
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif /* End of CPP guard */
|
||||
|
||||
#endif /* BMI270_HC_H_ */
|
|
@ -0,0 +1,212 @@
|
|||
/**
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* BSD-3-Clause
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
*
|
||||
* 3. Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived from
|
||||
* this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
|
||||
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
|
||||
* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
|
||||
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* @file bmi270_maximum_fifo.c
|
||||
* @date 2020-11-04
|
||||
* @version v2.63.1
|
||||
*
|
||||
*/
|
||||
|
||||
/***************************************************************************/
|
||||
|
||||
/*! Header files
|
||||
****************************************************************************/
|
||||
#include "bmi270_maximum_fifo.h"
|
||||
|
||||
/***************************************************************************/
|
||||
|
||||
/*! Global Variable
|
||||
****************************************************************************/
|
||||
|
||||
/*! @name Global array that stores the configuration file of BMI270 */
|
||||
const uint8_t bmi270_maximum_fifo_config_file[] = {
|
||||
0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x1a, 0x00, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00,
|
||||
0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0x90, 0x32, 0x21, 0x2e, 0x59, 0xf5,
|
||||
0x10, 0x30, 0x21, 0x2e, 0x6a, 0xf5, 0x1a, 0x24, 0x22, 0x00, 0x80, 0x2e, 0x3b, 0x00, 0xc8, 0x2e, 0x44, 0x47, 0x22,
|
||||
0x00, 0x37, 0x00, 0xa4, 0x00, 0xff, 0x0f, 0xd1, 0x00, 0x07, 0xad, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1,
|
||||
0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00,
|
||||
0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x11, 0x24, 0xfc, 0xf5, 0x80, 0x30, 0x40, 0x42, 0x50, 0x50, 0x00, 0x30, 0x12, 0x24, 0xeb,
|
||||
0x00, 0x03, 0x30, 0x00, 0x2e, 0xc1, 0x86, 0x5a, 0x0e, 0xfb, 0x2f, 0x21, 0x2e, 0xfc, 0xf5, 0x13, 0x24, 0x63, 0xf5,
|
||||
0xe0, 0x3c, 0x48, 0x00, 0x22, 0x30, 0xf7, 0x80, 0xc2, 0x42, 0xe1, 0x7f, 0x3a, 0x25, 0xfc, 0x86, 0xf0, 0x7f, 0x41,
|
||||
0x33, 0x98, 0x2e, 0xc2, 0xc4, 0xd6, 0x6f, 0xf1, 0x30, 0xf1, 0x08, 0xc4, 0x6f, 0x11, 0x24, 0xff, 0x03, 0x12, 0x24,
|
||||
0x00, 0xfc, 0x61, 0x09, 0xa2, 0x08, 0x36, 0xbe, 0x2a, 0xb9, 0x13, 0x24, 0x38, 0x00, 0x64, 0xbb, 0xd1, 0xbe, 0x94,
|
||||
0x0a, 0x71, 0x08, 0xd5, 0x42, 0x21, 0xbd, 0x91, 0xbc, 0xd2, 0x42, 0xc1, 0x42, 0x00, 0xb2, 0xfe, 0x82, 0x05, 0x2f,
|
||||
0x50, 0x30, 0x21, 0x2e, 0x21, 0xf2, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0xf0, 0x6f, 0x02, 0x30, 0x02, 0x42, 0x20,
|
||||
0x26, 0xe0, 0x6f, 0x02, 0x31, 0x03, 0x40, 0x9a, 0x0a, 0x02, 0x42, 0xf0, 0x37, 0x05, 0x2e, 0x5e, 0xf7, 0x10, 0x08,
|
||||
0x12, 0x24, 0x1e, 0xf2, 0x80, 0x42, 0x83, 0x84, 0xf1, 0x7f, 0x0a, 0x25, 0x13, 0x30, 0x83, 0x42, 0x3b, 0x82, 0xf0,
|
||||
0x6f, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x12, 0x40, 0x52, 0x42, 0x00, 0x2e, 0x12, 0x40, 0x52, 0x42, 0x3e, 0x84,
|
||||
0x00, 0x40, 0x40, 0x42, 0x7e, 0x82, 0xe1, 0x7f, 0xf2, 0x7f, 0x98, 0x2e, 0x6a, 0xd6, 0x21, 0x30, 0x23, 0x2e, 0x61,
|
||||
0xf5, 0xeb, 0x2c, 0xe1, 0x6f
|
||||
};
|
||||
|
||||
// The rest of this is not needed, avoid compiler errors due to pedantic settings
|
||||
#if false
|
||||
|
||||
/*! @name Global array that stores the feature input configuration of BMI270 */
|
||||
const struct bmi2_feature_config bmi270_maximum_fifo_feat_in[BMI270_MAXIMUM_FIFO_MAX_FEAT_IN] = {
|
||||
|
||||
};
|
||||
|
||||
/*! @name Global array that stores the feature output configuration */
|
||||
const struct bmi2_feature_config bmi270_maximum_fifo_feat_out[BMI270_MAXIMUM_FIFO_MAX_FEAT_OUT] = {
|
||||
|
||||
};
|
||||
|
||||
/******************************************************************************/
|
||||
|
||||
/*! Local Function Prototypes
|
||||
******************************************************************************/
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to validate the device pointer for
|
||||
* null conditions.
|
||||
*
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
static int8_t null_ptr_check(const struct bmi2_dev *dev);
|
||||
|
||||
/***************************************************************************/
|
||||
|
||||
/*! User Interface Definitions
|
||||
****************************************************************************/
|
||||
|
||||
/*!
|
||||
* @brief This API:
|
||||
* 1) updates the device structure with address of the configuration file.
|
||||
* 2) Initializes BMI270 sensor.
|
||||
* 3) Writes the configuration file.
|
||||
* 4) Updates the feature offset parameters in the device structure.
|
||||
* 5) Updates the maximum number of pages, in the device structure.
|
||||
*/
|
||||
int8_t bmi270_maximum_fifo_init(struct bmi2_dev *dev)
|
||||
{
|
||||
/* Variable to define result */
|
||||
int8_t rslt;
|
||||
|
||||
/* Null-pointer check */
|
||||
rslt = null_ptr_check(dev);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Assign chip id of BMI270 */
|
||||
dev->chip_id = BMI270_MAXIMUM_FIFO_CHIP_ID;
|
||||
|
||||
dev->config_size = sizeof(bmi270_maximum_fifo_config_file);
|
||||
|
||||
/* Enable the variant specific features if any */
|
||||
dev->variant_feature = BMI2_GYRO_CROSS_SENS_ENABLE | BMI2_MAXIMUM_FIFO_VARIANT;
|
||||
|
||||
/* An extra dummy byte is read during SPI read */
|
||||
if (dev->intf == BMI2_SPI_INTF)
|
||||
{
|
||||
dev->dummy_byte = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
dev->dummy_byte = 0;
|
||||
}
|
||||
|
||||
/* If configuration file pointer is not assigned any address */
|
||||
if (!dev->config_file_ptr)
|
||||
{
|
||||
/* Give the address of the configuration file array to
|
||||
* the device pointer
|
||||
*/
|
||||
dev->config_file_ptr = bmi270_maximum_fifo_config_file;
|
||||
}
|
||||
|
||||
/* Initialize BMI2 sensor */
|
||||
rslt = bmi2_sec_init(dev);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Assign the offsets of the feature input
|
||||
* configuration to the device structure
|
||||
*/
|
||||
dev->feat_config = bmi270_maximum_fifo_feat_in;
|
||||
|
||||
/* Assign the offsets of the feature output to
|
||||
* the device structure
|
||||
*/
|
||||
dev->feat_output = bmi270_maximum_fifo_feat_out;
|
||||
|
||||
/* Assign the maximum number of pages to the
|
||||
* device structure
|
||||
*/
|
||||
dev->page_max = BMI270_MAXIMUM_FIFO_MAX_PAGE_NUM;
|
||||
|
||||
/* Assign maximum number of input sensors/
|
||||
* features to device structure
|
||||
*/
|
||||
dev->input_sens = BMI270_MAXIMUM_FIFO_MAX_FEAT_IN;
|
||||
|
||||
/* Assign maximum number of output sensors/
|
||||
* features to device structure
|
||||
*/
|
||||
dev->out_sens = BMI270_MAXIMUM_FIFO_MAX_FEAT_OUT;
|
||||
|
||||
/* Get the gyroscope cross axis sensitivity */
|
||||
rslt = bmi2_get_gyro_cross_sense(dev);
|
||||
}
|
||||
}
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/***************************************************************************/
|
||||
|
||||
/*! Local Function Definitions
|
||||
****************************************************************************/
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to validate the device structure pointer for
|
||||
* null conditions.
|
||||
*/
|
||||
static int8_t null_ptr_check(const struct bmi2_dev *dev)
|
||||
{
|
||||
/* Variable to define result */
|
||||
int8_t rslt = BMI2_OK;
|
||||
|
||||
if ((dev == NULL) || (dev->read == NULL) || (dev->write == NULL) || (dev->delay_us == NULL))
|
||||
{
|
||||
/* Device structure pointer is not valid */
|
||||
rslt = BMI2_E_NULL_PTR;
|
||||
}
|
||||
|
||||
return rslt;
|
||||
}
|
||||
#endif
|
|
@ -0,0 +1,117 @@
|
|||
/**
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* BSD-3-Clause
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
*
|
||||
* 3. Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived from
|
||||
* this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
|
||||
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
|
||||
* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
|
||||
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* @file bmi270_maximum_fifo.h
|
||||
* @date 2020-11-04
|
||||
* @version v2.63.1
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
* \ingroup bmi2xy
|
||||
* \defgroup bmi270_maximum_fifo BMI270_MAXIMUM_FIFO
|
||||
* @brief Sensor driver for BMI270_MAXIMUM_FIFO sensor
|
||||
*/
|
||||
|
||||
#ifndef BMI270_MAXIMUM_FIFO_H_
|
||||
#define BMI270_MAXIMUM_FIFO_H_
|
||||
|
||||
/*! CPP guard */
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/***************************************************************************/
|
||||
|
||||
/*! Header files
|
||||
****************************************************************************/
|
||||
#include "bmi2.h"
|
||||
|
||||
/***************************************************************************/
|
||||
|
||||
/*! Macro definitions
|
||||
****************************************************************************/
|
||||
|
||||
/*! @name BMI270 Chip identifier */
|
||||
#define BMI270_MAXIMUM_FIFO_CHIP_ID UINT8_C(0x24)
|
||||
|
||||
/*! @name Defines maximum number of pages */
|
||||
#define BMI270_MAXIMUM_FIFO_MAX_PAGE_NUM UINT8_C(0)
|
||||
|
||||
/*! @name Defines maximum number of feature input configurations */
|
||||
#define BMI270_MAXIMUM_FIFO_MAX_FEAT_IN UINT8_C(0)
|
||||
|
||||
/*! @name Defines maximum number of feature outputs */
|
||||
#define BMI270_MAXIMUM_FIFO_MAX_FEAT_OUT UINT8_C(0)
|
||||
|
||||
/*! @name Mask definitions for feature interrupt status bits */
|
||||
|
||||
/***************************************************************************/
|
||||
|
||||
/*! BMI270 User Interface function prototypes
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* \ingroup bmi270_maximum_fifo
|
||||
* \defgroup bmi270_maximum_fifoApiInit Initialization
|
||||
* @brief Initialize the sensor and device structure
|
||||
*/
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270_maximum_fifoApiInit
|
||||
* \page bmi270_maximum_fifo_api_bmi270_maximum_fifo_init bmi270_maximum_fifo_init
|
||||
* \code
|
||||
* int8_t bmi270_maximum_fifo_init(struct bmi2_dev *dev);
|
||||
* \endcode
|
||||
* @details This API:
|
||||
* 1) updates the device structure with address of the configuration file.
|
||||
* 2) Initializes BMI270 sensor.
|
||||
* 3) Writes the configuration file.
|
||||
* 4) Updates the feature offset parameters in the device structure.
|
||||
* 5) Updates the maximum number of pages, in the device structure.
|
||||
*
|
||||
* @param[in, out] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_maximum_fifo_init(struct bmi2_dev *dev);
|
||||
|
||||
/******************************************************************************/
|
||||
/*! @name C++ Guard Macros */
|
||||
/******************************************************************************/
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif /* End of CPP guard */
|
||||
|
||||
#endif /* BMI270_MAXIMUM_FIFO_H_ */
|
File diff suppressed because it is too large
Load Diff
|
@ -0,0 +1,402 @@
|
|||
/**
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* BSD-3-Clause
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
*
|
||||
* 3. Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived from
|
||||
* this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
|
||||
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
|
||||
* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
|
||||
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* @file bmi270_wh.h
|
||||
* @date 2020-11-04
|
||||
* @version v2.63.1
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
* \ingroup bmi2xy
|
||||
* \defgroup bmi270_wh BMI270_WH
|
||||
* @brief Sensor driver for BMI270_WH sensor
|
||||
*/
|
||||
|
||||
#ifndef BMI270_WH_H_
|
||||
#define BMI270_WH_H_
|
||||
|
||||
/*! CPP guard */
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/***************************************************************************/
|
||||
|
||||
/*! Header files
|
||||
****************************************************************************/
|
||||
#include "bmi2.h"
|
||||
|
||||
/***************************************************************************/
|
||||
|
||||
/*! Macro definitions
|
||||
****************************************************************************/
|
||||
|
||||
/*! @name BMI270_WH Chip identifier */
|
||||
#define BMI270_WH_CHIP_ID UINT8_C(0x24)
|
||||
|
||||
/*! @name BMI270_WH feature input start addresses */
|
||||
#define BMI270_WH_CONFIG_ID_STRT_ADDR UINT8_C(0x00)
|
||||
#define BMI270_WH_MAX_BURST_LEN_STRT_ADDR UINT8_C(0x02)
|
||||
#define BMI270_WH_AXIS_MAP_STRT_ADDR UINT8_C(0x04)
|
||||
#define BMI270_WH_NVM_PROG_PREP_STRT_ADDR UINT8_C(0x05)
|
||||
#define BMI270_WH_GYRO_GAIN_UPDATE_STRT_ADDR UINT8_C(0x06)
|
||||
#define BMI270_WH_ANY_MOT_STRT_ADDR UINT8_C(0x0C)
|
||||
#define BMI270_WH_NO_MOT_STRT_ADDR UINT8_C(0x00)
|
||||
#define BMI270_WH_STEP_CNT_1_STRT_ADDR UINT8_C(0x00)
|
||||
#define BMI270_WH_STEP_CNT_4_STRT_ADDR UINT8_C(0x02)
|
||||
#define BMI270_WH_WRIST_WEAR_WAKE_UP_STRT_ADDR UINT8_C(0x04)
|
||||
|
||||
/*! @name BMI270_WH feature output start addresses */
|
||||
#define BMI270_WH_STEP_CNT_OUT_STRT_ADDR UINT8_C(0x00)
|
||||
#define BMI270_WH_STEP_ACT_OUT_STRT_ADDR UINT8_C(0x04)
|
||||
#define BMI270_WH_WRIST_GEST_OUT_STRT_ADDR UINT8_C(0x06)
|
||||
#define BMI270_WH_GYR_USER_GAIN_OUT_STRT_ADDR UINT8_C(0x08)
|
||||
#define BMI270_WH_GYRO_CROSS_SENSE_STRT_ADDR UINT8_C(0x0C)
|
||||
#define BMI270_WH_NVM_VFRM_OUT_STRT_ADDR UINT8_C(0x0E)
|
||||
|
||||
/*! @name Defines maximum number of pages */
|
||||
#define BMI270_WH_MAX_PAGE_NUM UINT8_C(8)
|
||||
|
||||
/*! @name Defines maximum number of feature input configurations */
|
||||
#define BMI270_WH_MAX_FEAT_IN UINT8_C(12)
|
||||
|
||||
/*! @name Defines maximum number of feature outputs */
|
||||
#define BMI270_WH_MAX_FEAT_OUT UINT8_C(6)
|
||||
|
||||
/*! @name Mask definitions for feature interrupt status bits */
|
||||
#define BMI270_WH_STEP_CNT_STATUS_MASK UINT8_C(0x02)
|
||||
#define BMI270_WH_STEP_ACT_STATUS_MASK UINT8_C(0x04)
|
||||
#define BMI270_WH_WRIST_WEAR_WAKEUP_WH_STATUS_MASK UINT8_C(0x08)
|
||||
#define BMI270_WH_NO_MOT_STATUS_MASK UINT8_C(0x20)
|
||||
#define BMI270_WH_ANY_MOT_STATUS_MASK UINT8_C(0x40)
|
||||
|
||||
/*! @name Mask definitions for feature interrupt mapping bits */
|
||||
#define BMI270_WH_INT_STEP_COUNTER_MASK UINT8_C(0x02)
|
||||
#define BMI270_WH_INT_STEP_DETECTOR_MASK UINT8_C(0x02)
|
||||
#define BMI270_WH_INT_STEP_ACT_MASK UINT8_C(0x04)
|
||||
#define BMI270_WH_INT_WRIST_WEAR_WAKEUP_WH_MASK UINT8_C(0x08)
|
||||
#define BMI270_WH_INT_NO_MOT_MASK UINT8_C(0x20)
|
||||
#define BMI270_WH_INT_ANY_MOT_MASK UINT8_C(0x40)
|
||||
|
||||
/*! @name Defines maximum number of feature interrupts */
|
||||
#define BMI270_WH_MAX_INT_MAP UINT8_C(6)
|
||||
|
||||
/***************************************************************************/
|
||||
|
||||
/*! BMI270_WH User Interface function prototypes
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* \ingroup bmi270_wh
|
||||
* \defgroup bmi270_whApiInit Initialization
|
||||
* @brief Initialize the sensor and device structure
|
||||
*/
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270_whApiInit
|
||||
* \page bmi270_wh_api_bmi270_wh_init bmi270_wh_init
|
||||
* \code
|
||||
* int8_t bmi270_wh_init(struct bmi2_dev *dev);
|
||||
* \endcode
|
||||
* @details This API:
|
||||
* 1) updates the device structure with address of the configuration file.
|
||||
* 2) Initializes BMI270_WH sensor.
|
||||
* 3) Writes the configuration file.
|
||||
* 4) Updates the feature offset parameters in the device structure.
|
||||
* 5) Updates the maximum number of pages, in the device structure.
|
||||
*
|
||||
* @param[in, out] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_wh_init(struct bmi2_dev *dev);
|
||||
|
||||
/**
|
||||
* \ingroup bmi270_wh
|
||||
* \defgroup bmi270_whApiSensor Feature Set
|
||||
* @brief Enable / Disable features of the sensor
|
||||
*/
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270_whApiSensor
|
||||
* \page bmi270_wh_api_bmi270_wh_sensor_enable bmi270_wh_sensor_enable
|
||||
* \code
|
||||
* int8_t bmi270_wh_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
* \endcode
|
||||
* @details This API selects the sensors/features to be enabled.
|
||||
*
|
||||
* @param[in] sens_list : Pointer to select the sensor/feature.
|
||||
* @param[in] n_sens : Number of sensors selected.
|
||||
* @param[in, out] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @note Sensors/features that can be enabled.
|
||||
*
|
||||
*@verbatim
|
||||
* sens_list | Values
|
||||
* ----------------------------|-----------
|
||||
* BMI2_ACCEL | 0
|
||||
* BMI2_GYRO | 1
|
||||
* BMI2_AUX | 2
|
||||
* BMI2_ANY_MOTION | 4
|
||||
* BMI2_NO_MOTION | 5
|
||||
* BMI2_STEP_DETECTOR | 6
|
||||
* BMI2_STEP_COUNTER | 7
|
||||
* BMI2_STEP_ACTIVITY | 8
|
||||
* BMI2_GYRO_GAIN_UPDATE | 9
|
||||
* BMI2_WRIST_WEAR_WAKE_UP_WH | 21
|
||||
*@endverbatim
|
||||
*
|
||||
* @note :
|
||||
* example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO};
|
||||
* uint8_t n_sens = 2;
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_wh_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270_whApiSensor
|
||||
* \page bmi270_wh_api_bmi270_wh_sensor_disable bmi270_wh_sensor_disable
|
||||
* \code
|
||||
* int8_t bmi270_wh_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
* \endcode
|
||||
* @details This API selects the sensors/features to be disabled.
|
||||
*
|
||||
* @param[in] sens_list : Pointer to select the sensor/feature.
|
||||
* @param[in] n_sens : Number of sensors selected.
|
||||
* @param[in, out] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @note Sensors/features that can be disabled.
|
||||
*
|
||||
*@verbatim
|
||||
* sens_list | Values
|
||||
* ----------------------------|-----------
|
||||
* BMI2_ACCEL | 0
|
||||
* BMI2_GYRO | 1
|
||||
* BMI2_AUX | 2
|
||||
* BMI2_ANY_MOTION | 4
|
||||
* BMI2_NO_MOTION | 5
|
||||
* BMI2_STEP_DETECTOR | 6
|
||||
* BMI2_STEP_COUNTER | 7
|
||||
* BMI2_STEP_ACTIVITY | 8
|
||||
* BMI2_GYRO_GAIN_UPDATE | 9
|
||||
* BMI2_WRIST_WEAR_WAKE_UP_WH | 21
|
||||
*@endverbatim
|
||||
*
|
||||
* @note :
|
||||
* example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO};
|
||||
* uint8_t n_sens = 2;
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_wh_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
|
||||
/**
|
||||
* \ingroup bmi270_wh
|
||||
* \defgroup bmi270_whApiSensorC Sensor Configuration
|
||||
* @brief Enable / Disable feature configuration of the sensor
|
||||
*/
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270_whApiSensorC
|
||||
* \page bmi270_wh_api_bmi270_wh_set_sensor_config bmi270_wh_set_sensor_config
|
||||
* \code
|
||||
* int8_t bmi270_wh_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
* \endcode
|
||||
* @details This API sets the sensor/feature configuration.
|
||||
*
|
||||
* @param[in] sens_cfg : Structure instance of bmi2_sens_config.
|
||||
* @param[in] n_sens : Number of sensors selected.
|
||||
* @param[in, out] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @note Sensors/features that can be configured
|
||||
*
|
||||
*@verbatim
|
||||
* sens_list | Values
|
||||
* ----------------------------|-----------
|
||||
* BMI2_ANY_MOTION | 4
|
||||
* BMI2_NO_MOTION | 5
|
||||
* BMI2_STEP_DETECTOR | 6
|
||||
* BMI2_STEP_COUNTER | 7
|
||||
* BMI2_STEP_ACTIVITY | 8
|
||||
* BMI2_WRIST_WEAR_WAKE_UP_WH | 21
|
||||
* BMI2_STEP_COUNTER_PARAMS | 28
|
||||
*@endverbatim
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_wh_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270_whApiSensorC
|
||||
* \page bmi270_wh_api_bmi270_wh_get_sensor_config bmi270_wh_get_sensor_config
|
||||
* \code
|
||||
* int8_t bmi270_wh_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
* \endcode
|
||||
* @details This API gets the sensor/feature configuration.
|
||||
*
|
||||
* @param[in] sens_cfg : Structure instance of bmi2_sens_config.
|
||||
* @param[in] n_sens : Number of sensors selected.
|
||||
* @param[in, out] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @note Sensors/features whose configurations can be read.
|
||||
*
|
||||
*@verbatim
|
||||
* sens_list | Values
|
||||
* ----------------------------|-----------
|
||||
* BMI2_ANY_MOTION | 4
|
||||
* BMI2_NO_MOTION | 5
|
||||
* BMI2_STEP_DETECTOR | 6
|
||||
* BMI2_STEP_COUNTER | 7
|
||||
* BMI2_STEP_ACTIVITY | 8
|
||||
* BMI2_WRIST_WEAR_WAKE_UP_WH | 21
|
||||
* BMI2_STEP_COUNTER_PARAMS | 28
|
||||
*@endverbatim
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_wh_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
|
||||
/**
|
||||
* \ingroup bmi270_wh
|
||||
* \defgroup bmi270_whApiSensorD Sensor Data
|
||||
* @brief Get sensor data
|
||||
*/
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270_whApiSensorD
|
||||
* \page bmi270_wh_api_bmi270_wh_get_sensor_data bmi270_wh_get_sensor_data
|
||||
* \code
|
||||
* int8_t bmi270_wh_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
* \endcode
|
||||
* @details This API gets the sensor/feature data for accelerometer, gyroscope,
|
||||
* auxiliary sensor, step counter, high-g, gyroscope user-gain update,
|
||||
* orientation, gyroscope cross sensitivity and error status for NVM and VFRM.
|
||||
*
|
||||
* @param[out] sensor_data : Structure instance of bmi2_sensor_data.
|
||||
* @param[in] n_sens : Number of sensors selected.
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @note Sensors/features whose data can be read
|
||||
*
|
||||
*@verbatim
|
||||
* sens_list | Values
|
||||
* ---------------------|-----------
|
||||
* BMI2_STEP_COUNTER | 7
|
||||
* BMI2_STEP_ACTIVITY | 8
|
||||
* BMI2_NVM_STATUS | 38
|
||||
* BMI2_VFRM_STATUS | 39
|
||||
*@endverbatim
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_wh_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
|
||||
/**
|
||||
* \ingroup bmi270_wh
|
||||
* \defgroup bmi270_whApiGyroUG Gyro User Gain
|
||||
* @brief Set / Get Gyro User Gain of the sensor
|
||||
*/
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270_whApiGyroUG
|
||||
* \page bmi270_wh_api_bmi270_wh_update_gyro_user_gain bmi270_wh_update_gyro_user_gain
|
||||
* \code
|
||||
* int8_t bmi270_wh_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev);
|
||||
* \endcode
|
||||
* @details This API updates the gyroscope user-gain.
|
||||
*
|
||||
* @param[in] user_gain : Structure that stores user-gain configurations.
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_wh_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev);
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270_whApiGyroUG
|
||||
* \page bmi270_wh_api_bmi270_wh_read_gyro_user_gain bmi270_wh_read_gyro_user_gain
|
||||
* \code
|
||||
* int8_t bmi270_wh_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, const struct bmi2_dev *dev);
|
||||
* \endcode
|
||||
* @details This API reads the compensated gyroscope user-gain values.
|
||||
*
|
||||
* @param[out] gyr_usr_gain : Structure that stores gain values.
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_wh_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, struct bmi2_dev *dev);
|
||||
|
||||
/*!
|
||||
* \ingroup bmi270_whApiInt
|
||||
* \page bmi270_wh_api_bmi270_wh_map_feat_int bmi270_wh_map_feat_int
|
||||
* \code
|
||||
* int8_t bmi270_wh_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev)
|
||||
* \endcode
|
||||
* @details This API maps/unmaps feature interrupts to that of interrupt pins.
|
||||
*
|
||||
* @param[in] sens_int : Structure instance of bmi2_sens_int_config.
|
||||
* @param[in] n_sens : Number of interrupts to be mapped.
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi270_wh_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev);
|
||||
|
||||
/******************************************************************************/
|
||||
/*! @name C++ Guard Macros */
|
||||
/******************************************************************************/
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif /* End of CPP guard */
|
||||
|
||||
#endif /* BMI270_WH_H_ */
|
File diff suppressed because it is too large
Load Diff
|
@ -1,40 +1,42 @@
|
|||
/**
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* BSD-3-Clause
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
*
|
||||
* 3. Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived from
|
||||
* this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
|
||||
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
|
||||
* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
|
||||
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* @file bmi2_ois.c
|
||||
* @date 2020-01-10
|
||||
* @version v2.46.1
|
||||
*
|
||||
*//******************************************************************************/
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* BSD-3-Clause
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
*
|
||||
* 3. Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived from
|
||||
* this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
|
||||
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
|
||||
* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
|
||||
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* @file bmi2_ois.c
|
||||
* @date 2020-05-05
|
||||
* @version v2.23.2
|
||||
*
|
||||
*/
|
||||
|
||||
/******************************************************************************/
|
||||
|
||||
/*! @name Header Files */
|
||||
/******************************************************************************/
|
||||
|
@ -55,11 +57,12 @@
|
|||
* bmi2xy_init(), refer the example ois_accel_gyro.c for more info"
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval zero -> Success / +ve value -> Warning / -ve value -> Error
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
static int8_t get_ois_acc_gyr_data(struct bmi2_ois_sens_axes_data *ois_data,
|
||||
uint8_t reg_addr,
|
||||
const struct bmi2_ois_dev *ois_dev,
|
||||
struct bmi2_ois_dev *ois_dev,
|
||||
int16_t ois_gyr_cross_sens_zx);
|
||||
|
||||
/*!
|
||||
|
@ -69,7 +72,8 @@ static int8_t get_ois_acc_gyr_data(struct bmi2_ois_sens_axes_data *ois_data,
|
|||
* @param[in] ois_dev : Structure instance of bmi2_ois_dev.
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval zero -> Success / +ve value -> Warning / -ve value -> Error
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
static int8_t null_ptr_check(const struct bmi2_ois_dev *ois_dev);
|
||||
|
||||
|
@ -92,10 +96,7 @@ static void comp_gyro_cross_axis_sensitivity(struct bmi2_ois_sens_axes_data *ois
|
|||
* @brief This API reads the data from the given OIS register address of bmi2
|
||||
* sensor.
|
||||
*/
|
||||
int8_t bmi2_ois_get_regs(uint8_t ois_reg_addr,
|
||||
uint8_t *ois_reg_data,
|
||||
uint16_t data_len,
|
||||
const struct bmi2_ois_dev *ois_dev)
|
||||
int8_t bmi2_ois_get_regs(uint8_t ois_reg_addr, uint8_t *ois_reg_data, uint16_t data_len, struct bmi2_ois_dev *ois_dev)
|
||||
{
|
||||
/* Variable to define error */
|
||||
int8_t rslt;
|
||||
|
@ -120,8 +121,8 @@ int8_t bmi2_ois_get_regs(uint8_t ois_reg_addr,
|
|||
ois_reg_addr = (ois_reg_addr | BMI2_OIS_SPI_RD_MASK);
|
||||
|
||||
/* Read from OIS register through OIS interface */
|
||||
rslt = ois_dev->ois_read(ois_dev->dev_id, ois_reg_addr, temp_buf, temp_len);
|
||||
if (rslt == BMI2_OIS_OK)
|
||||
ois_dev->intf_rslt = ois_dev->ois_read(ois_reg_addr, temp_buf, temp_len, ois_dev->intf_ptr);
|
||||
if (ois_dev->intf_rslt == BMI2_INTF_RET_SUCCESS)
|
||||
{
|
||||
/* Read the data from the position next to dummy byte */
|
||||
while (index < data_len)
|
||||
|
@ -147,9 +148,9 @@ int8_t bmi2_ois_get_regs(uint8_t ois_reg_addr,
|
|||
* @brief This API writes data to the given OIS register address of bmi2 sensor.
|
||||
*/
|
||||
int8_t bmi2_ois_set_regs(uint8_t ois_reg_addr,
|
||||
uint8_t *ois_reg_data,
|
||||
const uint8_t *ois_reg_data,
|
||||
uint16_t data_len,
|
||||
const struct bmi2_ois_dev *ois_dev)
|
||||
struct bmi2_ois_dev *ois_dev)
|
||||
{
|
||||
/* Variable to define error */
|
||||
int8_t rslt;
|
||||
|
@ -162,8 +163,8 @@ int8_t bmi2_ois_set_regs(uint8_t ois_reg_addr,
|
|||
ois_reg_addr = (ois_reg_addr & BMI2_OIS_SPI_WR_MASK);
|
||||
|
||||
/* Burst write to OIS register through OIS interface */
|
||||
rslt = ois_dev->ois_write(ois_dev->dev_id, ois_reg_addr, ois_reg_data, data_len);
|
||||
if (rslt != BMI2_OIS_OK)
|
||||
ois_dev->intf_rslt = ois_dev->ois_write(ois_reg_addr, ois_reg_data, data_len, ois_dev->intf_ptr);
|
||||
if (ois_dev->intf_rslt != BMI2_INTF_RET_SUCCESS)
|
||||
{
|
||||
rslt = BMI2_OIS_E_COM_FAIL;
|
||||
}
|
||||
|
@ -180,7 +181,7 @@ int8_t bmi2_ois_set_regs(uint8_t ois_reg_addr,
|
|||
* @brief This API enables/disables accelerometer/gyroscope data read through
|
||||
* OIS interface.
|
||||
*/
|
||||
int8_t bmi2_ois_set_config(const struct bmi2_ois_dev *ois_dev)
|
||||
int8_t bmi2_ois_set_config(struct bmi2_ois_dev *ois_dev)
|
||||
{
|
||||
/* Variable to define error */
|
||||
int8_t rslt;
|
||||
|
@ -324,12 +325,17 @@ int8_t bmi2_ois_read_data(const uint8_t *sens_sel,
|
|||
/*! Local Function Definitions
|
||||
****************************************************************************/
|
||||
|
||||
/*! @cond DOXYGEN_SUPRESS */
|
||||
|
||||
/* Suppressing doxygen warnings triggered for same static function names present across various sensor variant
|
||||
* directories */
|
||||
|
||||
/*!
|
||||
* @brief This internal API gets the accelerometer and the gyroscope data.
|
||||
*/
|
||||
static int8_t get_ois_acc_gyr_data(struct bmi2_ois_sens_axes_data *ois_data,
|
||||
uint8_t reg_addr,
|
||||
const struct bmi2_ois_dev *ois_dev,
|
||||
struct bmi2_ois_dev *ois_dev,
|
||||
int16_t ois_gyr_cross_sens_zx)
|
||||
{
|
||||
/* Variable to define error */
|
||||
|
@ -388,7 +394,7 @@ static int8_t null_ptr_check(const struct bmi2_ois_dev *ois_dev)
|
|||
int8_t rslt = BMI2_OIS_OK;
|
||||
|
||||
if ((ois_dev == NULL) || (ois_dev->ois_read == NULL) || (ois_dev->ois_write == NULL) ||
|
||||
(ois_dev->ois_delay_ms == NULL))
|
||||
(ois_dev->ois_delay_us == NULL))
|
||||
{
|
||||
/* Device structure pointer is NULL */
|
||||
rslt = BMI2_OIS_E_NULL_PTR;
|
||||
|
@ -407,3 +413,5 @@ static void comp_gyro_cross_axis_sensitivity(struct bmi2_ois_sens_axes_data *ois
|
|||
/* Get the compensated gyroscope x-axis */
|
||||
ois_data->x = ois_data->x - (int16_t)(((int32_t)ois_gyr_cross_sens_zx * (int32_t)ois_data->z) / 512);
|
||||
}
|
||||
|
||||
/*! @endcond */
|
||||
|
|
|
@ -1,79 +1,48 @@
|
|||
/**
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* BSD-3-Clause
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
*
|
||||
* 3. Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived from
|
||||
* this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
|
||||
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
|
||||
* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
|
||||
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* @file bmi2_ois.h
|
||||
* @date 2020-01-10
|
||||
* @version v2.46.1
|
||||
*
|
||||
*//**
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* BSD-3-Clause
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
*
|
||||
* 3. Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived from
|
||||
* this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
|
||||
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
|
||||
* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
|
||||
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* @file bmi2_ois.h
|
||||
* @date 10/01/2020
|
||||
* @version
|
||||
*
|
||||
*/
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* BSD-3-Clause
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
*
|
||||
* 3. Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived from
|
||||
* this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
|
||||
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
|
||||
* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
|
||||
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* @file bmi2_ois.h
|
||||
* @date 2020-05-05
|
||||
* @version v2.23.2
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef BMI2_OIS_H_
|
||||
#define BMI2_OIS_H_
|
||||
/**
|
||||
* \ingroup bmi2xy
|
||||
* \defgroup bmi2_ois BMI2_OIS
|
||||
* @brief Sensor driver for BMI2_OIS sensor
|
||||
*/
|
||||
#ifndef _BMI2_OIS_H
|
||||
#define _BMI2_OIS_H
|
||||
|
||||
/*! CPP guard */
|
||||
#ifdef __cplusplus
|
||||
|
@ -95,6 +64,11 @@ extern "C" {
|
|||
/******************************************************************************/
|
||||
/*! @name Macros */
|
||||
/******************************************************************************/
|
||||
|
||||
#ifndef BMI2_INTF_RETURN_TYPE
|
||||
#define BMI2_INTF_RETURN_TYPE int8_t
|
||||
#endif
|
||||
|
||||
/*! @name Utility macros */
|
||||
#define BMI2_OIS_SET_BITS(reg_data, bitname, data) \
|
||||
((reg_data & ~(bitname##_MASK)) | \
|
||||
|
@ -108,58 +82,105 @@ extern "C" {
|
|||
((reg_data & ~(bitname##_MASK)) | \
|
||||
(data & bitname##_MASK))
|
||||
|
||||
#define BMI2_GET_BIT_POS0(reg_data, bitname) (reg_data & (bitname##_MASK))
|
||||
#define BMI2_GET_BIT_POS0(reg_data, bitname) (reg_data & (bitname##_MASK))
|
||||
|
||||
/*! @name For enable and disable */
|
||||
#define BMI2_OIS_ENABLE UINT8_C(1)
|
||||
#define BMI2_OIS_DISABLE UINT8_C(0)
|
||||
|
||||
/*! @name To define sensor interface success code */
|
||||
#define BMI2_INTF_RET_SUCCESS INT8_C(0)
|
||||
|
||||
/*! @name To define success code */
|
||||
#define BMI2_OIS_OK UINT8_C(0)
|
||||
#define BMI2_OIS_OK UINT8_C(0)
|
||||
|
||||
/*! @name To define error codes */
|
||||
#define BMI2_OIS_E_NULL_PTR INT8_C(-1)
|
||||
#define BMI2_OIS_E_COM_FAIL INT8_C(-2)
|
||||
#define BMI2_OIS_E_INVALID_SENSOR INT8_C(-8)
|
||||
#define BMI2_OIS_E_NULL_PTR INT8_C(-1)
|
||||
#define BMI2_OIS_E_COM_FAIL INT8_C(-2)
|
||||
#define BMI2_OIS_E_INVALID_SENSOR INT8_C(-8)
|
||||
|
||||
/*! @name Mask definitions for SPI read/write address for OIS */
|
||||
#define BMI2_OIS_SPI_RD_MASK UINT8_C(0x80)
|
||||
#define BMI2_OIS_SPI_WR_MASK UINT8_C(0x7F)
|
||||
#define BMI2_OIS_SPI_RD_MASK UINT8_C(0x80)
|
||||
#define BMI2_OIS_SPI_WR_MASK UINT8_C(0x7F)
|
||||
|
||||
/*! @name BMI2 OIS data bytes */
|
||||
#define BMI2_OIS_ACC_GYR_NUM_BYTES UINT8_C(6)
|
||||
#define BMI2_OIS_ACC_GYR_NUM_BYTES UINT8_C(6)
|
||||
|
||||
/*! @name Macros to select sensor for OIS data read */
|
||||
#define BMI2_OIS_ACCEL UINT8_C(0x01)
|
||||
#define BMI2_OIS_GYRO UINT8_C(0x02)
|
||||
#define BMI2_OIS_ACCEL UINT8_C(0x01)
|
||||
#define BMI2_OIS_GYRO UINT8_C(0x02)
|
||||
|
||||
/*! @name Macros to define OIS register addresses */
|
||||
#define BMI2_OIS_CONFIG_ADDR UINT8_C(0x40)
|
||||
#define BMI2_OIS_ACC_X_LSB_ADDR UINT8_C(0x0C)
|
||||
#define BMI2_OIS_GYR_X_LSB_ADDR UINT8_C(0x12)
|
||||
#define BMI2_OIS_CONFIG_ADDR UINT8_C(0x40)
|
||||
#define BMI2_OIS_ACC_X_LSB_ADDR UINT8_C(0x0C)
|
||||
#define BMI2_OIS_GYR_X_LSB_ADDR UINT8_C(0x12)
|
||||
|
||||
/*! @name Mask definitions for OIS configurations */
|
||||
#define BMI2_OIS_GYR_EN_MASK UINT8_C(0x40)
|
||||
#define BMI2_OIS_ACC_EN_MASK UINT8_C(0x80)
|
||||
#define BMI2_OIS_GYR_EN_MASK UINT8_C(0x40)
|
||||
#define BMI2_OIS_ACC_EN_MASK UINT8_C(0x80)
|
||||
|
||||
/*! @name Bit Positions for OIS configurations */
|
||||
#define BMI2_OIS_GYR_EN_POS UINT8_C(0x06)
|
||||
#define BMI2_OIS_ACC_EN_POS UINT8_C(0x07)
|
||||
#define BMI2_OIS_GYR_EN_POS UINT8_C(0x06)
|
||||
#define BMI2_OIS_ACC_EN_POS UINT8_C(0x07)
|
||||
|
||||
/*! Low pass filter configuration position and mask */
|
||||
#define BMI2_OIS_LP_FILTER_EN_POS UINT8_C(0x00)
|
||||
#define BMI2_OIS_LP_FILTER_EN_MASK UINT8_C(0x01)
|
||||
#define BMI2_OIS_LP_FILTER_EN_POS UINT8_C(0x00)
|
||||
#define BMI2_OIS_LP_FILTER_EN_MASK UINT8_C(0x01)
|
||||
|
||||
#define BMI2_OIS_LP_FILTER_CONFIG_POS UINT8_C(0x01)
|
||||
#define BMI2_OIS_LP_FILTER_CONFIG_MASK UINT8_C(0x06)
|
||||
#define BMI2_OIS_LP_FILTER_CONFIG_POS UINT8_C(0x01)
|
||||
#define BMI2_OIS_LP_FILTER_CONFIG_MASK UINT8_C(0x06)
|
||||
|
||||
#define BMI2_OIS_LP_FILTER_MUTE_POS UINT8_C(0x05)
|
||||
#define BMI2_OIS_LP_FILTER_MUTE_MASK UINT8_C(0x20)
|
||||
#define BMI2_OIS_LP_FILTER_MUTE_POS UINT8_C(0x05)
|
||||
#define BMI2_OIS_LP_FILTER_MUTE_MASK UINT8_C(0x20)
|
||||
|
||||
/******************************************************************************/
|
||||
/*! @name Function Pointers */
|
||||
/******************************************************************************/
|
||||
/*! For interfacing to the I2C or SPI read and write functions */
|
||||
typedef int8_t (*bmi2_ois_com_fptr_t)(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len);
|
||||
|
||||
/*! For interfacing to the delay function */
|
||||
typedef void (*bmi2_ois_delay_fptr_t)(uint32_t period);
|
||||
/*!
|
||||
* @brief Bus communication function pointer which should be mapped to
|
||||
* the platform specific read functions of the user
|
||||
*
|
||||
* @param[in] reg_addr : Register address from which data is read.
|
||||
* @param[out] reg_data : Pointer to data buffer where read data is stored.
|
||||
* @param[in] len : Number of bytes of data to be read.
|
||||
* @param[in, out] intf_ptr : Void pointer that can enable the linking of descriptors
|
||||
* for interface related call backs.
|
||||
*
|
||||
* retval = BMI2_INTF_RET_SUCCESS -> Success
|
||||
* retval != BMI2_INTF_RET_SUCCESS -> Failure
|
||||
*
|
||||
*/
|
||||
typedef BMI2_INTF_RETURN_TYPE (*bmi2_ois_read_fptr_t)(uint8_t reg_addr, uint8_t *data, uint32_t len, void *intf_ptr);
|
||||
|
||||
/*!
|
||||
* @brief Bus communication function pointer which should be mapped to
|
||||
* the platform specific write functions of the user
|
||||
*
|
||||
* @param[in] reg_addr : Register address to which the data is written.
|
||||
* @param[in] reg_data : Pointer to data buffer in which data to be written
|
||||
* is stored.
|
||||
* @param[in] len : Number of bytes of data to be written.
|
||||
* @param[in, out] intf_ptr : Void pointer that can enable the linking of descriptors
|
||||
* for interface related call backs
|
||||
*
|
||||
* retval = BMI2_INTF_RET_SUCCESS -> Success
|
||||
* retval != BMI2_INTF_RET_SUCCESS -> Failure
|
||||
*
|
||||
*/
|
||||
typedef BMI2_INTF_RETURN_TYPE (*bmi2_ois_write_fptr_t)(uint8_t reg_addr, const uint8_t *data, uint32_t len,
|
||||
void *intf_ptr);
|
||||
|
||||
/*!
|
||||
* @brief Delay function pointer which should be mapped to
|
||||
* delay function of the user
|
||||
*
|
||||
* @param[in] period : Delay in microseconds.
|
||||
* @param[in, out] intf_ptr : Void pointer that can enable the linking of descriptors
|
||||
* for interface related call backs
|
||||
*
|
||||
*/
|
||||
typedef void (*bmi2_ois_delay_fptr_t)(uint32_t period, void *intf_ptr);
|
||||
|
||||
/******************************************************************************/
|
||||
/*! @name Structure Declarations */
|
||||
|
@ -181,21 +202,24 @@ struct bmi2_ois_sens_axes_data
|
|||
/*! @name Structure to define bmi2 OIS sensor configurations */
|
||||
struct bmi2_ois_dev
|
||||
{
|
||||
/*! Device id of BMI2 */
|
||||
uint8_t dev_id;
|
||||
|
||||
/*! Read function pointer */
|
||||
bmi2_ois_com_fptr_t ois_read;
|
||||
bmi2_ois_read_fptr_t ois_read;
|
||||
|
||||
/*! Write function pointer */
|
||||
bmi2_ois_com_fptr_t ois_write;
|
||||
bmi2_ois_write_fptr_t ois_write;
|
||||
|
||||
/*! Delay function pointer */
|
||||
bmi2_ois_delay_fptr_t ois_delay_ms;
|
||||
bmi2_ois_delay_fptr_t ois_delay_us;
|
||||
|
||||
/*! Low pass filter en/dis */
|
||||
uint8_t lp_filter_en;
|
||||
|
||||
/*! Void interface pointer */
|
||||
void *intf_ptr;
|
||||
|
||||
/*! To store interface pointer error */
|
||||
int8_t intf_rslt;
|
||||
|
||||
/*! Low pass filter cut-off frequency */
|
||||
uint8_t lp_filter_config;
|
||||
|
||||
|
@ -213,7 +237,6 @@ struct bmi2_ois_dev
|
|||
|
||||
/*! Gyroscope data axes */
|
||||
struct bmi2_ois_sens_axes_data gyr_data;
|
||||
|
||||
};
|
||||
|
||||
/***************************************************************************/
|
||||
|
@ -221,8 +244,22 @@ struct bmi2_ois_dev
|
|||
/*! BMI2 OIS User Interface function prototypes
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* \ingroup bmi2_ois
|
||||
* \defgroup bmi2_oisApiRegs Registers
|
||||
* @brief Read data from the given OIS register address of bmi2
|
||||
*/
|
||||
|
||||
/*!
|
||||
* @brief This API reads the data from the given OIS register address of bmi2
|
||||
* \ingroup bmi2_oisApiRegs
|
||||
* \page bmi2_ois_api_bmi2_ois_get_regs bmi2_ois_get_regs
|
||||
* \code
|
||||
* int8_t bmi2_ois_get_regs(uint8_t ois_reg_addr,
|
||||
* uint8_t *ois_reg_data,
|
||||
* uint16_t data_len,
|
||||
* const struct bmi2_ois_dev *ois_dev);
|
||||
* \endcode
|
||||
* @details This API reads the data from the given OIS register address of bmi2
|
||||
* sensor.
|
||||
*
|
||||
* @param[in] ois_reg_addr : OIS register address from which data is read.
|
||||
|
@ -231,15 +268,21 @@ struct bmi2_ois_dev
|
|||
* @param[in] ois_dev : Structure instance of bmi2_ois_dev.
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval zero -> Success / -ve value -> Error
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi2_ois_get_regs(uint8_t ois_reg_addr,
|
||||
uint8_t *ois_reg_data,
|
||||
uint16_t data_len,
|
||||
const struct bmi2_ois_dev *ois_dev);
|
||||
int8_t bmi2_ois_get_regs(uint8_t ois_reg_addr, uint8_t *ois_reg_data, uint16_t data_len, struct bmi2_ois_dev *ois_dev);
|
||||
|
||||
/*!
|
||||
* @brief This API writes data to the given OIS register address of bmi2 sensor.
|
||||
* \ingroup bmi2_oisApiRegs
|
||||
* \page bmi2_ois_api_bmi2_ois_set_regs bmi2_ois_set_regs
|
||||
* \code
|
||||
* int8_t bmi2_ois_set_regs(uint8_t ois_reg_addr,
|
||||
* uint8_t *ois_reg_data,
|
||||
* uint16_t data_len,
|
||||
* const struct bmi2_ois_dev *ois_dev);
|
||||
* \endcode
|
||||
* @details This API writes data to the given OIS register address of bmi2 sensor.
|
||||
*
|
||||
* @param[in] ois_reg_addr : OIS register address to which the data is written.
|
||||
* @param[in] ois_reg_data : Pointer to data buffer in which data to be written
|
||||
|
@ -248,26 +291,44 @@ int8_t bmi2_ois_get_regs(uint8_t ois_reg_addr,
|
|||
* @param[in] ois_dev : Structure instance of bmi2_ois_dev.
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval zero -> Success / -ve value -> Error
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi2_ois_set_regs(uint8_t ois_reg_addr,
|
||||
uint8_t *ois_reg_data,
|
||||
const uint8_t *ois_reg_data,
|
||||
uint16_t data_len,
|
||||
const struct bmi2_ois_dev *ois_dev);
|
||||
struct bmi2_ois_dev *ois_dev);
|
||||
|
||||
/**
|
||||
* \ingroup bmi2_ois
|
||||
* \defgroup bmi2_oisApiConfig Status update
|
||||
* @brief Get / Set the status of Enable / Disable accelerometer / gyroscope data read through OIS interface
|
||||
*/
|
||||
|
||||
/*!
|
||||
* @brief This API enables/disables accelerometer/gyroscope data read through
|
||||
* \ingroup bmi2_oisApiConfig
|
||||
* \page bmi2_ois_api_bmi2_ois_set_config bmi2_ois_set_config
|
||||
* \code
|
||||
* int8_t bmi2_ois_set_config(const struct bmi2_ois_dev *ois_dev);
|
||||
* \endcode
|
||||
* @details This API sets the status of enable/disable accelerometer/gyroscope data read through
|
||||
* OIS interface.
|
||||
*
|
||||
* @param[in] ois_dev : Structure instance of bmi2_ois_dev.
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval zero -> Success / -ve value -> Error
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi2_ois_set_config(const struct bmi2_ois_dev *ois_dev);
|
||||
int8_t bmi2_ois_set_config(struct bmi2_ois_dev *ois_dev);
|
||||
|
||||
/*!
|
||||
* @brief This API gets the status of accelerometer/gyroscope enable for data
|
||||
* \ingroup bmi2_oisApiConfig
|
||||
* \page bmi2_ois_api_bmi2_ois_get_config bmi2_ois_get_config
|
||||
* \code
|
||||
* int8_t bmi2_ois_get_config(struct bmi2_ois_dev *ois_dev);
|
||||
* \endcode
|
||||
* @details This API gets the status of accelerometer/gyroscope enable for data
|
||||
* read through OIS interface.
|
||||
*
|
||||
* @param[in, out] ois_dev : Structure instance of bmi2_ois_dev.
|
||||
|
@ -275,12 +336,27 @@ int8_t bmi2_ois_set_config(const struct bmi2_ois_dev *ois_dev);
|
|||
* @note Enabling and disabling is done during OIS structure initialization.
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval zero -> Success / -ve value -> Error
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi2_ois_get_config(struct bmi2_ois_dev *ois_dev);
|
||||
|
||||
/**
|
||||
* \ingroup bmi2_ois
|
||||
* \defgroup bmi2_oisApiRead Data read
|
||||
* @brief Read accelerometer / gyroscope data through OIS interface
|
||||
*/
|
||||
|
||||
/*!
|
||||
* @brief This API reads accelerometer/gyroscope data through OIS interface.
|
||||
* \ingroup bmi2_oisApiRead
|
||||
* \page bmi2_ois_api_bmi2_ois_read_data bmi2_ois_read_data
|
||||
* \code
|
||||
* int8_t bmi2_ois_read_data(const uint8_t *sens_sel,
|
||||
* uint8_t n_sens,
|
||||
* struct bmi2_ois_dev *ois_dev,
|
||||
* int16_t gyr_cross_sens_zx);
|
||||
* \endcode
|
||||
* @details This API reads accelerometer/gyroscope data through OIS interface.
|
||||
*
|
||||
* @param[in] sens_sel : Select sensor whose data is to be read.
|
||||
* @param[in] n_sens : Number of sensors selected.
|
||||
|
@ -288,24 +364,24 @@ int8_t bmi2_ois_get_config(struct bmi2_ois_dev *ois_dev);
|
|||
* @param[in] gyr_cross_sens_zx : Store the gyroscope cross sensitivity values taken from the bmi2xy
|
||||
* (refer bmi2_ois example).
|
||||
*
|
||||
*@verbatim
|
||||
* sens_sel | Value
|
||||
* ---------------|---------------
|
||||
* BMI2_OIS_ACCEL | 0x01
|
||||
* BMI2_OIS_GYRO | 0x02
|
||||
*@endverbatim
|
||||
*
|
||||
* @return Result of API execution status
|
||||
* @retval zero -> Success / -ve value -> Error.
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Fail
|
||||
*/
|
||||
int8_t bmi2_ois_read_data(const uint8_t *sens_sel,
|
||||
uint8_t n_sens,
|
||||
struct bmi2_ois_dev *ois_dev,
|
||||
int16_t gyr_cross_sens_zx);
|
||||
|
||||
/******************************************************************************/
|
||||
/*! @name C++ Guard Macros */
|
||||
/******************************************************************************/
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif /* End of CPP guard */
|
||||
|
||||
#endif /* BMI2_OIS_H_ */
|
||||
#endif /* End of _BMI2_OIS_H */
|
||||
|
|
|
@ -1,177 +0,0 @@
|
|||
#include <stdio.h>
|
||||
#include "bmi2.h"
|
||||
#include "bmi270.h"
|
||||
|
||||
void delay_us(uint32_t period);
|
||||
int8_t i2c_reg_write(uint8_t i2c_addr, uint8_t reg_addr, const uint8_t *reg_data, uint16_t length);
|
||||
int8_t i2c_reg_read(uint8_t i2c_addr, uint8_t reg_addr, uint8_t *reg_data, uint16_t length);
|
||||
int8_t spi_reg_write(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length);
|
||||
int8_t spi_reg_read(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length);
|
||||
void print_rslt(int8_t rslt);
|
||||
|
||||
int main(void)
|
||||
{
|
||||
/* Variable to define rslt */
|
||||
int8_t rslt;
|
||||
struct bmi2_dev dev;
|
||||
|
||||
/*To enable I2C interface*/
|
||||
dev.read = i2c_reg_read;
|
||||
dev.write = i2c_reg_write;
|
||||
dev.delay_us = delay_us;
|
||||
dev.read_write_len = 128;
|
||||
dev.intf = BMI2_I2C_INTERFACE;
|
||||
dev.dev_id = BMI2_I2C_PRIM_ADDR;
|
||||
|
||||
/* To enable SPI interface*/
|
||||
|
||||
/* dev.read = spi_reg_read;
|
||||
* dev.write = spi_reg_write;
|
||||
* dev.delay_us = delay_us;
|
||||
* dev.read_write_len = 4096;
|
||||
* dev.intf = BMI2_SPI_INTERFACE;
|
||||
* dev.dev_id = SPI_CS;
|
||||
* dev.dummy_byte = 1;
|
||||
*/
|
||||
|
||||
dev.config_file_ptr = NULL;
|
||||
|
||||
/* Initialize BMI2 */
|
||||
rslt = bmi270_init(&dev);
|
||||
print_rslt(rslt);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Function that creates a mandatory delay required in some of the APIs such as "bma4_write_config_file",
|
||||
* "bma4_write_regs", "bma4_set_accel_config" and so on.
|
||||
*
|
||||
* @param[in] period_us : the required wait time in microseconds.
|
||||
* @return void.
|
||||
*
|
||||
*/
|
||||
void delay_us(uint32_t period)
|
||||
{
|
||||
/* Wait for a period amount of us*/
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Function for writing the sensor's registers through I2C bus.
|
||||
*
|
||||
* @param[in] i2c_addr : sensor I2C address.
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[in] reg_data : Pointer to the data buffer whose value is to be written.
|
||||
* @param[in] length : No of bytes to write.
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval 0 -> Success
|
||||
* @retval >0 -> Failure Info
|
||||
*
|
||||
*/
|
||||
int8_t i2c_reg_write(uint8_t i2c_addr, uint8_t reg_addr, const uint8_t *reg_data, uint16_t length)
|
||||
{
|
||||
|
||||
/* Write to registers using I2C. Return 0 for a successful execution. */
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Function for reading the sensor's registers through I2C bus.
|
||||
*
|
||||
* @param[in] i2c_addr : Sensor I2C address.
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[out] reg_data : Pointer to the data buffer to store the read data.
|
||||
* @param[in] length : No of bytes to read.
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval 0 -> Success
|
||||
* @retval >0 -> Failure Info
|
||||
*
|
||||
*/
|
||||
int8_t i2c_reg_read(uint8_t i2c_addr, uint8_t reg_addr, uint8_t *reg_data, uint16_t length)
|
||||
{
|
||||
|
||||
/* Read from registers using I2C. Return 0 for a successful execution. */
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Function for writing the sensor's registers through SPI bus.
|
||||
*
|
||||
* @param[in] cs : Chip select to enable the sensor.
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[in] reg_data : Pointer to the data buffer whose data has to be written.
|
||||
* @param[in] length : No of bytes to write.
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval 0 -> Success
|
||||
* @retval >0 -> Failure Info
|
||||
*
|
||||
*/
|
||||
int8_t spi_reg_write(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length)
|
||||
{
|
||||
|
||||
/* Write to registers using SPI. Return 0 for a successful execution. */
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Function for reading the sensor's registers through SPI bus.
|
||||
*
|
||||
* @param[in] cs : Chip select to enable the sensor.
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[out] reg_data : Pointer to the data buffer to store the read data.
|
||||
* @param[in] length : No of bytes to read.
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval 0 -> Success
|
||||
* @retval >0 -> Failure Info
|
||||
*
|
||||
*/
|
||||
int8_t spi_reg_read(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length)
|
||||
{
|
||||
|
||||
/* Read from registers using SPI. Return 0 for a successful execution. */
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Prints the execution status of the APIs.
|
||||
*
|
||||
* @param[in] api_name : name of the API whose execution status has to be printed.
|
||||
* @param[in] rslt : error code returned by the API whose execution status has to be printed.
|
||||
*
|
||||
* @return void.
|
||||
*/
|
||||
void print_rslt(int8_t rslt)
|
||||
{
|
||||
switch (rslt)
|
||||
{
|
||||
case BMI2_OK:
|
||||
|
||||
/* Do nothing */
|
||||
break;
|
||||
case BMI2_E_NULL_PTR:
|
||||
printf("Error [%d] : Null pointer\r\n", rslt);
|
||||
break;
|
||||
case BMI2_E_COM_FAIL:
|
||||
printf("Error [%d] : Communication failure\r\n", rslt);
|
||||
break;
|
||||
case BMI2_E_DEV_NOT_FOUND:
|
||||
printf("Error [%d] : Device not found\r\n", rslt);
|
||||
break;
|
||||
case BMI2_E_INVALID_SENSOR:
|
||||
printf("Error [%d] : Invalid sensor\r\n", rslt);
|
||||
break;
|
||||
case BMI2_E_SELF_TEST_FAIL:
|
||||
printf("Warning [%d] : Self test failed\r\n", rslt);
|
||||
break;
|
||||
case BMI2_E_INVALID_INT_PIN:
|
||||
printf("warning [%d] : invalid int pin\r\n", rslt);
|
||||
break;
|
||||
default:
|
||||
printf("Error [%d] : Unknown error code\r\n", rslt);
|
||||
break;
|
||||
}
|
||||
}
|
|
@ -1,219 +0,0 @@
|
|||
#include <stdio.h>
|
||||
#include "bmi2.h"
|
||||
#include "bmi270.h"
|
||||
|
||||
|
||||
void delay_us(uint32_t period);
|
||||
int8_t i2c_reg_read(uint8_t i2c_add, uint8_t reg_add, uint8_t *regd_data, uint16_t length);
|
||||
int8_t i2c_reg_write(uint8_t i2c_add, uint8_t reg_add, const uint8_t *reg_data, uint16_t length);
|
||||
int8_t spi_reg_read(uint8_t cs, uint8_t reg_add, uint8_t *reg_data, uint16_t length);
|
||||
int8_t spi_reg_write(uint8_t cs, uint8_t reg_add, uint8_t *reg_data, uint16_t length);
|
||||
void print_rslt(int8_t rslt);
|
||||
|
||||
int main(void)
|
||||
{
|
||||
|
||||
struct bmi2_dev dev;
|
||||
|
||||
/*! @name Structure to define type of sensor and their respective data */
|
||||
struct bmi2_sensor_data sensor_data;
|
||||
|
||||
uint8_t sens_sel[2] = { BMI2_ACCEL, BMI2_GYRO };
|
||||
|
||||
/*variable to define rslt*/
|
||||
int8_t rslt;
|
||||
|
||||
/*to enable i2c interface*/
|
||||
dev.read = i2c_reg_read;
|
||||
dev.write = i2c_reg_write;
|
||||
dev.delay_us = delay_us;
|
||||
dev.read_write_len = 128;
|
||||
dev.intf = BMI2_I2C_INTERFACE;
|
||||
dev.dev_id = BMI2_I2C_PRIM_ADDR;
|
||||
|
||||
/*to enable spi interface*/
|
||||
/* dev.read = spi_reg_read;
|
||||
dev.write = spi_reg_write;
|
||||
dev.delay_us = delay_us;
|
||||
dev.read_write_len = 4096;
|
||||
dev.intf = BMI2_SPI_INTERFACE;
|
||||
dev.dev_id = SPI_CS;
|
||||
dev.dummy_byte = 1;
|
||||
*/
|
||||
dev.config_file_ptr = NULL;
|
||||
|
||||
/*to Initialize bmi270*/
|
||||
rslt = bmi270_init(&dev);
|
||||
print_rslt(rslt);
|
||||
|
||||
rslt = bmi2_sensor_enable(&sens_sel[0], 1, &dev);
|
||||
print_rslt(rslt);
|
||||
|
||||
rslt = bmi2_sensor_disable(&sens_sel[1], 1, &dev);
|
||||
print_rslt(rslt);
|
||||
|
||||
sensor_data.type = BMI2_ACCEL;
|
||||
|
||||
dev.delay_us(100000);
|
||||
|
||||
printf("\nbefore CRT Accel x,y,z values\n");
|
||||
|
||||
/* read the accel data before CRT*/
|
||||
rslt = bmi2_get_sensor_data(&sensor_data, 1, &dev);
|
||||
print_rslt(rslt);
|
||||
printf("\nX axes: %d, Y axes: %d, Z axes: %d \n", sensor_data.sens_data.acc.x, sensor_data.sens_data.acc.y, sensor_data.sens_data.acc.z );
|
||||
|
||||
/*brief This API is to run the CRT process*/
|
||||
rslt = bmi2_do_crt(&dev);
|
||||
print_rslt(rslt);
|
||||
if(rslt == BMI2_OK)
|
||||
{
|
||||
printf("\nAfter CRT Accel x,y,z values\n");
|
||||
|
||||
/* read the accel data after CRT*/
|
||||
rslt = bmi2_get_sensor_data(&sensor_data, 1, &dev);
|
||||
print_rslt(rslt);
|
||||
|
||||
printf("X axes: %d, Y axes: %d, Z axes: %d",sensor_data.sens_data.acc.x, sensor_data.sens_data.acc.y, sensor_data.sens_data.acc.z );
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Function that creates a mandatory delay required in some of the APIs such as "bma4_write_config_file",
|
||||
* "bma4_write_regs", "bma4_set_accel_config" and so on.
|
||||
*
|
||||
* @param[in] period_us : the required wait time in microseconds.
|
||||
* @return void.
|
||||
*
|
||||
*/
|
||||
void delay_us(uint32_t period)
|
||||
{
|
||||
/* Wait for a period amount of us*/
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Function for writing the sensor's registers through I2C bus.
|
||||
*
|
||||
* @param[in] i2c_addr : sensor I2C address.
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[in] reg_data : Pointer to the data buffer whose value is to be written.
|
||||
* @param[in] length : No of bytes to write.
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval 0 -> Success
|
||||
* @retval >0 -> Failure Info
|
||||
*
|
||||
*/
|
||||
int8_t i2c_reg_write(uint8_t i2c_addr, uint8_t reg_addr, const uint8_t *reg_data, uint16_t length)
|
||||
{
|
||||
|
||||
/* Write to registers using I2C. Return 0 for a successful execution. */
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Function for reading the sensor's registers through I2C bus.
|
||||
*
|
||||
* @param[in] i2c_addr : Sensor I2C address.
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[out] reg_data : Pointer to the data buffer to store the read data.
|
||||
* @param[in] length : No of bytes to read.
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval 0 -> Success
|
||||
* @retval >0 -> Failure Info
|
||||
*
|
||||
*/
|
||||
int8_t i2c_reg_read(uint8_t i2c_addr, uint8_t reg_addr, uint8_t *reg_data, uint16_t length)
|
||||
{
|
||||
|
||||
/* Read from registers using I2C. Return 0 for a successful execution. */
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Function for writing the sensor's registers through SPI bus.
|
||||
*
|
||||
* @param[in] cs : Chip select to enable the sensor.
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[in] reg_data : Pointer to the data buffer whose data has to be written.
|
||||
* @param[in] length : No of bytes to write.
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval 0 -> Success
|
||||
* @retval >0 -> Failure Info
|
||||
*
|
||||
*/
|
||||
int8_t spi_reg_write(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length)
|
||||
{
|
||||
|
||||
/* Write to registers using SPI. Return 0 for a successful execution. */
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Function for reading the sensor's registers through SPI bus.
|
||||
*
|
||||
* @param[in] cs : Chip select to enable the sensor.
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[out] reg_data : Pointer to the data buffer to store the read data.
|
||||
* @param[in] length : No of bytes to read.
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval 0 -> Success
|
||||
* @retval >0 -> Failure Info
|
||||
*
|
||||
*/
|
||||
int8_t spi_reg_read(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length)
|
||||
{
|
||||
|
||||
/* Read from registers using SPI. Return 0 for a successful execution. */
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Prints the execution status of the APIs.
|
||||
*
|
||||
* @param[in] api_name : name of the API whose execution status has to be printed.
|
||||
* @param[in] rslt : error code returned by the API whose execution status has to be printed.
|
||||
*
|
||||
* @return void.
|
||||
*/
|
||||
void print_rslt(int8_t rslt)
|
||||
{
|
||||
switch (rslt)
|
||||
{
|
||||
case BMI2_OK:
|
||||
|
||||
/* Do nothing */
|
||||
break;
|
||||
case BMI2_E_NULL_PTR:
|
||||
printf("Error [%d] : Null pointer\r\n", rslt);
|
||||
break;
|
||||
case BMI2_E_COM_FAIL:
|
||||
printf("Error [%d] : Communication failure\r\n", rslt);
|
||||
break;
|
||||
case BMI2_E_DEV_NOT_FOUND:
|
||||
printf("Error [%d] : Device not found\r\n", rslt);
|
||||
break;
|
||||
case BMI2_E_INVALID_SENSOR:
|
||||
printf("Error [%d] : Invalid sensor\r\n", rslt);
|
||||
break;
|
||||
case BMI2_E_SELF_TEST_FAIL:
|
||||
printf("Warning [%d] : Self test failed\r\n", rslt);
|
||||
break;
|
||||
case BMI2_E_INVALID_INT_PIN:
|
||||
printf("warning [%d] : invalid int pin\r\n", rslt);
|
||||
break;
|
||||
case BMI2_E_CRT_ERROR:
|
||||
printf("warning[%d] : CRT fail\r\n", rslt);
|
||||
break;
|
||||
case BMI2_E_ABORT_ERROR:
|
||||
printf("warning[%d] : Abort eror\r\n", rslt);
|
||||
break;
|
||||
default:
|
||||
printf("Error [%d] : Unknown error code\r\n", rslt);
|
||||
break;
|
||||
}
|
||||
}
|
|
@ -1,230 +0,0 @@
|
|||
#include <stdio.h>
|
||||
#include "bmi2.h"
|
||||
#include "bmi270.h"
|
||||
|
||||
void delay_us(uint32_t period);
|
||||
int8_t i2c_reg_write(uint8_t i2c_addr, uint8_t reg_addr, const uint8_t *reg_data, uint16_t length);
|
||||
int8_t i2c_reg_read(uint8_t i2c_addr, uint8_t reg_addr, uint8_t *reg_data, uint16_t length);
|
||||
int8_t spi_reg_write(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length);
|
||||
int8_t spi_reg_read(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length);
|
||||
void print_rslt(int8_t rslt);
|
||||
|
||||
int main(void)
|
||||
{
|
||||
/* Variable to define rslt */
|
||||
int8_t rslt;
|
||||
struct bmi2_dev dev;
|
||||
struct bmi2_sens_config config;
|
||||
|
||||
/* Array to select sensors */
|
||||
uint8_t sens_list[2] = { BMI2_ACCEL, BMI2_NO_MOTION };
|
||||
|
||||
/* Variable to get any-motion status */
|
||||
uint16_t no_mot_status = 0;
|
||||
|
||||
/* Select features and their pins to be mapped to */
|
||||
struct bmi2_sens_int_config sens_int = { .type = BMI2_NO_MOTION, .hw_int_pin = BMI2_INT1 };
|
||||
|
||||
|
||||
/*to enable the i2c interface*/
|
||||
dev.read = i2c_reg_read;
|
||||
dev.write = i2c_reg_write;
|
||||
dev.delay_us = delay_us;
|
||||
dev.read_write_len = 128;
|
||||
dev.intf = BMI2_I2C_INTERFACE;
|
||||
dev.dev_id = BMI2_I2C_PRIM_ADDR;
|
||||
|
||||
/*to enable spi interface*/
|
||||
/*
|
||||
* ac_setup_interface(AC_SPI_STD);
|
||||
* dev.read = ac_spi_read;
|
||||
* dev.write = ac_spi_write;
|
||||
* dev.delay_us = delay_us;
|
||||
* dev.read_write_len = 4096;
|
||||
* dev.intf = BMI2_SPI_INTERFACE;
|
||||
* dev.dev_id = SPI_CS;
|
||||
* dev.dummy_byte = 1;
|
||||
*/
|
||||
|
||||
dev.config_file_ptr = NULL;
|
||||
|
||||
/*initialize bmi270 */
|
||||
rslt = bmi270_init(&dev);
|
||||
print_rslt(rslt);
|
||||
|
||||
/* Enable the selected sensors */
|
||||
rslt = bmi2_sensor_enable(sens_list, 2, &dev);
|
||||
print_rslt(rslt);
|
||||
|
||||
/* Configure type of feature */
|
||||
config.type = BMI2_NO_MOTION;
|
||||
|
||||
/* Get default configurations for the type of feature selected */
|
||||
rslt = bmi2_get_sensor_config(&config, 1, &dev);
|
||||
print_rslt(rslt);
|
||||
|
||||
/* Set the new configuration */
|
||||
rslt = bmi2_set_sensor_config(&config, 1, &dev);
|
||||
print_rslt(rslt);
|
||||
|
||||
/* Set the new configuration */
|
||||
rslt = bmi2_map_feat_int(&sens_int, 1, &dev);
|
||||
print_rslt(rslt);
|
||||
|
||||
printf("Do not move the board\n");
|
||||
|
||||
while (1)
|
||||
{
|
||||
/* Clear buffer */
|
||||
no_mot_status = 0;
|
||||
|
||||
dev.delay_us(50000);
|
||||
|
||||
/* Check the interrupt status of the no-motion */
|
||||
rslt = bmi2_get_int_status(&no_mot_status, &dev);
|
||||
print_rslt(rslt);
|
||||
if (no_mot_status & BMI270_NO_MOT_STATUS_MASK)
|
||||
{
|
||||
printf("no_mot_status = %x\n", no_mot_status);
|
||||
printf("No-motion interrupt generated");
|
||||
break;
|
||||
}
|
||||
|
||||
dev.delay_us(50000);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Function that creates a mandatory delay required in some of the APIs such as "bma4_write_config_file",
|
||||
* "bma4_write_regs", "bma4_set_accel_config" and so on.
|
||||
*
|
||||
* @param[in] period_us : the required wait time in microseconds.
|
||||
* @return void.
|
||||
*
|
||||
*/
|
||||
void delay_us(uint32_t period)
|
||||
{
|
||||
/* Wait for a period amount of us*/
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Function for writing the sensor's registers through I2C bus.
|
||||
*
|
||||
* @param[in] i2c_addr : sensor I2C address.
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[in] reg_data : Pointer to the data buffer whose value is to be written.
|
||||
* @param[in] length : No of bytes to write.
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval 0 -> Success
|
||||
* @retval >0 -> Failure Info
|
||||
*
|
||||
*/
|
||||
int8_t i2c_reg_write(uint8_t i2c_addr, uint8_t reg_addr, const uint8_t *reg_data, uint16_t length)
|
||||
{
|
||||
|
||||
/* Write to registers using I2C. Return 0 for a successful execution. */
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Function for reading the sensor's registers through I2C bus.
|
||||
*
|
||||
* @param[in] i2c_addr : Sensor I2C address.
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[out] reg_data : Pointer to the data buffer to store the read data.
|
||||
* @param[in] length : No of bytes to read.
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval 0 -> Success
|
||||
* @retval >0 -> Failure Info
|
||||
*
|
||||
*/
|
||||
int8_t i2c_reg_read(uint8_t i2c_addr, uint8_t reg_addr, uint8_t *reg_data, uint16_t length)
|
||||
{
|
||||
|
||||
/* Read from registers using I2C. Return 0 for a successful execution. */
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Function for writing the sensor's registers through SPI bus.
|
||||
*
|
||||
* @param[in] cs : Chip select to enable the sensor.
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[in] reg_data : Pointer to the data buffer whose data has to be written.
|
||||
* @param[in] length : No of bytes to write.
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval 0 -> Success
|
||||
* @retval >0 -> Failure Info
|
||||
*
|
||||
*/
|
||||
int8_t spi_reg_write(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length)
|
||||
{
|
||||
|
||||
/* Write to registers using SPI. Return 0 for a successful execution. */
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Function for reading the sensor's registers through SPI bus.
|
||||
*
|
||||
* @param[in] cs : Chip select to enable the sensor.
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[out] reg_data : Pointer to the data buffer to store the read data.
|
||||
* @param[in] length : No of bytes to read.
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval 0 -> Success
|
||||
* @retval >0 -> Failure Info
|
||||
*
|
||||
*/
|
||||
int8_t spi_reg_read(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length)
|
||||
{
|
||||
|
||||
/* Read from registers using SPI. Return 0 for a successful execution. */
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Prints the execution status of the APIs.
|
||||
*
|
||||
* @param[in] api_name : name of the API whose execution status has to be printed.
|
||||
* @param[in] rslt : error code returned by the API whose execution status has to be printed.
|
||||
*
|
||||
* @return void.
|
||||
*/
|
||||
void print_rslt(int8_t rslt)
|
||||
{
|
||||
switch (rslt)
|
||||
{
|
||||
case BMI2_OK:
|
||||
|
||||
/* Do nothing */
|
||||
break;
|
||||
case BMI2_E_NULL_PTR:
|
||||
printf("Error [%d] : Null pointer\r\n", rslt);
|
||||
break;
|
||||
case BMI2_E_COM_FAIL:
|
||||
printf("Error [%d] : Communication failure\r\n", rslt);
|
||||
break;
|
||||
case BMI2_E_DEV_NOT_FOUND:
|
||||
printf("Error [%d] : Device not found\r\n", rslt);
|
||||
break;
|
||||
case BMI2_E_INVALID_SENSOR:
|
||||
printf("Error [%d] : Invalid sensor\r\n", rslt);
|
||||
break;
|
||||
case BMI2_E_SELF_TEST_FAIL:
|
||||
printf("Warning [%d] : Self test failed\r\n", rslt);
|
||||
break;
|
||||
case BMI2_E_INVALID_INT_PIN:
|
||||
printf("warning [%d] : invalid int pin\r\n", rslt);
|
||||
break;
|
||||
default:
|
||||
printf("Error [%d] : Unknown error code\r\n", rslt);
|
||||
break;
|
||||
}
|
||||
}
|
|
@ -1,238 +0,0 @@
|
|||
#include <stdio.h>
|
||||
#include "bmi2.h"
|
||||
#include "bmi270.h"
|
||||
|
||||
void delay_us(uint32_t period);
|
||||
int8_t i2c_reg_write(uint8_t i2c_addr, uint8_t reg_addr, const uint8_t *reg_data, uint16_t length);
|
||||
int8_t i2c_reg_read(uint8_t i2c_addr, uint8_t reg_addr, uint8_t *reg_data, uint16_t length);
|
||||
int8_t spi_reg_write(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length);
|
||||
int8_t spi_reg_read(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length);
|
||||
void print_rslt(int8_t rslt);
|
||||
|
||||
int main(void)
|
||||
{
|
||||
struct bmi2_dev dev;
|
||||
struct bmi2_sensor_data sensor_data;
|
||||
struct bmi2_sens_config config;
|
||||
|
||||
int8_t rslt;
|
||||
|
||||
/* Sensor type of sensor to select sensor */
|
||||
uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_STEP_COUNTER };
|
||||
|
||||
/* For status */
|
||||
uint16_t step_status = 0;
|
||||
|
||||
/* Select features and their pins to be mapped to */
|
||||
struct bmi2_sens_int_config sens_int = { .type = BMI2_STEP_COUNTER, .hw_int_pin = BMI2_INT2 };
|
||||
|
||||
/* Sensor type of sensor to get data */
|
||||
sensor_data.type = BMI2_STEP_COUNTER;
|
||||
|
||||
dev.read = i2c_reg_read;
|
||||
dev.write = i2c_reg_write;
|
||||
dev.delay_us = delay_us;
|
||||
dev.read_write_len = 128;
|
||||
dev.intf = BMI2_I2C_INTERFACE;
|
||||
dev.dev_id = BMI2_I2C_PRIM_ADDR;
|
||||
|
||||
/* To enable SPI interface*/
|
||||
/*
|
||||
* dev.read = spi_reg_read;
|
||||
* dev.write = spi_reg_write;
|
||||
* dev.delay_us = delay_us;
|
||||
* dev.read_write_len = 4096;
|
||||
* dev.intf = BMI2_SPI_INTERFACE;
|
||||
* dev.dev_id = SPI_CS;
|
||||
* dev.dummy_byte = 1;
|
||||
*/
|
||||
|
||||
/* Initialize by enabling firmware download */
|
||||
|
||||
dev.config_file_ptr = NULL;
|
||||
|
||||
rslt = bmi270_init(&dev);
|
||||
print_rslt(rslt);
|
||||
|
||||
/* Enable the accelerometer and step-counter sensor */
|
||||
rslt = bmi2_sensor_enable(sensor_sel, 2, &dev);
|
||||
print_rslt(rslt);
|
||||
|
||||
/* Update the type of sensor for setting the configurations */
|
||||
config.type = BMI2_STEP_COUNTER;
|
||||
|
||||
/* Get default configurations for the type of feature selected */
|
||||
rslt = bmi2_get_sensor_config(&config, 1, &dev);
|
||||
print_rslt(rslt);
|
||||
|
||||
/* Enable water-mark level for to get interrupt after 20 step counts */
|
||||
config.cfg.step_counter.watermark_level = 1;
|
||||
|
||||
/* Set the configurations */
|
||||
rslt = bmi2_set_sensor_config(&config, 1, &dev);
|
||||
print_rslt(rslt);
|
||||
|
||||
/* Map the feature interrupt */
|
||||
rslt = bmi2_map_feat_int(&sens_int, 1, &dev);
|
||||
print_rslt(rslt);
|
||||
|
||||
printf("Move the board for step counter\n");
|
||||
|
||||
while (1)
|
||||
{
|
||||
/* Check the interrupt status of the step counter */
|
||||
rslt = bmi2_get_int_status(&step_status, &dev);
|
||||
print_rslt(rslt);
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
if (step_status == BMI270_STEP_CNT_STATUS_MASK)
|
||||
{
|
||||
printf("Step detected\n");
|
||||
|
||||
/* Get step counter output */
|
||||
rslt = bmi2_get_sensor_data(&sensor_data, 1, &dev);
|
||||
print_rslt(rslt);
|
||||
|
||||
/* Print the step counter output */
|
||||
printf("No of steps counted = %ld", sensor_data.sens_data.step_counter_output);
|
||||
break;
|
||||
}
|
||||
}
|
||||
dev.delay_us(50000);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Function that creates a mandatory delay required in some of the APIs such as "bma4_write_config_file",
|
||||
* "bma4_write_regs", "bma4_set_accel_config" and so on.
|
||||
*
|
||||
* @param[in] period_us : the required wait time in microseconds.
|
||||
* @return void.
|
||||
*
|
||||
*/
|
||||
void delay_us(uint32_t period)
|
||||
{
|
||||
/* Wait for a period amount of us*/
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Function for writing the sensor's registers through I2C bus.
|
||||
*
|
||||
* @param[in] i2c_addr : sensor I2C address.
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[in] reg_data : Pointer to the data buffer whose value is to be written.
|
||||
* @param[in] length : No of bytes to write.
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval 0 -> Success
|
||||
* @retval >0 -> Failure Info
|
||||
*
|
||||
*/
|
||||
int8_t i2c_reg_write(uint8_t i2c_addr, uint8_t reg_addr, const uint8_t *reg_data, uint16_t length)
|
||||
{
|
||||
|
||||
/* Write to registers using I2C. Return 0 for a successful execution. */
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Function for reading the sensor's registers through I2C bus.
|
||||
*
|
||||
* @param[in] i2c_addr : Sensor I2C address.
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[out] reg_data : Pointer to the data buffer to store the read data.
|
||||
* @param[in] length : No of bytes to read.
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval 0 -> Success
|
||||
* @retval >0 -> Failure Info
|
||||
*
|
||||
*/
|
||||
int8_t i2c_reg_read(uint8_t i2c_addr, uint8_t reg_addr, uint8_t *reg_data, uint16_t length)
|
||||
{
|
||||
|
||||
/* Read from registers using I2C. Return 0 for a successful execution. */
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Function for writing the sensor's registers through SPI bus.
|
||||
*
|
||||
* @param[in] cs : Chip select to enable the sensor.
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[in] reg_data : Pointer to the data buffer whose data has to be written.
|
||||
* @param[in] length : No of bytes to write.
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval 0 -> Success
|
||||
* @retval >0 -> Failure Info
|
||||
*
|
||||
*/
|
||||
int8_t spi_reg_write(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length)
|
||||
{
|
||||
|
||||
/* Write to registers using SPI. Return 0 for a successful execution. */
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Function for reading the sensor's registers through SPI bus.
|
||||
*
|
||||
* @param[in] cs : Chip select to enable the sensor.
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[out] reg_data : Pointer to the data buffer to store the read data.
|
||||
* @param[in] length : No of bytes to read.
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval 0 -> Success
|
||||
* @retval >0 -> Failure Info
|
||||
*
|
||||
*/
|
||||
int8_t spi_reg_read(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length)
|
||||
{
|
||||
|
||||
/* Read from registers using SPI. Return 0 for a successful execution. */
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Prints the execution status of the APIs.
|
||||
*
|
||||
* @param[in] api_name : name of the API whose execution status has to be printed.
|
||||
* @param[in] rslt : error code returned by the API whose execution status has to be printed.
|
||||
*
|
||||
* @return void.
|
||||
*/
|
||||
void print_rslt(int8_t rslt)
|
||||
{
|
||||
switch (rslt)
|
||||
{
|
||||
case BMI2_OK:
|
||||
|
||||
/* Do nothing */
|
||||
break;
|
||||
case BMI2_E_NULL_PTR:
|
||||
printf("Error [%d] : Null pointer\r\n", rslt);
|
||||
break;
|
||||
case BMI2_E_COM_FAIL:
|
||||
printf("Error [%d] : Communication failure\r\n", rslt);
|
||||
break;
|
||||
case BMI2_E_DEV_NOT_FOUND:
|
||||
printf("Error [%d] : Device not found\r\n", rslt);
|
||||
break;
|
||||
case BMI2_E_INVALID_SENSOR:
|
||||
printf("Error [%d] : Invalid sensor\r\n", rslt);
|
||||
break;
|
||||
case BMI2_E_SELF_TEST_FAIL:
|
||||
printf("Warning [%d] : Self test failed\r\n", rslt);
|
||||
break;
|
||||
case BMI2_E_INVALID_INT_PIN:
|
||||
printf("warning [%d] : invalid int pin\r\n", rslt);
|
||||
break;
|
||||
default:
|
||||
printf("Error [%d] : Unknown error code\r\n", rslt);
|
||||
break;
|
||||
}
|
||||
}
|
|
@ -1,244 +0,0 @@
|
|||
#include <stdio.h>
|
||||
#include "bmi2.h"
|
||||
#include "bmi270.h"
|
||||
|
||||
|
||||
void delay_us(uint32_t period);
|
||||
int8_t i2c_reg_write(uint8_t i2c_addr, uint8_t reg_addr, const uint8_t *reg_data, uint16_t length);
|
||||
int8_t i2c_reg_read(uint8_t i2c_addr, uint8_t reg_addr, uint8_t *reg_data, uint16_t length);
|
||||
int8_t spi_reg_write(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length);
|
||||
int8_t spi_reg_read(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length);
|
||||
void print_rslt(int8_t rslt);
|
||||
|
||||
int main(void)
|
||||
{
|
||||
|
||||
struct bmi2_dev dev;
|
||||
|
||||
/* Variable to define rslt */
|
||||
int8_t rslt;
|
||||
|
||||
/* Array to select sensors */
|
||||
uint8_t sens_list[2] = { BMI2_ACCEL, BMI2_WRIST_GESTURE };
|
||||
|
||||
/* Variable to get wrist gesture status */
|
||||
uint16_t wrist_gest_status = 0;
|
||||
|
||||
/* Select features and their pins to be mapped to */
|
||||
struct bmi2_sens_int_config sens_int = { .type = BMI2_WRIST_GESTURE, .hw_int_pin = BMI2_INT1 };
|
||||
|
||||
/* Sensor configuration structure */
|
||||
struct bmi2_sens_config config = { 0 };
|
||||
|
||||
/* Sensor data structure */
|
||||
struct bmi2_sensor_data sens_data = { 0 };
|
||||
|
||||
/*To enable I2C interface*/
|
||||
dev.read = i2c_reg_read;
|
||||
dev.write = i2c_reg_write;
|
||||
dev.delay_us = delay_us;
|
||||
dev.read_write_len = 128;
|
||||
dev.intf = BMI2_I2C_INTERFACE;
|
||||
dev.dev_id = BMI2_I2C_PRIM_ADDR;
|
||||
|
||||
/* To enable SPI interface*/
|
||||
|
||||
/*
|
||||
* dev.read = spi_reg_read;
|
||||
* dev.write = spi_reg_write;
|
||||
* dev.delay_us = delay_us;
|
||||
* dev.read_write_len = 4096;
|
||||
* dev.intf = BMI2_SPI_INTERFACE;
|
||||
* dev.dev_id = SPI_CS;
|
||||
* dev.dummy_byte = 1;
|
||||
*/
|
||||
|
||||
dev.config_file_ptr = NULL;
|
||||
|
||||
/* Initialize BMI270 */
|
||||
rslt = bmi270_init(&dev);
|
||||
print_rslt(rslt);
|
||||
|
||||
/* Enable the selected sensors */
|
||||
rslt = bmi2_sensor_enable(sens_list, 2, &dev);
|
||||
print_rslt(rslt);
|
||||
|
||||
/* Configure type of feature */
|
||||
config.type = BMI2_WRIST_GESTURE;
|
||||
|
||||
/* Get default configurations for the type of feature selected */
|
||||
rslt = bmi2_get_sensor_config(&config, 1, &dev);
|
||||
print_rslt(rslt);
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Arm Position where the device is attached */
|
||||
config.cfg.wrist_gest.wearable_arm = BMI2_ARM_LEFT;
|
||||
|
||||
/* Set the new configuration along with interrupt mapping */
|
||||
rslt = bmi2_set_sensor_config(&config, 1, &dev);
|
||||
print_rslt(rslt);
|
||||
}
|
||||
|
||||
/* Map the feature interrupt */
|
||||
rslt = bmi2_map_feat_int(&sens_int, 1, &dev);
|
||||
print_rslt(rslt);
|
||||
printf("Flip the board in portrait landscape mode:\n");
|
||||
while (1)
|
||||
{
|
||||
/* Check the interrupt status of the wrist gesture */
|
||||
rslt = bmi2_get_int_status(&wrist_gest_status, &dev);
|
||||
print_rslt(rslt);
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
if (wrist_gest_status & BMI270_WRIST_GEST_STATUS_MASK)
|
||||
{
|
||||
printf("Wrist gesture detected\n");
|
||||
|
||||
/* Get wrist gesture output */
|
||||
rslt = bmi2_get_sensor_data(&sens_data, 1, &dev);
|
||||
print_rslt(rslt);
|
||||
|
||||
/* Print the wrist gesture output */
|
||||
printf("wrist gesture = %d\r\n", sens_data.sens_data.wrist_gesture_output);
|
||||
break;
|
||||
}
|
||||
|
||||
dev.delay_us(100000);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Function that creates a mandatory delay required in some of the APIs such as "bma4_write_config_file",
|
||||
* "bma4_write_regs", "bma4_set_accel_config" and so on.
|
||||
*
|
||||
* @param[in] period_us : the required wait time in microseconds.
|
||||
* @return void.
|
||||
*
|
||||
*/
|
||||
void delay_us(uint32_t period)
|
||||
{
|
||||
/* Wait for a period amount of us*/
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Function for writing the sensor's registers through I2C bus.
|
||||
*
|
||||
* @param[in] i2c_addr : sensor I2C address.
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[in] reg_data : Pointer to the data buffer whose value is to be written.
|
||||
* @param[in] length : No of bytes to write.
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval 0 -> Success
|
||||
* @retval >0 -> Failure Info
|
||||
*
|
||||
*/
|
||||
int8_t i2c_reg_write(uint8_t i2c_addr, uint8_t reg_addr, const uint8_t *reg_data, uint16_t length)
|
||||
{
|
||||
|
||||
/* Write to registers using I2C. Return 0 for a successful execution. */
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Function for reading the sensor's registers through I2C bus.
|
||||
*
|
||||
* @param[in] i2c_addr : Sensor I2C address.
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[out] reg_data : Pointer to the data buffer to store the read data.
|
||||
* @param[in] length : No of bytes to read.
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval 0 -> Success
|
||||
* @retval >0 -> Failure Info
|
||||
*
|
||||
*/
|
||||
int8_t i2c_reg_read(uint8_t i2c_addr, uint8_t reg_addr, uint8_t *reg_data, uint16_t length)
|
||||
{
|
||||
|
||||
/* Read from registers using I2C. Return 0 for a successful execution. */
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Function for writing the sensor's registers through SPI bus.
|
||||
*
|
||||
* @param[in] cs : Chip select to enable the sensor.
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[in] reg_data : Pointer to the data buffer whose data has to be written.
|
||||
* @param[in] length : No of bytes to write.
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval 0 -> Success
|
||||
* @retval >0 -> Failure Info
|
||||
*
|
||||
*/
|
||||
int8_t spi_reg_write(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length)
|
||||
{
|
||||
|
||||
/* Write to registers using SPI. Return 0 for a successful execution. */
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Function for reading the sensor's registers through SPI bus.
|
||||
*
|
||||
* @param[in] cs : Chip select to enable the sensor.
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[out] reg_data : Pointer to the data buffer to store the read data.
|
||||
* @param[in] length : No of bytes to read.
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval 0 -> Success
|
||||
* @retval >0 -> Failure Info
|
||||
*
|
||||
*/
|
||||
int8_t spi_reg_read(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length)
|
||||
{
|
||||
|
||||
/* Read from registers using SPI. Return 0 for a successful execution. */
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Prints the execution status of the APIs.
|
||||
*
|
||||
* @param[in] api_name : name of the API whose execution status has to be printed.
|
||||
* @param[in] rslt : error code returned by the API whose execution status has to be printed.
|
||||
*
|
||||
* @return void.
|
||||
*/
|
||||
void print_rslt(int8_t rslt)
|
||||
{
|
||||
switch (rslt)
|
||||
{
|
||||
case BMI2_OK:
|
||||
|
||||
/* Do nothing */
|
||||
break;
|
||||
case BMI2_E_NULL_PTR:
|
||||
printf("Error [%d] : Null pointer\r\n", rslt);
|
||||
break;
|
||||
case BMI2_E_COM_FAIL:
|
||||
printf("Error [%d] : Communication failure\r\n", rslt);
|
||||
break;
|
||||
case BMI2_E_DEV_NOT_FOUND:
|
||||
printf("Error [%d] : Device not found\r\n", rslt);
|
||||
break;
|
||||
case BMI2_E_INVALID_SENSOR:
|
||||
printf("Error [%d] : Invalid sensor\r\n", rslt);
|
||||
break;
|
||||
case BMI2_E_SELF_TEST_FAIL:
|
||||
printf("Warning [%d] : Self test failed\r\n", rslt);
|
||||
break;
|
||||
case BMI2_E_INVALID_INT_PIN:
|
||||
printf("warning [%d] : invalid int pin\r\n", rslt);
|
||||
break;
|
||||
default:
|
||||
printf("Error [%d] : Unknown error code\r\n", rslt);
|
||||
break;
|
||||
}
|
||||
}
|
|
@ -1,235 +0,0 @@
|
|||
#include <stdio.h>
|
||||
#include "bmi2.h"
|
||||
#include "bmi270.h"
|
||||
|
||||
void delay_us(uint32_t period);
|
||||
int8_t i2c_reg_write(uint8_t i2c_addr, uint8_t reg_addr, const uint8_t *reg_data, uint16_t length);
|
||||
int8_t i2c_reg_read(uint8_t i2c_addr, uint8_t reg_addr, uint8_t *reg_data, uint16_t length);
|
||||
int8_t spi_reg_write(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length);
|
||||
int8_t spi_reg_read(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length);
|
||||
void print_rslt(int8_t rslt);
|
||||
|
||||
int main(void)
|
||||
{
|
||||
struct bmi2_dev dev;
|
||||
|
||||
/* Variable to define rslt */
|
||||
int8_t rslt;
|
||||
|
||||
/* Array to select sensors */
|
||||
uint8_t sens_list[2] = { BMI2_ACCEL, BMI2_WRIST_WEAR_WAKE_UP };
|
||||
|
||||
/* Variable to get wrist gesture status */
|
||||
uint16_t wrist_wear_wakeup_status = 0;
|
||||
|
||||
/* Select features and their pins to be mapped to */
|
||||
struct bmi2_sens_int_config sens_int = { .type = BMI2_WRIST_WEAR_WAKE_UP, .hw_int_pin = BMI2_INT1 };
|
||||
|
||||
/* Sensor configuration structure */
|
||||
struct bmi2_sens_config config = { 0 };
|
||||
|
||||
/* Sensor data structure */
|
||||
struct bmi2_sensor_data sens_data = { 0 };
|
||||
|
||||
dev.read = i2c_reg_read;
|
||||
dev.write = i2c_reg_write;
|
||||
dev.delay_us = delay_us;
|
||||
dev.read_write_len = 128;
|
||||
dev.intf = BMI2_I2C_INTERFACE;
|
||||
dev.dev_id = BMI2_I2C_PRIM_ADDR;
|
||||
|
||||
/* To enable SPI interface*/
|
||||
/* dev.read = spi_reg_read;
|
||||
* dev.write = spi_reg_write;
|
||||
* dev.delay_us = delay_us;
|
||||
* dev.read_write_len = 4096;
|
||||
* dev.intf = BMI2_SPI_INTERFACE;
|
||||
* dev.dev_id = SPI_CS;
|
||||
* dev.dummy_byte = 1;
|
||||
*/
|
||||
dev.config_file_ptr = NULL;
|
||||
|
||||
/* Initialize BMI2 */
|
||||
rslt = bmi270_init(&dev);
|
||||
print_rslt(rslt);
|
||||
|
||||
/* Enable the selected sensors */
|
||||
rslt = bmi2_sensor_enable(sens_list, 2, &dev);
|
||||
print_rslt(rslt);
|
||||
|
||||
/* Configure type of feature */
|
||||
config.type = BMI2_WRIST_WEAR_WAKE_UP;
|
||||
|
||||
/* Get default configurations for the type of feature selected */
|
||||
rslt = bmi2_get_sensor_config(&config, 1, &dev);
|
||||
print_rslt(rslt);
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Set the new configuration along with interrupt mapping */
|
||||
rslt = bmi2_set_sensor_config(&config, 1, &dev);
|
||||
print_rslt(rslt);
|
||||
}
|
||||
|
||||
/* Map the feature interrupt */
|
||||
rslt = bmi2_map_feat_int(&sens_int, 1, &dev);
|
||||
print_rslt(rslt);
|
||||
printf("Lift the board in portrait landscape position and tilt in a particular direction\n");
|
||||
while (1)
|
||||
{
|
||||
/* Check the interrupt status of the wrist gesture */
|
||||
rslt = bmi2_get_int_status(&wrist_wear_wakeup_status, &dev);
|
||||
print_rslt(rslt);
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
if (wrist_wear_wakeup_status & BMI270_WRIST_WAKE_UP_STATUS_MASK)
|
||||
{
|
||||
printf("Wrist wear wakeup detected\n");
|
||||
|
||||
/* Get wrist gesture output */
|
||||
rslt = bmi2_get_sensor_data(&sens_data, 1, &dev);
|
||||
print_rslt(rslt);
|
||||
|
||||
/* Print the wrist gesture output */
|
||||
printf("wrist gesture = %d\r\n", sens_data.sens_data.wrist_gesture_output);
|
||||
break;
|
||||
}
|
||||
|
||||
dev.delay_us(100000);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Function that creates a mandatory delay required in some of the APIs such as "bma4_write_config_file",
|
||||
* "bma4_write_regs", "bma4_set_accel_config" and so on.
|
||||
*
|
||||
* @param[in] period_us : the required wait time in microseconds.
|
||||
* @return void.
|
||||
*
|
||||
*/
|
||||
void delay_us(uint32_t period)
|
||||
{
|
||||
/* Wait for a period amount of ms*/
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Function for writing the sensor's registers through I2C bus.
|
||||
*
|
||||
* @param[in] i2c_addr : sensor I2C address.
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[in] reg_data : Pointer to the data buffer whose value is to be written.
|
||||
* @param[in] length : No of bytes to write.
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval 0 -> Success
|
||||
* @retval >0 -> Failure Info
|
||||
*
|
||||
*/
|
||||
int8_t i2c_reg_write(uint8_t i2c_addr, uint8_t reg_addr, const uint8_t *reg_data, uint16_t length)
|
||||
{
|
||||
|
||||
/* Write to registers using I2C. Return 0 for a successful execution. */
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Function for reading the sensor's registers through I2C bus.
|
||||
*
|
||||
* @param[in] i2c_addr : Sensor I2C address.
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[out] reg_data : Pointer to the data buffer to store the read data.
|
||||
* @param[in] length : No of bytes to read.
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval 0 -> Success
|
||||
* @retval >0 -> Failure Info
|
||||
*
|
||||
*/
|
||||
int8_t i2c_reg_read(uint8_t i2c_addr, uint8_t reg_addr, uint8_t *reg_data, uint16_t length)
|
||||
{
|
||||
|
||||
/* Read from registers using I2C. Return 0 for a successful execution. */
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Function for writing the sensor's registers through SPI bus.
|
||||
*
|
||||
* @param[in] cs : Chip select to enable the sensor.
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[in] reg_data : Pointer to the data buffer whose data has to be written.
|
||||
* @param[in] length : No of bytes to write.
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval 0 -> Success
|
||||
* @retval >0 -> Failure Info
|
||||
*
|
||||
*/
|
||||
int8_t spi_reg_write(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length)
|
||||
{
|
||||
|
||||
/* Write to registers using SPI. Return 0 for a successful execution. */
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Function for reading the sensor's registers through SPI bus.
|
||||
*
|
||||
* @param[in] cs : Chip select to enable the sensor.
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[out] reg_data : Pointer to the data buffer to store the read data.
|
||||
* @param[in] length : No of bytes to read.
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval 0 -> Success
|
||||
* @retval >0 -> Failure Info
|
||||
*
|
||||
*/
|
||||
int8_t spi_reg_read(uint8_t cs, uint8_t reg_addr, uint8_t *reg_data, uint16_t length)
|
||||
{
|
||||
|
||||
/* Read from registers using SPI. Return 0 for a successful execution. */
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Prints the execution status of the APIs.
|
||||
*
|
||||
* @param[in] api_name : name of the API whose execution status has to be printed.
|
||||
* @param[in] rslt : error code returned by the API whose execution status has to be printed.
|
||||
*
|
||||
* @return void.
|
||||
*/
|
||||
void print_rslt(int8_t rslt)
|
||||
{
|
||||
switch (rslt)
|
||||
{
|
||||
case BMI2_OK:
|
||||
|
||||
/* Do nothing */
|
||||
break;
|
||||
case BMI2_E_NULL_PTR:
|
||||
printf("Error [%d] : Null pointer\r\n", rslt);
|
||||
break;
|
||||
case BMI2_E_COM_FAIL:
|
||||
printf("Error [%d] : Communication failure\r\n", rslt);
|
||||
break;
|
||||
case BMI2_E_DEV_NOT_FOUND:
|
||||
printf("Error [%d] : Device not found\r\n", rslt);
|
||||
break;
|
||||
case BMI2_E_INVALID_SENSOR:
|
||||
printf("Error [%d] : Invalid sensor\r\n", rslt);
|
||||
break;
|
||||
case BMI2_E_SELF_TEST_FAIL:
|
||||
printf("Warning [%d] : Self test failed\r\n", rslt);
|
||||
break;
|
||||
case BMI2_E_INVALID_INT_PIN:
|
||||
printf("warning [%d] : invalid int pin\r\n", rslt);
|
||||
break;
|
||||
default:
|
||||
printf("Error [%d] : Unknown error code\r\n", rslt);
|
||||
break;
|
||||
}
|
||||
}
|
|
@ -0,0 +1,18 @@
|
|||
COINES_INSTALL_PATH ?= ../../../..
|
||||
|
||||
EXAMPLE_FILE ?= accel.c
|
||||
|
||||
API_LOCATION ?= ../..
|
||||
|
||||
COMMON_LOCATION ?= ..
|
||||
|
||||
C_SRCS += \
|
||||
$(API_LOCATION)/bmi2.c \
|
||||
$(API_LOCATION)/bmi270.c \
|
||||
$(COMMON_LOCATION)/common/common.c
|
||||
|
||||
INCLUDEPATHS += \
|
||||
$(API_LOCATION) \
|
||||
$(COMMON_LOCATION)/common
|
||||
|
||||
include $(COINES_INSTALL_PATH)/coines.mk
|
|
@ -0,0 +1,200 @@
|
|||
/**\
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
**/
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Header Files */
|
||||
#include <stdio.h>
|
||||
#include "bmi270.h"
|
||||
#include "common.h"
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Macro definition */
|
||||
|
||||
/*! Earth's gravity in m/s^2 */
|
||||
#define GRAVITY_EARTH (9.80665f)
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Static Function Declaration */
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for accel.
|
||||
*
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @return Status of execution.
|
||||
*/
|
||||
static int8_t set_accel_config(struct bmi2_dev *bmi2_dev);
|
||||
|
||||
/*!
|
||||
* @brief This function converts lsb to meter per second squared for 16 bit accelerometer at
|
||||
* range 2G, 4G, 8G or 16G.
|
||||
*
|
||||
* @param[in] val : LSB from each axis.
|
||||
* @param[in] g_range : Gravity range.
|
||||
* @param[in] bit_width : Resolution for accel.
|
||||
*
|
||||
* @return Gravity.
|
||||
*/
|
||||
static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width);
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Functions */
|
||||
|
||||
/* This function starts the execution of program. */
|
||||
int main(void)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Variable to define limit to print accel data. */
|
||||
uint8_t limit = 20;
|
||||
|
||||
/* Assign accel sensor to variable. */
|
||||
uint8_t sensor_list = BMI2_ACCEL;
|
||||
|
||||
/* Sensor initialization configuration. */
|
||||
struct bmi2_dev bmi2_dev;
|
||||
|
||||
/* Create an instance of sensor data structure. */
|
||||
struct bmi2_sensor_data sensor_data = { 0 };
|
||||
|
||||
/* Initialize the interrupt status of accel. */
|
||||
uint16_t int_status = 0;
|
||||
|
||||
uint8_t indx = 0;
|
||||
float x = 0, y = 0, z = 0;
|
||||
|
||||
/* Interface reference is given as a parameter
|
||||
* For I2C : BMI2_I2C_INTF
|
||||
* For SPI : BMI2_SPI_INTF
|
||||
*/
|
||||
rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initialize bmi270. */
|
||||
rslt = bmi270_init(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Enable the accel sensor. */
|
||||
rslt = bmi270_sensor_enable(&sensor_list, 1, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Accel configuration settings. */
|
||||
rslt = set_accel_config(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Assign accel sensor. */
|
||||
sensor_data.type = BMI2_ACCEL;
|
||||
printf("Accel and m/s2 data \n");
|
||||
printf("Accel data collected at 2G Range with 16-bit resolution\n");
|
||||
|
||||
/* Loop to print the accel data when interrupt occurs. */
|
||||
while (indx <= limit)
|
||||
{
|
||||
/* To get the status of accel data ready interrupt. */
|
||||
rslt = bmi2_get_int_status(&int_status, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* To check the accel data ready interrupt status and print the status for 10 samples. */
|
||||
if (int_status & BMI2_ACC_DRDY_INT_MASK)
|
||||
{
|
||||
/* Get accelerometer data for x, y and z axis. */
|
||||
rslt = bmi270_get_sensor_data(&sensor_data, 1, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
printf("\nAcc_X = %d\t", sensor_data.sens_data.acc.x);
|
||||
printf("Acc_Y = %d\t", sensor_data.sens_data.acc.y);
|
||||
printf("Acc_Z = %d\r\n", sensor_data.sens_data.acc.z);
|
||||
|
||||
/* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */
|
||||
x = lsb_to_mps2(sensor_data.sens_data.acc.x, 2, bmi2_dev.resolution);
|
||||
y = lsb_to_mps2(sensor_data.sens_data.acc.y, 2, bmi2_dev.resolution);
|
||||
z = lsb_to_mps2(sensor_data.sens_data.acc.z, 2, bmi2_dev.resolution);
|
||||
|
||||
/* Print the data in m/s2. */
|
||||
printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z);
|
||||
|
||||
indx++;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bmi2_coines_deinit();
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for accel.
|
||||
*/
|
||||
static int8_t set_accel_config(struct bmi2_dev *bmi2_dev)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Structure to define accelerometer configuration. */
|
||||
struct bmi2_sens_config config;
|
||||
|
||||
/* Configure the type of feature. */
|
||||
config.type = BMI2_ACCEL;
|
||||
|
||||
/* Get default configurations for the type of feature selected. */
|
||||
rslt = bmi270_get_sensor_config(&config, 1, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* NOTE: The user can change the following configuration parameters according to their requirement. */
|
||||
/* Set Output Data Rate */
|
||||
config.cfg.acc.odr = BMI2_ACC_ODR_200HZ;
|
||||
|
||||
/* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */
|
||||
config.cfg.acc.range = BMI2_ACC_RANGE_2G;
|
||||
|
||||
/* The bandwidth parameter is used to configure the number of sensor samples that are averaged
|
||||
* if it is set to 2, then 2^(bandwidth parameter) samples
|
||||
* are averaged, resulting in 4 averaged samples.
|
||||
* Note1 : For more information, refer the datasheet.
|
||||
* Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but
|
||||
* this has an adverse effect on the power consumed.
|
||||
*/
|
||||
config.cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4;
|
||||
|
||||
/* Enable the filter performance mode where averaging of samples
|
||||
* will be done based on above set bandwidth and ODR.
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode
|
||||
* 1 -> High performance mode(Default)
|
||||
* For more info refer datasheet.
|
||||
*/
|
||||
config.cfg.acc.filter_perf = BMI2_PERF_OPT_MODE;
|
||||
|
||||
/* Set the accel configurations. */
|
||||
rslt = bmi270_set_sensor_config(&config, 1, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Map data ready interrupt to interrupt pin. */
|
||||
rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
}
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This function converts lsb to meter per second squared for 16 bit accelerometer at
|
||||
* range 2G, 4G, 8G or 16G.
|
||||
*/
|
||||
static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width)
|
||||
{
|
||||
float half_scale = ((float)(1 << bit_width) / 2.0f);
|
||||
|
||||
return (GRAVITY_EARTH * val * g_range) / half_scale;
|
||||
}
|
|
@ -0,0 +1,18 @@
|
|||
COINES_INSTALL_PATH ?= ../../../..
|
||||
|
||||
EXAMPLE_FILE ?= accel_gyro.c
|
||||
|
||||
API_LOCATION ?= ../..
|
||||
|
||||
COMMON_LOCATION ?= ..
|
||||
|
||||
C_SRCS += \
|
||||
$(API_LOCATION)/bmi2.c \
|
||||
$(API_LOCATION)/bmi270.c \
|
||||
$(COMMON_LOCATION)/common/common.c
|
||||
|
||||
INCLUDEPATHS += \
|
||||
$(API_LOCATION) \
|
||||
$(COMMON_LOCATION)/common
|
||||
|
||||
include $(COINES_INSTALL_PATH)/coines.mk
|
|
@ -0,0 +1,268 @@
|
|||
/**\
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
**/
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Header Files */
|
||||
#include <stdio.h>
|
||||
#include "bmi270.h"
|
||||
#include "common.h"
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Macro definition */
|
||||
|
||||
/*! Earth's gravity in m/s^2 */
|
||||
#define GRAVITY_EARTH (9.80665f)
|
||||
|
||||
/*! Macros to select the sensors */
|
||||
#define ACCEL UINT8_C(0x00)
|
||||
#define GYRO UINT8_C(0x01)
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Static Function Declaration */
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for accel.
|
||||
*
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @return Status of execution.
|
||||
*/
|
||||
static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev);
|
||||
|
||||
/*!
|
||||
* @brief This function converts lsb to meter per second squared for 16 bit accelerometer at
|
||||
* range 2G, 4G, 8G or 16G.
|
||||
*
|
||||
* @param[in] val : LSB from each axis.
|
||||
* @param[in] g_range : Gravity range.
|
||||
* @param[in] bit_width : Resolution for accel.
|
||||
*
|
||||
* @return Gravity.
|
||||
*/
|
||||
static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width);
|
||||
|
||||
/*!
|
||||
* @brief This function converts lsb to degree per second for 16 bit gyro at
|
||||
* range 125, 250, 500, 1000 or 2000dps.
|
||||
*
|
||||
* @param[in] val : LSB from each axis.
|
||||
* @param[in] dps : Degree per second.
|
||||
* @param[in] bit_width : Resolution for gyro.
|
||||
*
|
||||
* @return Degree per second.
|
||||
*/
|
||||
static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width);
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Functions */
|
||||
|
||||
/* This function starts the execution of program. */
|
||||
int main(void)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Variable to define limit to print accel data. */
|
||||
uint8_t limit = 10;
|
||||
|
||||
/* Assign accel and gyro sensor to variable. */
|
||||
uint8_t sensor_list[2] = { BMI2_ACCEL, BMI2_GYRO };
|
||||
|
||||
/* Sensor initialization configuration. */
|
||||
struct bmi2_dev bmi2_dev;
|
||||
|
||||
/* Create an instance of sensor data structure. */
|
||||
struct bmi2_sensor_data sensor_data[2] = { { 0 } };
|
||||
|
||||
/* Initialize the interrupt status of accel and gyro. */
|
||||
uint16_t int_status = 0;
|
||||
|
||||
uint8_t indx = 1;
|
||||
|
||||
float x = 0, y = 0, z = 0;
|
||||
|
||||
/* Interface reference is given as a parameter
|
||||
* For I2C : BMI2_I2C_INTF
|
||||
* For SPI : BMI2_SPI_INTF
|
||||
*/
|
||||
rslt = bmi2_interface_init(&bmi2_dev, BMI2_SPI_INTF);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initialize bmi270. */
|
||||
rslt = bmi270_init(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Enable the accel and gyro sensor. */
|
||||
rslt = bmi270_sensor_enable(sensor_list, 2, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Accel and gyro configuration settings. */
|
||||
rslt = set_accel_gyro_config(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Assign accel and gyro sensor. */
|
||||
sensor_data[ACCEL].type = BMI2_ACCEL;
|
||||
sensor_data[GYRO].type = BMI2_GYRO;
|
||||
|
||||
/* Loop to print accel and gyro data when interrupt occurs. */
|
||||
while (indx <= limit)
|
||||
{
|
||||
/* To get the data ready interrupt status of accel and gyro. */
|
||||
rslt = bmi2_get_int_status(&int_status, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* To check the data ready interrupt status and print the status for 10 samples. */
|
||||
if ((int_status & BMI2_ACC_DRDY_INT_MASK) && (int_status & BMI2_GYR_DRDY_INT_MASK))
|
||||
{
|
||||
/* Get accel and gyro data for x, y and z axis. */
|
||||
rslt = bmi270_get_sensor_data(sensor_data, 2, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
printf("\n******* Accel(Raw and m/s2) Gyro(Raw and dps) data : %d *******\n", indx);
|
||||
|
||||
printf("\nAcc_x = %d\t", sensor_data[ACCEL].sens_data.acc.x);
|
||||
printf("Acc_y = %d\t", sensor_data[ACCEL].sens_data.acc.y);
|
||||
printf("Acc_z = %d", sensor_data[ACCEL].sens_data.acc.z);
|
||||
|
||||
/* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */
|
||||
x = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.x, 2, bmi2_dev.resolution);
|
||||
y = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.y, 2, bmi2_dev.resolution);
|
||||
z = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.z, 2, bmi2_dev.resolution);
|
||||
|
||||
/* Print the data in m/s2. */
|
||||
printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z);
|
||||
|
||||
printf("\nGyr_X = %d\t", sensor_data[GYRO].sens_data.gyr.x);
|
||||
printf("Gyr_Y = %d\t", sensor_data[GYRO].sens_data.gyr.y);
|
||||
printf("Gyr_Z= %d\n", sensor_data[GYRO].sens_data.gyr.z);
|
||||
|
||||
/* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */
|
||||
x = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.x, 2000, bmi2_dev.resolution);
|
||||
y = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.y, 2000, bmi2_dev.resolution);
|
||||
z = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.z, 2000, bmi2_dev.resolution);
|
||||
|
||||
/* Print the data in dps. */
|
||||
printf("Gyro_DPS_X = %4.2f, Gyro_DPS_Y = %4.2f, Gyro_DPS_Z = %4.2f\n", x, y, z);
|
||||
|
||||
indx++;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bmi2_coines_deinit();
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for accel and gyro.
|
||||
*/
|
||||
static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Structure to define accelerometer and gyro configuration. */
|
||||
struct bmi2_sens_config config[2];
|
||||
|
||||
/* Configure the type of feature. */
|
||||
config[ACCEL].type = BMI2_ACCEL;
|
||||
config[GYRO].type = BMI2_GYRO;
|
||||
|
||||
/* Get default configurations for the type of feature selected. */
|
||||
rslt = bmi270_get_sensor_config(config, 2, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Map data ready interrupt to interrupt pin. */
|
||||
rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* NOTE: The user can change the following configuration parameters according to their requirement. */
|
||||
/* Set Output Data Rate */
|
||||
config[ACCEL].cfg.acc.odr = BMI2_ACC_ODR_200HZ;
|
||||
|
||||
/* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */
|
||||
config[ACCEL].cfg.acc.range = BMI2_ACC_RANGE_2G;
|
||||
|
||||
/* The bandwidth parameter is used to configure the number of sensor samples that are averaged
|
||||
* if it is set to 2, then 2^(bandwidth parameter) samples
|
||||
* are averaged, resulting in 4 averaged samples.
|
||||
* Note1 : For more information, refer the datasheet.
|
||||
* Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but
|
||||
* this has an adverse effect on the power consumed.
|
||||
*/
|
||||
config[ACCEL].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4;
|
||||
|
||||
/* Enable the filter performance mode where averaging of samples
|
||||
* will be done based on above set bandwidth and ODR.
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode
|
||||
* 1 -> High performance mode(Default)
|
||||
* For more info refer datasheet.
|
||||
*/
|
||||
config[ACCEL].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE;
|
||||
|
||||
/* The user can change the following configuration parameters according to their requirement. */
|
||||
/* Set Output Data Rate */
|
||||
config[GYRO].cfg.gyr.odr = BMI2_GYR_ODR_200HZ;
|
||||
|
||||
/* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */
|
||||
config[GYRO].cfg.gyr.range = BMI2_GYR_RANGE_2000;
|
||||
|
||||
/* Gyroscope bandwidth parameters. By default the gyro bandwidth is in normal mode. */
|
||||
config[GYRO].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE;
|
||||
|
||||
/* Enable/Disable the noise performance mode for precision yaw rate sensing
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode(Default)
|
||||
* 1 -> High performance mode
|
||||
*/
|
||||
config[GYRO].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE;
|
||||
|
||||
/* Enable/Disable the filter performance mode where averaging of samples
|
||||
* will be done based on above set bandwidth and ODR.
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode
|
||||
* 1 -> High performance mode(Default)
|
||||
*/
|
||||
config[GYRO].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE;
|
||||
|
||||
/* Set the accel and gyro configurations. */
|
||||
rslt = bmi270_set_sensor_config(config, 2, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
}
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This function converts lsb to meter per second squared for 16 bit accelerometer at
|
||||
* range 2G, 4G, 8G or 16G.
|
||||
*/
|
||||
static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width)
|
||||
{
|
||||
float half_scale = ((float)(1 << bit_width) / 2.0f);
|
||||
|
||||
return (GRAVITY_EARTH * val * g_range) / half_scale;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This function converts lsb to degree per second for 16 bit gyro at
|
||||
* range 125, 250, 500, 1000 or 2000dps.
|
||||
*/
|
||||
static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width)
|
||||
{
|
||||
float half_scale = ((float)(1 << bit_width) / 2.0f);
|
||||
|
||||
return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val);
|
||||
}
|
|
@ -0,0 +1,18 @@
|
|||
COINES_INSTALL_PATH ?= ../../../..
|
||||
|
||||
EXAMPLE_FILE ?= any_motion_interrupt.c
|
||||
|
||||
API_LOCATION ?= ../..
|
||||
|
||||
COMMON_LOCATION ?= ..
|
||||
|
||||
C_SRCS += \
|
||||
$(API_LOCATION)/bmi2.c \
|
||||
$(API_LOCATION)/bmi270.c \
|
||||
$(COMMON_LOCATION)/common/common.c
|
||||
|
||||
INCLUDEPATHS += \
|
||||
$(API_LOCATION) \
|
||||
$(COMMON_LOCATION)/common
|
||||
|
||||
include $(COINES_INSTALL_PATH)/coines.mk
|
|
@ -0,0 +1,135 @@
|
|||
/**\
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
**/
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Header Files */
|
||||
#include <stdio.h>
|
||||
#include "bmi270.h"
|
||||
#include "common.h"
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Static Function Declaration */
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for any-motion.
|
||||
*
|
||||
* @param[in] bmi2_dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @return Status of execution.
|
||||
*/
|
||||
static int8_t set_feature_config(struct bmi2_dev *bmi2_dev);
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Functions */
|
||||
|
||||
/* This function starts the execution of program. */
|
||||
int main(void)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Accel sensor and any-motion feature are listed in array. */
|
||||
uint8_t sens_list[2] = { BMI2_ACCEL, BMI2_ANY_MOTION };
|
||||
|
||||
/* Variable to get any-motion interrupt status. */
|
||||
uint16_t int_status = 0;
|
||||
|
||||
/* Sensor initialization configuration. */
|
||||
struct bmi2_dev bmi2_dev;
|
||||
|
||||
/* Select features and their pins to be mapped to. */
|
||||
struct bmi2_sens_int_config sens_int = { .type = BMI2_ANY_MOTION, .hw_int_pin = BMI2_INT1 };
|
||||
|
||||
/* Interface reference is given as a parameter
|
||||
* For I2C : BMI2_I2C_INTF
|
||||
* For SPI : BMI2_SPI_INTF
|
||||
*/
|
||||
rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initialize bmi270. */
|
||||
rslt = bmi270_init(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Enable the selected sensors. */
|
||||
rslt = bmi270_sensor_enable(sens_list, 2, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Set feature configurations for any-motion. */
|
||||
rslt = set_feature_config(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Map the feature interrupt for any-motion. */
|
||||
rslt = bmi270_map_feat_int(&sens_int, 1, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
printf("Move the board\n");
|
||||
|
||||
/* Loop to get any-motion interrupt. */
|
||||
do
|
||||
{
|
||||
/* Clear buffer. */
|
||||
int_status = 0;
|
||||
|
||||
/* To get the interrupt status of any-motion. */
|
||||
rslt = bmi2_get_int_status(&int_status, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* To check the interrupt status of any-motion. */
|
||||
if (int_status & BMI270_ANY_MOT_STATUS_MASK)
|
||||
{
|
||||
printf("Any-motion interrupt is generated\n");
|
||||
break;
|
||||
}
|
||||
} while (rslt == BMI2_OK);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bmi2_coines_deinit();
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for any-motion.
|
||||
*/
|
||||
static int8_t set_feature_config(struct bmi2_dev *bmi2_dev)
|
||||
{
|
||||
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Structure to define the type of sensor and its configurations. */
|
||||
struct bmi2_sens_config config;
|
||||
|
||||
/* Configure the type of feature. */
|
||||
config.type = BMI2_ANY_MOTION;
|
||||
|
||||
/* Get default configurations for the type of feature selected. */
|
||||
rslt = bmi270_get_sensor_config(&config, 1, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* NOTE: The user can change the following configuration parameters according to their requirement. */
|
||||
/* 1LSB equals 20ms. Default is 100ms, setting to 80ms. */
|
||||
config.cfg.any_motion.duration = 0x04;
|
||||
|
||||
/* 1LSB equals to 0.48mg. Default is 83mg, setting to 50mg. */
|
||||
config.cfg.any_motion.threshold = 0x68;
|
||||
|
||||
/* Set new configurations. */
|
||||
rslt = bmi270_set_sensor_config(&config, 1, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
}
|
||||
|
||||
return rslt;
|
||||
}
|
|
@ -0,0 +1,372 @@
|
|||
/**
|
||||
* Copyright (C) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include "coines.h"
|
||||
#include "bmi2_defs.h"
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Macro definitions */
|
||||
#define BMI2XY_SHUTTLE_ID UINT16_C(0x1B8)
|
||||
|
||||
/*! Macro that defines read write length */
|
||||
#define READ_WRITE_LEN UINT8_C(46)
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Static variable definition */
|
||||
|
||||
/*! Variable that holds the I2C device address or SPI chip selection */
|
||||
static uint8_t dev_addr;
|
||||
|
||||
/******************************************************************************/
|
||||
/*! User interface functions */
|
||||
|
||||
/*!
|
||||
* I2C read function map to COINES platform
|
||||
*/
|
||||
BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr)
|
||||
{
|
||||
uint8_t dev_addr = *(uint8_t*)intf_ptr;
|
||||
|
||||
return coines_read_i2c(dev_addr, reg_addr, reg_data, (uint16_t)len);
|
||||
}
|
||||
|
||||
/*!
|
||||
* I2C write function map to COINES platform
|
||||
*/
|
||||
BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr)
|
||||
{
|
||||
uint8_t dev_addr = *(uint8_t*)intf_ptr;
|
||||
|
||||
return coines_write_i2c(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len);
|
||||
}
|
||||
|
||||
/*!
|
||||
* SPI read function map to COINES platform
|
||||
*/
|
||||
BMI2_INTF_RETURN_TYPE bmi2_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr)
|
||||
{
|
||||
uint8_t dev_addr = *(uint8_t*)intf_ptr;
|
||||
|
||||
return coines_read_spi(dev_addr, reg_addr, reg_data, (uint16_t)len);
|
||||
}
|
||||
|
||||
/*!
|
||||
* SPI write function map to COINES platform
|
||||
*/
|
||||
BMI2_INTF_RETURN_TYPE bmi2_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr)
|
||||
{
|
||||
uint8_t dev_addr = *(uint8_t*)intf_ptr;
|
||||
|
||||
return coines_write_spi(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len);
|
||||
}
|
||||
|
||||
/*!
|
||||
* Delay function map to COINES platform
|
||||
*/
|
||||
void bmi2_delay_us(uint32_t period, void *intf_ptr)
|
||||
{
|
||||
coines_delay_usec(period);
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Function to select the interface between SPI and I2C.
|
||||
* Also to initialize coines platform
|
||||
*/
|
||||
int8_t bmi2_interface_init(struct bmi2_dev *bmi, uint8_t intf)
|
||||
{
|
||||
int8_t rslt = BMI2_OK;
|
||||
struct coines_board_info board_info;
|
||||
|
||||
if (bmi != NULL)
|
||||
{
|
||||
int16_t result = coines_open_comm_intf(COINES_COMM_INTF_USB);
|
||||
if (result < COINES_SUCCESS)
|
||||
{
|
||||
printf(
|
||||
"\n Unable to connect with Application Board ! \n" " 1. Check if the board is connected and powered on. \n" " 2. Check if Application Board USB driver is installed. \n"
|
||||
" 3. Check if board is in use by another application. (Insufficient permissions to access USB) \n");
|
||||
exit(result);
|
||||
}
|
||||
|
||||
result = coines_get_board_info(&board_info);
|
||||
|
||||
#if defined(PC)
|
||||
setbuf(stdout, NULL);
|
||||
#endif
|
||||
|
||||
if (result == COINES_SUCCESS)
|
||||
{
|
||||
if ((board_info.shuttle_id != BMI2XY_SHUTTLE_ID))
|
||||
{
|
||||
printf("! Warning invalid sensor shuttle \n ," "This application will not support this sensor \n");
|
||||
exit(COINES_E_FAILURE);
|
||||
}
|
||||
}
|
||||
|
||||
coines_set_shuttleboard_vdd_vddio_config(0, 0);
|
||||
coines_delay_msec(100);
|
||||
|
||||
/* Bus configuration : I2C */
|
||||
if (intf == BMI2_I2C_INTF)
|
||||
{
|
||||
printf("I2C Interface \n");
|
||||
|
||||
/* To initialize the user I2C function */
|
||||
dev_addr = BMI2_I2C_PRIM_ADDR;
|
||||
bmi->intf = BMI2_I2C_INTF;
|
||||
bmi->read = bmi2_i2c_read;
|
||||
bmi->write = bmi2_i2c_write;
|
||||
coines_config_i2c_bus(COINES_I2C_BUS_0, COINES_I2C_FAST_MODE);
|
||||
}
|
||||
/* Bus configuration : SPI */
|
||||
else if (intf == BMI2_SPI_INTF)
|
||||
{
|
||||
printf("SPI Interface \n");
|
||||
|
||||
/* To initialize the user SPI function */
|
||||
dev_addr = COINES_SHUTTLE_PIN_7;
|
||||
bmi->intf = BMI2_SPI_INTF;
|
||||
bmi->read = bmi2_spi_read;
|
||||
bmi->write = bmi2_spi_write;
|
||||
coines_config_spi_bus(COINES_SPI_BUS_0, COINES_SPI_SPEED_5_MHZ, COINES_SPI_MODE3);
|
||||
coines_set_pin_config(COINES_SHUTTLE_PIN_7, COINES_PIN_DIRECTION_OUT, COINES_PIN_VALUE_HIGH);
|
||||
}
|
||||
|
||||
/* Assign device address to interface pointer */
|
||||
bmi->intf_ptr = &dev_addr;
|
||||
|
||||
/* Configure delay in microseconds */
|
||||
bmi->delay_us = bmi2_delay_us;
|
||||
|
||||
/* Configure max read/write length (in bytes) ( Supported length depends on target machine) */
|
||||
bmi->read_write_len = READ_WRITE_LEN;
|
||||
|
||||
/* Assign to NULL to load the default config file. */
|
||||
bmi->config_file_ptr = NULL;
|
||||
|
||||
coines_delay_usec(10000);
|
||||
|
||||
coines_set_shuttleboard_vdd_vddio_config(3300, 3300);
|
||||
|
||||
coines_delay_usec(10000);
|
||||
}
|
||||
else
|
||||
{
|
||||
rslt = BMI2_E_NULL_PTR;
|
||||
}
|
||||
|
||||
return rslt;
|
||||
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Prints the execution status of the APIs.
|
||||
*/
|
||||
void bmi2_error_codes_print_result(int8_t rslt)
|
||||
{
|
||||
switch (rslt)
|
||||
{
|
||||
case BMI2_OK:
|
||||
|
||||
/* Do nothing */
|
||||
break;
|
||||
|
||||
case BMI2_W_FIFO_EMPTY:
|
||||
printf("Warning [%d] : FIFO empty\r\n", rslt);
|
||||
break;
|
||||
case BMI2_W_PARTIAL_READ:
|
||||
printf("Warning [%d] : FIFO partial read\r\n", rslt);
|
||||
break;
|
||||
case BMI2_E_NULL_PTR:
|
||||
printf(
|
||||
"Error [%d] : Null pointer error. It occurs when the user tries to assign value (not address) to a pointer," " which has been initialized to NULL.\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_COM_FAIL:
|
||||
printf(
|
||||
"Error [%d] : Communication failure error. It occurs due to read/write operation failure and also due " "to power failure during communication\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_DEV_NOT_FOUND:
|
||||
printf("Error [%d] : Device not found error. It occurs when the device chip id is incorrectly read\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_INVALID_SENSOR:
|
||||
printf(
|
||||
"Error [%d] : Invalid sensor error. It occurs when there is a mismatch in the requested feature with the " "available one\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_SELF_TEST_FAIL:
|
||||
printf(
|
||||
"Error [%d] : Self-test failed error. It occurs when the validation of accel self-test data is " "not satisfied\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_INVALID_INT_PIN:
|
||||
printf(
|
||||
"Error [%d] : Invalid interrupt pin error. It occurs when the user tries to configure interrupt pins " "apart from INT1 and INT2\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_OUT_OF_RANGE:
|
||||
printf(
|
||||
"Error [%d] : Out of range error. It occurs when the data exceeds from filtered or unfiltered data from " "fifo and also when the range exceeds the maximum range for accel and gyro while performing FOC\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_ACC_INVALID_CFG:
|
||||
printf(
|
||||
"Error [%d] : Invalid Accel configuration error. It occurs when there is an error in accel configuration" " register which could be one among range, BW or filter performance in reg address 0x40\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_GYRO_INVALID_CFG:
|
||||
printf(
|
||||
"Error [%d] : Invalid Gyro configuration error. It occurs when there is a error in gyro configuration" "register which could be one among range, BW or filter performance in reg address 0x42\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_ACC_GYR_INVALID_CFG:
|
||||
printf(
|
||||
"Error [%d] : Invalid Accel-Gyro configuration error. It occurs when there is a error in accel and gyro" " configuration registers which could be one among range, BW or filter performance in reg address 0x40 " "and 0x42\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_CONFIG_LOAD:
|
||||
printf(
|
||||
"Error [%d] : Configuration load error. It occurs when failure observed while loading the configuration " "into the sensor\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_INVALID_PAGE:
|
||||
printf(
|
||||
"Error [%d] : Invalid page error. It occurs due to failure in writing the correct feature configuration " "from selected page\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_SET_APS_FAIL:
|
||||
printf(
|
||||
"Error [%d] : APS failure error. It occurs due to failure in write of advance power mode configuration " "register\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_AUX_INVALID_CFG:
|
||||
printf(
|
||||
"Error [%d] : Invalid AUX configuration error. It occurs when the auxiliary interface settings are not " "enabled properly\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_AUX_BUSY:
|
||||
printf(
|
||||
"Error [%d] : AUX busy error. It occurs when the auxiliary interface buses are engaged while configuring" " the AUX\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_REMAP_ERROR:
|
||||
printf(
|
||||
"Error [%d] : Remap error. It occurs due to failure in assigning the remap axes data for all the axes " "after change in axis position\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_GYR_USER_GAIN_UPD_FAIL:
|
||||
printf(
|
||||
"Error [%d] : Gyro user gain update fail error. It occurs when the reading of user gain update status " "fails\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_SELF_TEST_NOT_DONE:
|
||||
printf(
|
||||
"Error [%d] : Self-test not done error. It occurs when the self-test process is ongoing or not " "completed\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_INVALID_INPUT:
|
||||
printf("Error [%d] : Invalid input error. It occurs when the sensor input validity fails\r\n", rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_INVALID_STATUS:
|
||||
printf("Error [%d] : Invalid status error. It occurs when the feature/sensor validity fails\r\n", rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_CRT_ERROR:
|
||||
printf("Error [%d] : CRT error. It occurs when the CRT test has failed\r\n", rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_ST_ALREADY_RUNNING:
|
||||
printf(
|
||||
"Error [%d] : Self-test already running error. It occurs when the self-test is already running and " "another has been initiated\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_CRT_READY_FOR_DL_FAIL_ABORT:
|
||||
printf(
|
||||
"Error [%d] : CRT ready for download fail abort error. It occurs when download in CRT fails due to wrong " "address location\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_DL_ERROR:
|
||||
printf(
|
||||
"Error [%d] : Download error. It occurs when write length exceeds that of the maximum burst length\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_PRECON_ERROR:
|
||||
printf(
|
||||
"Error [%d] : Pre-conditional error. It occurs when precondition to start the feature was not " "completed\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_ABORT_ERROR:
|
||||
printf("Error [%d] : Abort error. It occurs when the device was shaken during CRT test\r\n", rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_WRITE_CYCLE_ONGOING:
|
||||
printf(
|
||||
"Error [%d] : Write cycle ongoing error. It occurs when the write cycle is already running and another " "has been initiated\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_ST_NOT_RUNING:
|
||||
printf(
|
||||
"Error [%d] : Self-test is not running error. It occurs when self-test running is disabled while it's " "running\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_DATA_RDY_INT_FAILED:
|
||||
printf(
|
||||
"Error [%d] : Data ready interrupt error. It occurs when the sample count exceeds the FOC sample limit " "and data ready status is not updated\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_INVALID_FOC_POSITION:
|
||||
printf(
|
||||
"Error [%d] : Invalid FOC position error. It occurs when average FOC data is obtained for the wrong" " axes\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
default:
|
||||
printf("Error [%d] : Unknown error code\r\n", rslt);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Deinitializes coines platform
|
||||
*
|
||||
* @return void.
|
||||
*/
|
||||
void bmi2_coines_deinit(void)
|
||||
{
|
||||
coines_close_comm_intf(COINES_COMM_INTF_USB);
|
||||
}
|
|
@ -0,0 +1,122 @@
|
|||
/**\
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
**/
|
||||
|
||||
#ifndef _COMMON_H
|
||||
#define _COMMON_H
|
||||
|
||||
/*! CPP guard */
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include <stdio.h>
|
||||
#include "bmi2.h"
|
||||
|
||||
/*!
|
||||
* @brief Function for reading the sensor's registers through I2C bus.
|
||||
*
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[out] reg_data : Pointer to the data buffer to store the read data.
|
||||
* @param[in] length : No of bytes to read.
|
||||
* @param[in] intf_ptr : Interface pointer
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval = BMI2_INTF_RET_SUCCESS -> Success
|
||||
* @retval != BMI2_INTF_RET_SUCCESS -> Failure Info
|
||||
*
|
||||
*/
|
||||
BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr);
|
||||
|
||||
/*!
|
||||
* @brief Function for writing the sensor's registers through I2C bus.
|
||||
*
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[in] reg_data : Pointer to the data buffer whose value is to be written.
|
||||
* @param[in] length : No of bytes to write.
|
||||
* @param[in] intf_ptr : Interface pointer
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval = BMI2_INTF_RET_SUCCESS -> Success
|
||||
* @retval != BMI2_INTF_RET_SUCCESS -> Failure Info
|
||||
*
|
||||
*/
|
||||
BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr);
|
||||
|
||||
/*!
|
||||
* @brief Function for reading the sensor's registers through SPI bus.
|
||||
*
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[out] reg_data : Pointer to the data buffer to store the read data.
|
||||
* @param[in] length : No of bytes to read.
|
||||
* @param[in] intf_ptr : Interface pointer
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval = BMI2_INTF_RET_SUCCESS -> Success
|
||||
* @retval != BMI2_INTF_RET_SUCCESS -> Failure Info
|
||||
*
|
||||
*/
|
||||
BMI2_INTF_RETURN_TYPE bmi2_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr);
|
||||
|
||||
/*!
|
||||
* @brief Function for writing the sensor's registers through SPI bus.
|
||||
*
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[in] reg_data : Pointer to the data buffer whose data has to be written.
|
||||
* @param[in] length : No of bytes to write.
|
||||
* @param[in] intf_ptr : Interface pointer
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval = BMI2_INTF_RET_SUCCESS -> Success
|
||||
* @retval != BMI2_INTF_RET_SUCCESS -> Failure Info
|
||||
*
|
||||
*/
|
||||
BMI2_INTF_RETURN_TYPE bmi2_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr);
|
||||
|
||||
/*!
|
||||
* @brief This function provides the delay for required time (Microsecond) as per the input provided in some of the
|
||||
* APIs.
|
||||
*
|
||||
* @param[in] period_us : The required wait time in microsecond.
|
||||
* @param[in] intf_ptr : Interface pointer
|
||||
*
|
||||
* @return void.
|
||||
*
|
||||
*/
|
||||
void bmi2_delay_us(uint32_t period, void *intf_ptr);
|
||||
|
||||
/*!
|
||||
* @brief Function to select the interface between SPI and I2C.
|
||||
*
|
||||
* @param[in] bma : Structure instance of bmi2_dev
|
||||
* @param[in] intf : Interface selection parameter
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Failure Info
|
||||
*/
|
||||
int8_t bmi2_interface_init(struct bmi2_dev *bma, uint8_t intf);
|
||||
|
||||
/*!
|
||||
* @brief Prints the execution status of the APIs.
|
||||
*
|
||||
* @param[in] rslt : Error code returned by the API whose execution status has to be printed.
|
||||
*
|
||||
* @return void.
|
||||
*/
|
||||
void bmi2_error_codes_print_result(int8_t rslt);
|
||||
|
||||
/*!
|
||||
* @brief Deinitializes coines platform
|
||||
*
|
||||
* @return void.
|
||||
*/
|
||||
void bmi2_coines_deinit(void);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif /* End of CPP guard */
|
||||
|
||||
#endif /* _COMMON_H */
|
|
@ -0,0 +1,18 @@
|
|||
COINES_INSTALL_PATH ?= ../../../..
|
||||
|
||||
EXAMPLE_FILE ?= component_retrim.c
|
||||
|
||||
API_LOCATION ?= ../..
|
||||
|
||||
COMMON_LOCATION ?= ..
|
||||
|
||||
C_SRCS += \
|
||||
$(API_LOCATION)/bmi2.c \
|
||||
$(API_LOCATION)/bmi270.c \
|
||||
$(COMMON_LOCATION)/common/common.c
|
||||
|
||||
INCLUDEPATHS += \
|
||||
$(API_LOCATION) \
|
||||
$(COMMON_LOCATION)/common
|
||||
|
||||
include $(COINES_INSTALL_PATH)/coines.mk
|
|
@ -0,0 +1,52 @@
|
|||
/**\
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
**/
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Header Files */
|
||||
#include <stdio.h>
|
||||
#include "bmi270.h"
|
||||
#include "common.h"
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Functions */
|
||||
|
||||
/* This function starts the execution of program. */
|
||||
int main(void)
|
||||
{
|
||||
/* Sensor initialization configuration. */
|
||||
struct bmi2_dev bmi2_dev;
|
||||
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Interface reference is given as a parameter
|
||||
* For I2C : BMI2_I2C_INTF
|
||||
* For SPI : BMI2_SPI_INTF
|
||||
*/
|
||||
rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initialize bmi270. */
|
||||
rslt = bmi270_init(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* This API runs the CRT process. */
|
||||
rslt = bmi2_do_crt(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Do not move the board while doing CRT. If so, it will throw an abort error. */
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
printf("CRT successfully completed.");
|
||||
}
|
||||
}
|
||||
|
||||
bmi2_coines_deinit();
|
||||
|
||||
return rslt;
|
||||
}
|
|
@ -0,0 +1,18 @@
|
|||
COINES_INSTALL_PATH ?= ../../../..
|
||||
|
||||
EXAMPLE_FILE ?= fifo_full_header_mode.c
|
||||
|
||||
API_LOCATION ?= ../..
|
||||
|
||||
COMMON_LOCATION ?= ..
|
||||
|
||||
C_SRCS += \
|
||||
$(API_LOCATION)/bmi2.c \
|
||||
$(API_LOCATION)/bmi270.c \
|
||||
$(COMMON_LOCATION)/common/common.c
|
||||
|
||||
INCLUDEPATHS += \
|
||||
$(API_LOCATION) \
|
||||
$(COMMON_LOCATION)/common
|
||||
|
||||
include $(COINES_INSTALL_PATH)/coines.mk
|
|
@ -0,0 +1,315 @@
|
|||
/**\
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
**/
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Header Files */
|
||||
#include <stdio.h>
|
||||
#include "bmi270.h"
|
||||
#include "common.h"
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Macros */
|
||||
|
||||
/*! Buffer size allocated to store raw FIFO data. */
|
||||
#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048)
|
||||
|
||||
/*! Length of data to be read from FIFO. */
|
||||
#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048)
|
||||
|
||||
/*! Number of accel frames to be extracted from FIFO. */
|
||||
|
||||
/*! Calculation for frame count: Total frame count = Fifo buffer size(2048)/ Total frames(6 Accel, 6 Gyro and 1 header,
|
||||
* totaling to 13) which equals to 157.
|
||||
*/
|
||||
#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(157)
|
||||
|
||||
/*! Number of gyro frames to be extracted from FIFO. */
|
||||
#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(157)
|
||||
|
||||
/*! Macro to read sensortime byte in FIFO. */
|
||||
#define SENSORTIME_OVERHEAD_BYTE UINT8_C(3)
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Static Function Declaration */
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for accel and gyro.
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
* @return Status of execution.
|
||||
*/
|
||||
static int8_t set_accel_gyro_config(struct bmi2_dev *dev);
|
||||
|
||||
/******************************************************************************/
|
||||
|
||||
/* This function starts the execution of program. */
|
||||
int main(void)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
uint16_t index = 0;
|
||||
uint16_t fifo_length = 0;
|
||||
uint16_t config = 0;
|
||||
|
||||
/* To read sensortime, extra 3 bytes are added to fifo buffer. */
|
||||
uint16_t fifo_buffer_size = BMI2_FIFO_RAW_DATA_BUFFER_SIZE + SENSORTIME_OVERHEAD_BYTE;
|
||||
|
||||
/* Variable to get fifo full interrupt status. */
|
||||
uint16_t int_status = 0;
|
||||
|
||||
uint16_t accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT;
|
||||
|
||||
uint16_t gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT;
|
||||
|
||||
/* Variable to store the available frame length count in FIFO */
|
||||
uint8_t frame_count;
|
||||
|
||||
int8_t try = 1;
|
||||
|
||||
/* Number of bytes of FIFO data. */
|
||||
uint8_t fifo_data[fifo_buffer_size];
|
||||
|
||||
/* Sensor initialization configuration. */
|
||||
struct bmi2_dev bmi2_dev;
|
||||
|
||||
/* Array of accelerometer frames -> Total bytes =
|
||||
* 157 * (6 axes + 1 header bytes) = 1099 bytes */
|
||||
struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } };
|
||||
|
||||
/* Array of gyro frames -> Total bytes =
|
||||
* 157 * (6 axes + 1 header bytes) = 1099 bytes */
|
||||
struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } };
|
||||
|
||||
/* Initialize FIFO frame structure. */
|
||||
struct bmi2_fifo_frame fifoframe = { 0 };
|
||||
|
||||
/* Accel and gyro sensor are listed in array. */
|
||||
uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO };
|
||||
|
||||
/* Interface reference is given as a parameter
|
||||
* For I2C : BMI2_I2C_INTF
|
||||
* For SPI : BMI2_SPI_INTF
|
||||
*/
|
||||
rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initialize bmi270. */
|
||||
rslt = bmi270_init(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Enable accel and gyro sensor. */
|
||||
rslt = bmi270_sensor_enable(sensor_sel, 2, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Configuration settings for accel and gyro. */
|
||||
rslt = set_accel_gyro_config(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Before setting FIFO, disable the advance power save mode. */
|
||||
rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initially disable all configurations in fifo. */
|
||||
rslt = bmi2_get_fifo_config(&config, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initially disable all configurations in fifo. */
|
||||
rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Update FIFO structure. */
|
||||
/* Mapping the buffer to store the fifo data. */
|
||||
fifoframe.data = fifo_data;
|
||||
|
||||
/* Length of FIFO frame. */
|
||||
/* To read sensortime, extra 3 bytes are added to fifo user length. */
|
||||
fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH + SENSORTIME_OVERHEAD_BYTE;
|
||||
|
||||
/* Set FIFO configuration by enabling accel, gyro and timestamp.
|
||||
* NOTE 1: The header mode is enabled by default.
|
||||
* NOTE 2: By default the FIFO operating mode is in FIFO mode.
|
||||
* NOTE 3: Sensortime is enabled by default */
|
||||
printf("FIFO is configured in header mode\n");
|
||||
rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Map FIFO full interrupt. */
|
||||
fifoframe.data_int_map = BMI2_FFULL_INT;
|
||||
rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
while (try <= 10)
|
||||
{
|
||||
/* Read FIFO data on interrupt. */
|
||||
rslt = bmi2_get_int_status(&int_status, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if ((rslt == BMI2_OK) && (int_status & BMI2_FFULL_INT_STATUS_MASK))
|
||||
{
|
||||
printf("\nIteration : %d\n", try);
|
||||
|
||||
accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT;
|
||||
|
||||
gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT;
|
||||
|
||||
rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
printf("\nFIFO data bytes available : %d \n", fifo_length);
|
||||
printf("\nFIFO data bytes requested : %d \n", fifoframe.length);
|
||||
|
||||
/* Read FIFO data. */
|
||||
rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Read FIFO data on interrupt. */
|
||||
rslt = bmi2_get_int_status(&int_status, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
printf("\nFIFO accel frames requested : %d \n", accel_frame_length);
|
||||
|
||||
/* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */
|
||||
rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev);
|
||||
printf("\nFIFO accel frames extracted : %d \n", accel_frame_length);
|
||||
|
||||
printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length);
|
||||
|
||||
/* Parse the FIFO data to extract gyro data from the FIFO buffer. */
|
||||
rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev);
|
||||
printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length);
|
||||
|
||||
/* Calculating the frame count from the available bytes in FIFO
|
||||
* frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len + header_byte)) */
|
||||
frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH + 1));
|
||||
printf("\nAvailable frame count from available bytes: %d\n", frame_count);
|
||||
|
||||
printf("\nExtracted accel frames\n");
|
||||
|
||||
if (accel_frame_length > frame_count)
|
||||
{
|
||||
accel_frame_length = frame_count;
|
||||
}
|
||||
|
||||
/* Print the parsed accelerometer data from the FIFO buffer. */
|
||||
for (index = 0; index < accel_frame_length; index++)
|
||||
{
|
||||
printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x,
|
||||
fifo_accel_data[index].y, fifo_accel_data[index].z);
|
||||
}
|
||||
|
||||
printf("\nExtracted gyro frames\n");
|
||||
|
||||
if (gyro_frame_length > frame_count)
|
||||
{
|
||||
gyro_frame_length = frame_count;
|
||||
}
|
||||
|
||||
/* Print the parsed gyro data from the FIFO buffer. */
|
||||
for (index = 0; index < gyro_frame_length; index++)
|
||||
{
|
||||
printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n",
|
||||
index,
|
||||
fifo_gyro_data[index].x,
|
||||
fifo_gyro_data[index].y,
|
||||
fifo_gyro_data[index].z);
|
||||
}
|
||||
|
||||
/* Print control frames like sensor time and skipped frame count. */
|
||||
printf("\nSkipped frame count = %d\n", fifoframe.skipped_frame_count);
|
||||
|
||||
printf("Sensor time = %lu\r\n", fifoframe.sensor_time);
|
||||
|
||||
try++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bmi2_coines_deinit();
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for accel and gyro.
|
||||
*/
|
||||
static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Structure to define accel and gyro configurations. */
|
||||
struct bmi2_sens_config config[2];
|
||||
|
||||
/* Configure the type of feature. */
|
||||
config[0].type = BMI2_ACCEL;
|
||||
config[1].type = BMI2_GYRO;
|
||||
|
||||
/* Get default configurations for the type of feature selected. */
|
||||
rslt = bmi270_get_sensor_config(config, 2, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* NOTE: The user can change the following configuration parameters according to their requirement. */
|
||||
/* Accel configuration settings. */
|
||||
/* Set Output Data Rate */
|
||||
config[0].cfg.acc.odr = BMI2_ACC_ODR_25HZ;
|
||||
|
||||
/* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */
|
||||
config[0].cfg.acc.range = BMI2_ACC_RANGE_2G;
|
||||
|
||||
/* The bandwidth parameter is used to configure the number of sensor samples that are averaged
|
||||
* if it is set to 2, then 2^(bandwidth parameter) samples
|
||||
* are averaged, resulting in 4 averaged samples
|
||||
* Note1 : For more information, refer the datasheet.
|
||||
* Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but
|
||||
* this has an adverse effect on the power consumed.
|
||||
*/
|
||||
config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4;
|
||||
|
||||
/* Enable the filter performance mode where averaging of samples
|
||||
* will be done based on above set bandwidth and ODR.
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode
|
||||
* 1 -> High performance mode(Default)
|
||||
* For more info refer datasheet.
|
||||
*/
|
||||
config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE;
|
||||
|
||||
/* Gyro configuration settings. */
|
||||
/* Set Output Data Rate */
|
||||
config[1].cfg.gyr.odr = BMI2_GYR_ODR_25HZ;
|
||||
|
||||
/* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */
|
||||
config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000;
|
||||
|
||||
/* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */
|
||||
config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE;
|
||||
|
||||
/* Enable/Disable the noise performance mode for precision yaw rate sensing
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode(Default)
|
||||
* 1 -> High performance mode
|
||||
*/
|
||||
config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE;
|
||||
|
||||
/* Enable/Disable the filter performance mode where averaging of samples
|
||||
* will be done based on above set bandwidth and ODR.
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode
|
||||
* 1 -> High performance mode(Default)
|
||||
*/
|
||||
config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE;
|
||||
|
||||
/* Set new configurations. */
|
||||
rslt = bmi270_set_sensor_config(config, 2, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
}
|
||||
|
||||
return rslt;
|
||||
}
|
|
@ -0,0 +1,18 @@
|
|||
COINES_INSTALL_PATH ?= ../../../..
|
||||
|
||||
EXAMPLE_FILE ?= fifo_full_headerless_mode.c
|
||||
|
||||
API_LOCATION ?= ../..
|
||||
|
||||
COMMON_LOCATION ?= ..
|
||||
|
||||
C_SRCS += \
|
||||
$(API_LOCATION)/bmi2.c \
|
||||
$(API_LOCATION)/bmi270.c \
|
||||
$(COMMON_LOCATION)/common/common.c
|
||||
|
||||
INCLUDEPATHS += \
|
||||
$(API_LOCATION) \
|
||||
$(COMMON_LOCATION)/common
|
||||
|
||||
include $(COINES_INSTALL_PATH)/coines.mk
|
|
@ -0,0 +1,304 @@
|
|||
/**\
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
**/
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Header Files */
|
||||
#include <stdio.h>
|
||||
#include "bmi270.h"
|
||||
#include "common.h"
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Macros */
|
||||
|
||||
/*! Buffer size allocated to store raw FIFO data */
|
||||
#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048)
|
||||
|
||||
/*! Length of data to be read from FIFO */
|
||||
#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048)
|
||||
|
||||
/*! Number of accel frames to be extracted from FIFO */
|
||||
|
||||
/*! Calculation for frame count: Total frame count = Fifo buffer size(2048)/ Total frames(6 Accel, 6 Gyro totaling to
|
||||
* 12) which equals to 170.
|
||||
*/
|
||||
#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(169)
|
||||
|
||||
/*! Number of gyro frames to be extracted from FIFO */
|
||||
#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(169)
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Static Function Declaration */
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for accel and gyro.
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
* @return Status of execution.
|
||||
*/
|
||||
static int8_t set_accel_gyro_config(struct bmi2_dev *dev);
|
||||
|
||||
/******************************************************************************/
|
||||
|
||||
/* This function starts the execution of program. */
|
||||
int main(void)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
uint16_t index = 0;
|
||||
|
||||
uint16_t fifo_length = 0;
|
||||
|
||||
uint8_t try = 1;
|
||||
|
||||
/* Variable to get fifo full interrupt status. */
|
||||
uint16_t int_status = 0;
|
||||
|
||||
uint16_t accel_frame_length;
|
||||
|
||||
uint16_t gyro_frame_length;
|
||||
|
||||
/* Variable to store the available frame length count in FIFO */
|
||||
uint8_t frame_count;
|
||||
|
||||
/* Number of bytes of FIFO data. */
|
||||
uint8_t fifo_data[BMI2_FIFO_RAW_DATA_BUFFER_SIZE] = { 0 };
|
||||
|
||||
/* Sensor initialization configuration. */
|
||||
struct bmi2_dev bmi2_dev;
|
||||
|
||||
/* Array of accelerometer frames -> Total bytes =
|
||||
* 170 * (6 axes bytes) = 1020 bytes */
|
||||
struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } };
|
||||
|
||||
/* Array of gyro frames -> Total bytes =
|
||||
* 170 * (6 axes bytes) = 1020 bytes */
|
||||
struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } };
|
||||
|
||||
/* Initialize FIFO frame structure. */
|
||||
struct bmi2_fifo_frame fifoframe = { 0 };
|
||||
|
||||
/* Accel and gyro sensor are listed in array. */
|
||||
uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO };
|
||||
|
||||
/* Interface reference is given as a parameter
|
||||
* For I2C : BMI2_I2C_INTF
|
||||
* For SPI : BMI2_SPI_INTF
|
||||
*/
|
||||
rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initialize bmi270. */
|
||||
rslt = bmi270_init(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Enable accel and gyro sensor. */
|
||||
rslt = bmi270_sensor_enable(sensor_sel, 2, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Configuration settings for accel and gyro. */
|
||||
rslt = set_accel_gyro_config(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Before setting FIFO, disable the advance power save mode. */
|
||||
rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initially disable all configurations in fifo. */
|
||||
rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Update FIFO structure. */
|
||||
/* Mapping the buffer to store the fifo data. */
|
||||
fifoframe.data = fifo_data;
|
||||
|
||||
/* Length of FIFO frame. */
|
||||
fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH;
|
||||
|
||||
/* Set FIFO configuration by enabling accel, gyro.
|
||||
* NOTE 1: The header mode is enabled by default.
|
||||
* NOTE 2: By default the FIFO operating mode is FIFO mode. */
|
||||
rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
printf("FIFO is configured in headerless mode\n");
|
||||
|
||||
/* To enable headerless mode, disable the header. */
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
rslt = bmi2_set_fifo_config(BMI2_FIFO_HEADER_EN, BMI2_DISABLE, &bmi2_dev);
|
||||
}
|
||||
|
||||
/* Map FIFO full interrupt. */
|
||||
fifoframe.data_int_map = BMI2_FFULL_INT;
|
||||
rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
while (try <= 10)
|
||||
{
|
||||
/* Read FIFO data on interrupt. */
|
||||
rslt = bmi2_get_int_status(&int_status, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if ((rslt == BMI2_OK) && (int_status & BMI2_FFULL_INT_STATUS_MASK))
|
||||
{
|
||||
printf("\nIteration : %d\n", try);
|
||||
|
||||
rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT;
|
||||
gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT;
|
||||
|
||||
printf("\nFIFO data bytes available : %d \n", fifo_length);
|
||||
printf("\nFIFO data bytes requested : %d \n", fifoframe.length);
|
||||
|
||||
/* Read FIFO data. */
|
||||
rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Read FIFO data on interrupt. */
|
||||
rslt = bmi2_get_int_status(&int_status, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
printf("\nFIFO accel frames requested : %d \n", accel_frame_length);
|
||||
|
||||
/* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */
|
||||
rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev);
|
||||
printf("\nFIFO accel frames extracted : %d \n", accel_frame_length);
|
||||
|
||||
printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length);
|
||||
|
||||
/* Parse the FIFO data to extract gyro data from the FIFO buffer. */
|
||||
rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev);
|
||||
printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length);
|
||||
|
||||
/* Calculating the frame count from the available bytes in FIFO
|
||||
* frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len)) */
|
||||
frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH));
|
||||
printf("\nAvailable frame count from available bytes: %d\n", frame_count);
|
||||
|
||||
printf("\nExtracted accel frames\n");
|
||||
|
||||
if (accel_frame_length > frame_count)
|
||||
{
|
||||
accel_frame_length = frame_count;
|
||||
}
|
||||
|
||||
/* Print the parsed accelerometer data from the FIFO buffer. */
|
||||
for (index = 0; index < accel_frame_length; index++)
|
||||
{
|
||||
printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x,
|
||||
fifo_accel_data[index].y, fifo_accel_data[index].z);
|
||||
}
|
||||
|
||||
printf("\nExtracted gyro frames\n");
|
||||
|
||||
if (gyro_frame_length > frame_count)
|
||||
{
|
||||
gyro_frame_length = frame_count;
|
||||
}
|
||||
|
||||
/* Print the parsed gyro data from the FIFO buffer. */
|
||||
for (index = 0; index < gyro_frame_length; index++)
|
||||
{
|
||||
printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n",
|
||||
index,
|
||||
fifo_gyro_data[index].x,
|
||||
fifo_gyro_data[index].y,
|
||||
fifo_gyro_data[index].z);
|
||||
}
|
||||
}
|
||||
|
||||
try++;
|
||||
}
|
||||
}
|
||||
|
||||
bmi2_coines_deinit();
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for accel and gyro.
|
||||
*/
|
||||
static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Structure to define accel and gyro configurations. */
|
||||
struct bmi2_sens_config config[2];
|
||||
|
||||
/* Configure the type of feature. */
|
||||
config[0].type = BMI2_ACCEL;
|
||||
config[1].type = BMI2_GYRO;
|
||||
|
||||
/* Get default configurations for the type of feature selected. */
|
||||
rslt = bmi270_get_sensor_config(config, 2, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* NOTE: The user can change the following configuration parameters according to their requirement. */
|
||||
/* Accel configuration settings. */
|
||||
/* Set Output Data Rate */
|
||||
config[0].cfg.acc.odr = BMI2_ACC_ODR_100HZ;
|
||||
|
||||
/* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */
|
||||
config[0].cfg.acc.range = BMI2_ACC_RANGE_2G;
|
||||
|
||||
/* The bandwidth parameter is used to configure the number of sensor samples that are averaged
|
||||
* if it is set to 2, then 2^(bandwidth parameter) samples
|
||||
* are averaged, resulting in 4 averaged samples
|
||||
* Note1 : For more information, refer the datasheet.
|
||||
* Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but
|
||||
* this has an adverse effect on the power consumed.
|
||||
*/
|
||||
config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4;
|
||||
|
||||
/* Enable the filter performance mode where averaging of samples
|
||||
* will be done based on above set bandwidth and ODR.
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode
|
||||
* 1 -> High performance mode(Default)
|
||||
* For more info refer datasheet.
|
||||
*/
|
||||
config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE;
|
||||
|
||||
/* Gyro configuration settings. */
|
||||
/* Set Output Data Rate */
|
||||
config[1].cfg.gyr.odr = BMI2_GYR_ODR_100HZ;
|
||||
|
||||
/* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */
|
||||
config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000;
|
||||
|
||||
/* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */
|
||||
config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE;
|
||||
|
||||
/* Enable/Disable the noise performance mode for precision yaw rate sensing
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode(Default)
|
||||
* 1 -> High performance mode
|
||||
*/
|
||||
config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE;
|
||||
|
||||
/* Enable/Disable the filter performance mode where averaging of samples
|
||||
* will be done based on above set bandwidth and ODR.
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode
|
||||
* 1 -> High performance mode(Default)
|
||||
*/
|
||||
config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE;
|
||||
|
||||
/* Set new configurations. */
|
||||
rslt = bmi270_set_sensor_config(config, 2, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
}
|
||||
|
||||
return rslt;
|
||||
}
|
|
@ -0,0 +1,18 @@
|
|||
COINES_INSTALL_PATH ?= ../../../..
|
||||
|
||||
EXAMPLE_FILE ?= fifo_watermark_header_mode.c
|
||||
|
||||
API_LOCATION ?= ../..
|
||||
|
||||
COMMON_LOCATION ?= ..
|
||||
|
||||
C_SRCS += \
|
||||
$(API_LOCATION)/bmi2.c \
|
||||
$(API_LOCATION)/bmi270.c \
|
||||
$(COMMON_LOCATION)/common/common.c
|
||||
|
||||
INCLUDEPATHS += \
|
||||
$(API_LOCATION) \
|
||||
$(COMMON_LOCATION)/common
|
||||
|
||||
include $(COINES_INSTALL_PATH)/coines.mk
|
|
@ -0,0 +1,335 @@
|
|||
/**\
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
**/
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Header Files */
|
||||
#include <stdio.h>
|
||||
#include "bmi270.h"
|
||||
#include "common.h"
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Macros */
|
||||
|
||||
/*! Buffer size allocated to store raw FIFO data */
|
||||
#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048)
|
||||
|
||||
/*! Length of data to be read from FIFO */
|
||||
#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048)
|
||||
|
||||
/*! Number of accel frames to be extracted from FIFO */
|
||||
|
||||
/*!
|
||||
* Calculation:
|
||||
* fifo_watermark_level = 650, accel_frame_len = 6, gyro_frame_len = 6 header_byte = 1.
|
||||
* fifo_accel_frame_count = (650 / (6 + 6 + 1 )) = 50 frames
|
||||
* NOTE: Extra frames are read in order to get sensor time
|
||||
*/
|
||||
#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(55)
|
||||
|
||||
/*! Number of gyro frames to be extracted from FIFO */
|
||||
#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(55)
|
||||
|
||||
/*! Setting a watermark level in FIFO */
|
||||
#define BMI270_FIFO_WATERMARK_LEVEL UINT16_C(650)
|
||||
|
||||
/*! Macro to read sensortime byte in FIFO */
|
||||
#define SENSORTIME_OVERHEAD_BYTE UINT8_C(3)
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Static Function Declaration */
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for accel and gyro.
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
* @return Status of execution.
|
||||
*/
|
||||
static int8_t set_accel_gyro_config(struct bmi2_dev *dev);
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Functions */
|
||||
|
||||
/* This function starts the execution of program. */
|
||||
int main(void)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Variable to store the available frame length count in FIFO */
|
||||
uint8_t frame_count;
|
||||
|
||||
/* Variable to index bytes. */
|
||||
uint16_t index = 0;
|
||||
|
||||
uint16_t fifo_length = 0;
|
||||
|
||||
uint16_t accel_frame_length;
|
||||
|
||||
uint16_t gyro_frame_length;
|
||||
|
||||
uint8_t try = 1;
|
||||
|
||||
/* To read sensortime, extra 3 bytes are added to fifo buffer */
|
||||
uint16_t fifo_buffer_size = BMI2_FIFO_RAW_DATA_BUFFER_SIZE + SENSORTIME_OVERHEAD_BYTE;
|
||||
|
||||
/* Number of bytes of FIFO data. */
|
||||
uint8_t fifo_data[fifo_buffer_size];
|
||||
|
||||
/* Sensor initialization configuration. */
|
||||
struct bmi2_dev bmi2_dev;
|
||||
|
||||
/* Array of accelerometer frames -> Total bytes =
|
||||
* 55 * (6 axes bytes) = 330 bytes */
|
||||
struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } };
|
||||
|
||||
/* Array of gyro frames -> Total bytes =
|
||||
* 55 * (6 axes bytes) = 330 bytes */
|
||||
struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } };
|
||||
|
||||
/* Initialize FIFO frame structure. */
|
||||
struct bmi2_fifo_frame fifoframe = { 0 };
|
||||
|
||||
/* Accel and gyro sensor are listed in array. */
|
||||
uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO };
|
||||
|
||||
/* Variable to get fifo water-mark interrupt status. */
|
||||
uint16_t int_status = 0;
|
||||
|
||||
uint16_t watermark = 0;
|
||||
|
||||
/* Interface reference is given as a parameter
|
||||
* For I2C : BMI2_I2C_INTF
|
||||
* For SPI : BMI2_SPI_INTF
|
||||
*/
|
||||
rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initialize bmi270. */
|
||||
rslt = bmi270_init(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Enable accel and gyro sensor. */
|
||||
rslt = bmi270_sensor_enable(sensor_sel, 2, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Configuration settings for accel and gyro. */
|
||||
rslt = set_accel_gyro_config(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Before setting FIFO, disable the advance power save mode. */
|
||||
rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initially disable all configurations in fifo. */
|
||||
rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Update FIFO structure. */
|
||||
/* Mapping the buffer to store the fifo data. */
|
||||
fifoframe.data = fifo_data;
|
||||
|
||||
/* Length of FIFO frame. */
|
||||
/* To read sensortime, extra 3 bytes are added to fifo user length. */
|
||||
fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH + SENSORTIME_OVERHEAD_BYTE;
|
||||
|
||||
/* Set FIFO configuration by enabling accel, gyro and timestamp.
|
||||
* NOTE 1: The header mode is enabled by default.
|
||||
* NOTE 2: By default the FIFO operating mode is FIFO mode.
|
||||
* NOTE 3: Sensortime is enabled by default */
|
||||
printf("FIFO is configured in header mode\n");
|
||||
rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* FIFO water-mark interrupt is enabled. */
|
||||
fifoframe.data_int_map = BMI2_FWM_INT;
|
||||
|
||||
/* Map water-mark interrupt to the required interrupt pin. */
|
||||
rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Set water-mark level. */
|
||||
fifoframe.wm_lvl = BMI270_FIFO_WATERMARK_LEVEL;
|
||||
|
||||
/* Set the water-mark level if water-mark interrupt is mapped. */
|
||||
rslt = bmi2_set_fifo_wm(fifoframe.wm_lvl, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
rslt = bmi2_get_fifo_wm(&watermark, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
printf("\nFIFO watermark level is %d\n", watermark);
|
||||
|
||||
while (try <= 10)
|
||||
{
|
||||
/* Read FIFO data on interrupt. */
|
||||
rslt = bmi2_get_int_status(&int_status, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* To check the status of FIFO watermark interrupt. */
|
||||
if ((rslt == BMI2_OK) && (int_status & BMI2_FWM_INT_STATUS_MASK))
|
||||
{
|
||||
printf("\nIteration : %d\n", try);
|
||||
|
||||
printf("\nWatermark interrupt occurred\n");
|
||||
|
||||
rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT;
|
||||
gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT;
|
||||
|
||||
printf("\nFIFO data bytes available : %d \n", fifo_length);
|
||||
printf("\nFIFO data bytes requested : %d \n", fifoframe.length);
|
||||
|
||||
/* Read FIFO data. */
|
||||
rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Read FIFO data on interrupt. */
|
||||
rslt = bmi2_get_int_status(&int_status, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
printf("\nFIFO accel frames requested : %d \n", accel_frame_length);
|
||||
|
||||
/* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */
|
||||
rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev);
|
||||
printf("\nFIFO accel frames extracted : %d \n", accel_frame_length);
|
||||
|
||||
printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length);
|
||||
|
||||
/* Parse the FIFO data to extract gyro data from the FIFO buffer. */
|
||||
rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev);
|
||||
printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length);
|
||||
|
||||
/* Calculating the frame count from the available bytes in FIFO
|
||||
* frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len + header_byte)) */
|
||||
frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH + 1));
|
||||
printf("\nAvailable frame count from available bytes: %d\n", frame_count);
|
||||
|
||||
printf("\nExracted accel frames\n");
|
||||
|
||||
if (accel_frame_length > frame_count)
|
||||
{
|
||||
accel_frame_length = frame_count;
|
||||
}
|
||||
|
||||
/* Print the parsed accelerometer data from the FIFO buffer. */
|
||||
for (index = 0; index < accel_frame_length; index++)
|
||||
{
|
||||
printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x,
|
||||
fifo_accel_data[index].y, fifo_accel_data[index].z);
|
||||
}
|
||||
|
||||
printf("\nExtracted gyro frames\n");
|
||||
|
||||
if (gyro_frame_length > frame_count)
|
||||
{
|
||||
gyro_frame_length = frame_count;
|
||||
}
|
||||
|
||||
/* Print the parsed gyro data from the FIFO buffer. */
|
||||
for (index = 0; index < gyro_frame_length; index++)
|
||||
{
|
||||
printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n",
|
||||
index,
|
||||
fifo_gyro_data[index].x,
|
||||
fifo_gyro_data[index].y,
|
||||
fifo_gyro_data[index].z);
|
||||
}
|
||||
|
||||
/* Print control frames like sensor time and skipped frame count. */
|
||||
printf("Skipped frame count = %d\n", fifoframe.skipped_frame_count);
|
||||
printf("Sensor time = %lu\r\n", fifoframe.sensor_time);
|
||||
|
||||
try++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bmi2_coines_deinit();
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for accel and gyro.
|
||||
*/
|
||||
static int8_t set_accel_gyro_config(struct bmi2_dev *dev)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Structure to define accel and gyro configurations. */
|
||||
struct bmi2_sens_config config[2];
|
||||
|
||||
/* Configure the type of feature. */
|
||||
config[0].type = BMI2_ACCEL;
|
||||
config[1].type = BMI2_GYRO;
|
||||
|
||||
/* Get default configurations for the type of feature selected. */
|
||||
rslt = bmi270_get_sensor_config(config, 2, dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* NOTE: The user can change the following configuration parameters according to their requirement. */
|
||||
/* Accel configuration settings. */
|
||||
/* Set Output Data Rate */
|
||||
config[0].cfg.acc.odr = BMI2_ACC_ODR_25HZ;
|
||||
|
||||
/* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */
|
||||
config[0].cfg.acc.range = BMI2_ACC_RANGE_2G;
|
||||
|
||||
/* The bandwidth parameter is used to configure the number of sensor samples that are averaged
|
||||
* if it is set to 2, then 2^(bandwidth parameter) samples
|
||||
* are averaged, resulting in 4 averaged samples
|
||||
* Note1 : For more information, refer the datasheet.
|
||||
* Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but
|
||||
* this has an adverse effect on the power consumed.
|
||||
*/
|
||||
config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4;
|
||||
|
||||
/* Enable the filter performance mode where averaging of samples
|
||||
* will be done based on above set bandwidth and ODR.
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode
|
||||
* 1 -> High performance mode(Default)
|
||||
* For more info refer datasheet.
|
||||
*/
|
||||
config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE;
|
||||
|
||||
/* Gyro configuration settings. */
|
||||
/* Set Output Data Rate */
|
||||
config[1].cfg.gyr.odr = BMI2_GYR_ODR_25HZ;
|
||||
|
||||
/* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */
|
||||
config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000;
|
||||
|
||||
/* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */
|
||||
config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE;
|
||||
|
||||
/* Enable/Disable the noise performance mode for precision yaw rate sensing
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode(Default)
|
||||
* 1 -> High performance mode
|
||||
*/
|
||||
config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE;
|
||||
|
||||
/* Enable/Disable the filter performance mode where averaging of samples
|
||||
* will be done based on above set bandwidth and ODR.
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode
|
||||
* 1 -> High performance mode(Default)
|
||||
*/
|
||||
config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE;
|
||||
|
||||
/* Set new configurations. */
|
||||
rslt = bmi270_set_sensor_config(config, 2, dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
}
|
||||
|
||||
return rslt;
|
||||
}
|
|
@ -0,0 +1,18 @@
|
|||
COINES_INSTALL_PATH ?= ../../../..
|
||||
|
||||
EXAMPLE_FILE ?= fifo_watermark_headerless_mode.c
|
||||
|
||||
API_LOCATION ?= ../..
|
||||
|
||||
COMMON_LOCATION ?= ..
|
||||
|
||||
C_SRCS += \
|
||||
$(API_LOCATION)/bmi2.c \
|
||||
$(API_LOCATION)/bmi270.c \
|
||||
$(COMMON_LOCATION)/common/common.c
|
||||
|
||||
INCLUDEPATHS += \
|
||||
$(API_LOCATION) \
|
||||
$(COMMON_LOCATION)/common
|
||||
|
||||
include $(COINES_INSTALL_PATH)/coines.mk
|
|
@ -0,0 +1,331 @@
|
|||
/**\
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
**/
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Header Files */
|
||||
#include <stdio.h>
|
||||
#include "bmi270.h"
|
||||
#include "common.h"
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Macros */
|
||||
|
||||
/*! Buffer size allocated to store raw FIFO data */
|
||||
#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048)
|
||||
|
||||
/*! Length of data to be read from FIFO */
|
||||
#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048)
|
||||
|
||||
/*! Number of accel frames to be extracted from FIFO */
|
||||
|
||||
/*!
|
||||
* Calculation:
|
||||
* fifo_watermark_level = 650, accel_frame_len = 6, gyro_frame_len = 6.
|
||||
* fifo_accel_frame_count = (650 / (6 + 6 )) = 54 frames
|
||||
*/
|
||||
#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(55)
|
||||
|
||||
/*! Number of gyro frames to be extracted from FIFO */
|
||||
#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(55)
|
||||
|
||||
/*! Setting a watermark level in FIFO */
|
||||
#define BMI270_FIFO_WATERMARK_LEVEL UINT16_C(650)
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Static Function Declaration */
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for accel and gyro.
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
* @return Status of execution.
|
||||
*/
|
||||
static int8_t set_accel_gyro_config(struct bmi2_dev *dev);
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Functions */
|
||||
|
||||
/* This function starts the execution of program. */
|
||||
int main(void)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Variable to index bytes. */
|
||||
uint16_t index = 0;
|
||||
|
||||
/* Variable to store the available frame length count in FIFO */
|
||||
uint8_t frame_count;
|
||||
|
||||
uint16_t fifo_length = 0;
|
||||
|
||||
uint16_t accel_frame_length;
|
||||
|
||||
uint16_t gyro_frame_length;
|
||||
|
||||
uint8_t try = 1;
|
||||
|
||||
/* Number of bytes of FIFO data. */
|
||||
uint8_t fifo_data[BMI2_FIFO_RAW_DATA_BUFFER_SIZE] = { 0 };
|
||||
|
||||
/* Sensor initialization configuration. */
|
||||
struct bmi2_dev bmi2_dev;
|
||||
|
||||
/* Array of accelerometer frames -> Total bytes =
|
||||
* 50 * (6 axes bytes) = 300 bytes */
|
||||
struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } };
|
||||
|
||||
/* Array of gyro frames -> Total bytes =
|
||||
* 50 * (6 axes bytes) = 300 bytes */
|
||||
struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } };
|
||||
|
||||
/* Initialize FIFO frame structure. */
|
||||
struct bmi2_fifo_frame fifoframe = { 0 };
|
||||
|
||||
/* Accel and gyro sensor are listed in array. */
|
||||
uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO };
|
||||
|
||||
/* Variable to get fifo water-mark interrupt status. */
|
||||
uint16_t int_status = 0;
|
||||
|
||||
uint16_t watermark = 0;
|
||||
|
||||
/* Interface reference is given as a parameter
|
||||
* For I2C : BMI2_I2C_INTF
|
||||
* For SPI : BMI2_SPI_INTF
|
||||
*/
|
||||
rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initialize bmi270. */
|
||||
rslt = bmi270_init(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Enable accel and gyro sensor. */
|
||||
rslt = bmi270_sensor_enable(sensor_sel, 2, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Configuration settings for accel and gyro. */
|
||||
rslt = set_accel_gyro_config(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Before setting FIFO, disable the advance power save mode. */
|
||||
rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initially disable all configurations in fifo. */
|
||||
rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Update FIFO structure. */
|
||||
/* Mapping the buffer to store the fifo data. */
|
||||
fifoframe.data = fifo_data;
|
||||
|
||||
/* Length of FIFO frame. */
|
||||
fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH;
|
||||
|
||||
/* Set FIFO configuration by enabling accel, gyro.
|
||||
* NOTE 1: The header mode is enabled by default.
|
||||
* NOTE 2: By default the FIFO operating mode is FIFO mode. */
|
||||
rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
printf("FIFO is configured in headerless mode\n");
|
||||
|
||||
/* To enable headerless mode, disable the header. */
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
rslt = bmi2_set_fifo_config(BMI2_FIFO_HEADER_EN, BMI2_DISABLE, &bmi2_dev);
|
||||
}
|
||||
|
||||
/* FIFO water-mark interrupt is enabled. */
|
||||
fifoframe.data_int_map = BMI2_FWM_INT;
|
||||
|
||||
/* Map water-mark interrupt to the required interrupt pin. */
|
||||
rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Set water-mark level. */
|
||||
fifoframe.wm_lvl = BMI270_FIFO_WATERMARK_LEVEL;
|
||||
|
||||
fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH;
|
||||
|
||||
/* Set the water-mark level if water-mark interrupt is mapped. */
|
||||
rslt = bmi2_set_fifo_wm(fifoframe.wm_lvl, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
rslt = bmi2_get_fifo_wm(&watermark, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
printf("\nFIFO watermark level is %d\n", watermark);
|
||||
|
||||
while (try <= 10)
|
||||
{
|
||||
/* Read FIFO data on interrupt. */
|
||||
rslt = bmi2_get_int_status(&int_status, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* To check the status of FIFO watermark interrupt. */
|
||||
if ((rslt == BMI2_OK) && (int_status & BMI2_FWM_INT_STATUS_MASK))
|
||||
{
|
||||
printf("\nIteration : %d\n", try);
|
||||
|
||||
printf("\nWatermark interrupt occurred\n");
|
||||
|
||||
rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT;
|
||||
gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT;
|
||||
|
||||
printf("\nFIFO data bytes available : %d \n", fifo_length);
|
||||
printf("\nFIFO data bytes requested : %d \n", fifoframe.length);
|
||||
|
||||
/* Read FIFO data. */
|
||||
rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Read FIFO data on interrupt. */
|
||||
rslt = bmi2_get_int_status(&int_status, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
printf("\nFIFO accel frames requested : %d \n", accel_frame_length);
|
||||
|
||||
/* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */
|
||||
rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev);
|
||||
printf("\nFIFO accel frames extracted : %d \n", accel_frame_length);
|
||||
|
||||
printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length);
|
||||
|
||||
/* Parse the FIFO data to extract gyro data from the FIFO buffer. */
|
||||
rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev);
|
||||
printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length);
|
||||
|
||||
/* Calculating the frame count from the available bytes in FIFO
|
||||
* frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len)) */
|
||||
frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH));
|
||||
printf("\nAvailable frame count from available bytes: %d\n", frame_count);
|
||||
|
||||
printf("\nExracted accel frames\n");
|
||||
|
||||
if (accel_frame_length > frame_count)
|
||||
{
|
||||
accel_frame_length = frame_count;
|
||||
}
|
||||
|
||||
/* Print the parsed accelerometer data from the FIFO buffer. */
|
||||
for (index = 0; index < accel_frame_length; index++)
|
||||
{
|
||||
printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x,
|
||||
fifo_accel_data[index].y, fifo_accel_data[index].z);
|
||||
}
|
||||
|
||||
printf("\nExtracted gyro frames\n");
|
||||
|
||||
if (gyro_frame_length > frame_count)
|
||||
{
|
||||
gyro_frame_length = frame_count;
|
||||
}
|
||||
|
||||
/* Print the parsed gyro data from the FIFO buffer. */
|
||||
for (index = 0; index < gyro_frame_length; index++)
|
||||
{
|
||||
printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n",
|
||||
index,
|
||||
fifo_gyro_data[index].x,
|
||||
fifo_gyro_data[index].y,
|
||||
fifo_gyro_data[index].z);
|
||||
}
|
||||
}
|
||||
|
||||
try++;
|
||||
}
|
||||
}
|
||||
|
||||
bmi2_coines_deinit();
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for accel.
|
||||
*/
|
||||
static int8_t set_accel_gyro_config(struct bmi2_dev *dev)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Structure to define accel and gyro configurations. */
|
||||
struct bmi2_sens_config config[2];
|
||||
|
||||
/* Configure the type of feature. */
|
||||
config[0].type = BMI2_ACCEL;
|
||||
config[1].type = BMI2_GYRO;
|
||||
|
||||
/* Get default configurations for the type of feature selected. */
|
||||
rslt = bmi270_get_sensor_config(config, 2, dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* NOTE: The user can change the following configuration parameter according to their requirement. */
|
||||
/* Accel configuration settings. */
|
||||
/* Set Output Data Rate */
|
||||
config[0].cfg.acc.odr = BMI2_ACC_ODR_100HZ;
|
||||
|
||||
/* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */
|
||||
config[0].cfg.acc.range = BMI2_ACC_RANGE_2G;
|
||||
|
||||
/* The bandwidth parameter is used to configure the number of sensor samples that are averaged
|
||||
* if it is set to 2, then 2^(bandwidth parameter) samples
|
||||
* are averaged, resulting in 4 averaged samples
|
||||
* Note1 : For more information, refer the datasheet.
|
||||
* Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but
|
||||
* this has an adverse effect on the power consumed.
|
||||
*/
|
||||
config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4;
|
||||
|
||||
/* Enable the filter performance mode where averaging of samples
|
||||
* will be done based on above set bandwidth and ODR.
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode
|
||||
* 1 -> High performance mode(Default)
|
||||
* For more info refer datasheet.
|
||||
*/
|
||||
config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE;
|
||||
|
||||
/* Gyro configuration settings. */
|
||||
/* Set Output Data Rate */
|
||||
config[1].cfg.gyr.odr = BMI2_GYR_ODR_100HZ;
|
||||
|
||||
/* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */
|
||||
config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000;
|
||||
|
||||
/* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */
|
||||
config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE;
|
||||
|
||||
/* Enable/Disable the noise performance mode for precision yaw rate sensing
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode(Default)
|
||||
* 1 -> High performance mode
|
||||
*/
|
||||
config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE;
|
||||
|
||||
/* Enable/Disable the filter performance mode where averaging of samples
|
||||
* will be done based on above set bandwidth and ODR.
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode
|
||||
* 1 -> High performance mode(Default)
|
||||
*/
|
||||
config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE;
|
||||
|
||||
/* Set new configurations. */
|
||||
rslt = bmi270_set_sensor_config(config, 2, dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
}
|
||||
|
||||
return rslt;
|
||||
}
|
|
@ -0,0 +1,18 @@
|
|||
COINES_INSTALL_PATH ?= ../../../..
|
||||
|
||||
EXAMPLE_FILE ?= gyro.c
|
||||
|
||||
API_LOCATION ?= ../..
|
||||
|
||||
COMMON_LOCATION ?= ..
|
||||
|
||||
C_SRCS += \
|
||||
$(API_LOCATION)/bmi2.c \
|
||||
$(API_LOCATION)/bmi270.c \
|
||||
$(COMMON_LOCATION)/common/common.c
|
||||
|
||||
INCLUDEPATHS += \
|
||||
$(API_LOCATION) \
|
||||
$(COMMON_LOCATION)/common
|
||||
|
||||
include $(COINES_INSTALL_PATH)/coines.mk
|
|
@ -0,0 +1,195 @@
|
|||
/**\
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
**/
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Header Files */
|
||||
#include <stdio.h>
|
||||
#include "bmi270.h"
|
||||
#include "common.h"
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Static Function Declaration */
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for gyro.
|
||||
*
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @return Status of execution.
|
||||
*/
|
||||
static int8_t set_gyro_config(struct bmi2_dev *dev);
|
||||
|
||||
/*!
|
||||
* @brief This function converts lsb to degree per second for 16 bit gyro at
|
||||
* range 125, 250, 500, 1000 or 2000dps.
|
||||
*
|
||||
* @param[in] val : LSB from each axis.
|
||||
* @param[in] dps : Degree per second.
|
||||
* @param[in] bit_width : Resolution for gyro.
|
||||
*
|
||||
* @return Degree per second.
|
||||
*/
|
||||
static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width);
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Functions */
|
||||
|
||||
/* This function starts the execution of program. */
|
||||
int main(void)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Variable to define limit to print gyro data. */
|
||||
uint8_t limit = 10;
|
||||
|
||||
uint8_t indx = 0;
|
||||
|
||||
float x = 0, y = 0, z = 0;
|
||||
|
||||
/* Sensor initialization configuration. */
|
||||
struct bmi2_dev bmi2_dev;
|
||||
|
||||
/* Create an instance of sensor data structure. */
|
||||
struct bmi2_sensor_data sensor_data = { 0 };
|
||||
|
||||
/* Assign gyro sensor to variable. */
|
||||
uint8_t sens_list = BMI2_GYRO;
|
||||
|
||||
/* Initialize the interrupt status of gyro. */
|
||||
uint16_t int_status = 0;
|
||||
|
||||
/* Interface reference is given as a parameter
|
||||
* For I2C : BMI2_I2C_INTF
|
||||
* For SPI : BMI2_SPI_INTF
|
||||
*/
|
||||
rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initialize bmi270. */
|
||||
rslt = bmi270_init(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Enable the selected sensors. */
|
||||
rslt = bmi270_sensor_enable(&sens_list, 1, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Gyro configuration settings. */
|
||||
rslt = set_gyro_config(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Select gyro sensor. */
|
||||
sensor_data.type = BMI2_GYRO;
|
||||
printf("Gyro and DPS data\n");
|
||||
printf("Gyro data collected at Range 2000 dps with 16-bit resolution\n");
|
||||
|
||||
/* Loop to print gyro data when interrupt occurs. */
|
||||
while (indx <= limit)
|
||||
{
|
||||
/* To get the data ready interrupt status of gyro. */
|
||||
rslt = bmi2_get_int_status(&int_status, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* To check the data ready interrupt status and print the status for 10 samples. */
|
||||
if (int_status & BMI2_GYR_DRDY_INT_MASK)
|
||||
{
|
||||
/* Get gyro data for x, y and z axis. */
|
||||
rslt = bmi270_get_sensor_data(&sensor_data, 1, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
printf("\nGyr_X = %d\t", sensor_data.sens_data.gyr.x);
|
||||
printf("Gyr_Y = %d\t", sensor_data.sens_data.gyr.y);
|
||||
printf("Gyr_Z = %d\r\n", sensor_data.sens_data.gyr.z);
|
||||
|
||||
/* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */
|
||||
x = lsb_to_dps(sensor_data.sens_data.gyr.x, 2000, bmi2_dev.resolution);
|
||||
y = lsb_to_dps(sensor_data.sens_data.gyr.y, 2000, bmi2_dev.resolution);
|
||||
z = lsb_to_dps(sensor_data.sens_data.gyr.z, 2000, bmi2_dev.resolution);
|
||||
|
||||
/* Print the data in dps. */
|
||||
printf("Gyr_DPS_X = %4.2f, Gyr_DPS_Y = %4.2f, Gyr_DPS_Z = %4.2f\n", x, y, z);
|
||||
|
||||
indx++;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bmi2_coines_deinit();
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for gyro.
|
||||
*/
|
||||
static int8_t set_gyro_config(struct bmi2_dev *dev)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Structure to define the type of sensor and its configurations. */
|
||||
struct bmi2_sens_config config;
|
||||
|
||||
/* Configure the type of feature. */
|
||||
config.type = BMI2_GYRO;
|
||||
|
||||
/* Get default configurations for the type of feature selected. */
|
||||
rslt = bmi270_get_sensor_config(&config, 1, dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Map data ready interrupt to interrupt pin. */
|
||||
rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT2, dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* The user can change the following configuration parameters according to their requirement. */
|
||||
/* Set Output Data Rate */
|
||||
config.cfg.gyr.odr = BMI2_GYR_ODR_100HZ;
|
||||
|
||||
/* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */
|
||||
config.cfg.gyr.range = BMI2_GYR_RANGE_2000;
|
||||
|
||||
/* Gyroscope bandwidth parameters. By default the gyro bandwidth is in normal mode. */
|
||||
config.cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE;
|
||||
|
||||
/* Enable/Disable the noise performance mode for precision yaw rate sensing
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode(Default)
|
||||
* 1 -> High performance mode
|
||||
*/
|
||||
config.cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE;
|
||||
|
||||
/* Enable/Disable the filter performance mode where averaging of samples
|
||||
* will be done based on above set bandwidth and ODR.
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode
|
||||
* 1 -> High performance mode(Default)
|
||||
*/
|
||||
config.cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE;
|
||||
|
||||
/* Set the gyro configurations. */
|
||||
rslt = bmi270_set_sensor_config(&config, 1, dev);
|
||||
}
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This function converts lsb to degree per second for 16 bit gyro at
|
||||
* range 125, 250, 500, 1000 or 2000dps.
|
||||
*/
|
||||
static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width)
|
||||
{
|
||||
float half_scale = ((float)(1 << bit_width) / 2.0f);
|
||||
|
||||
return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val);
|
||||
}
|
|
@ -0,0 +1,18 @@
|
|||
COINES_INSTALL_PATH ?= ../../../..
|
||||
|
||||
EXAMPLE_FILE ?= no_motion_interrupt.c
|
||||
|
||||
API_LOCATION ?= ../..
|
||||
|
||||
COMMON_LOCATION ?= ..
|
||||
|
||||
C_SRCS += \
|
||||
$(API_LOCATION)/bmi2.c \
|
||||
$(API_LOCATION)/bmi270.c \
|
||||
$(COMMON_LOCATION)/common/common.c
|
||||
|
||||
INCLUDEPATHS += \
|
||||
$(API_LOCATION) \
|
||||
$(COMMON_LOCATION)/common
|
||||
|
||||
include $(COINES_INSTALL_PATH)/coines.mk
|
|
@ -0,0 +1,134 @@
|
|||
/**\
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
**/
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Header Files */
|
||||
#include <stdio.h>
|
||||
#include "bmi270.h"
|
||||
#include "common.h"
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Static Function Declaration */
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for no-motion.
|
||||
*
|
||||
* @param[in] bmi2_dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @return Status of execution.
|
||||
*/
|
||||
static int8_t set_feature_config(struct bmi2_dev *bmi2_dev);
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Functions */
|
||||
|
||||
/* This function starts the execution of program. */
|
||||
int main(void)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Accel sensor and no-motion feature are listed in array. */
|
||||
uint8_t sens_list[2] = { BMI2_ACCEL, BMI2_NO_MOTION };
|
||||
|
||||
/* Variable to get no-motion interrupt status. */
|
||||
uint16_t int_status = 0;
|
||||
|
||||
/* Sensor initialization configuration. */
|
||||
struct bmi2_dev bmi2_dev;
|
||||
|
||||
/* Select features and their pins to be mapped to. */
|
||||
struct bmi2_sens_int_config sens_int = { .type = BMI2_NO_MOTION, .hw_int_pin = BMI2_INT1 };
|
||||
|
||||
/* Interface reference is given as a parameter
|
||||
* For I2C : BMI2_I2C_INTF
|
||||
* For SPI : BMI2_SPI_INTF
|
||||
*/
|
||||
rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initialize bmi270. */
|
||||
rslt = bmi270_init(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Enable the selected sensors. */
|
||||
rslt = bmi270_sensor_enable(sens_list, 2, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Set feature configurations for no-motion. */
|
||||
rslt = set_feature_config(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Map the feature interrupt for no-motion. */
|
||||
rslt = bmi270_map_feat_int(&sens_int, 1, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
printf("Do not move the board\n");
|
||||
|
||||
/* Loop to get no-motion interrupt. */
|
||||
do
|
||||
{
|
||||
/* Clear buffer. */
|
||||
int_status = 0;
|
||||
|
||||
/* To get the interrupt status of no-motion. */
|
||||
rslt = bmi2_get_int_status(&int_status, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* To check the interrupt status of no-motion. */
|
||||
if (int_status & BMI270_NO_MOT_STATUS_MASK)
|
||||
{
|
||||
printf("No-motion interrupt is generated\n");
|
||||
break;
|
||||
}
|
||||
} while (rslt == BMI2_OK);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bmi2_coines_deinit();
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for no-motion.
|
||||
*/
|
||||
static int8_t set_feature_config(struct bmi2_dev *bmi2_dev)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Structure to define the type of sensor and its configurations. */
|
||||
struct bmi2_sens_config config;
|
||||
|
||||
/* Configure the type of feature. */
|
||||
config.type = BMI2_NO_MOTION;
|
||||
|
||||
/* Get default configurations for the type of feature selected. */
|
||||
rslt = bmi270_get_sensor_config(&config, 1, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* NOTE: The user can change the following configuration parameters according to their requirement. */
|
||||
/* 1LSB equals 20ms. Default is 100ms, setting to 80ms. */
|
||||
config.cfg.no_motion.duration = 0x04;
|
||||
|
||||
/* 1LSB equals to 0.48mg. Default is 83mg, setting to 50mg. */
|
||||
config.cfg.no_motion.threshold = 0x68;
|
||||
|
||||
/* Set new configurations. */
|
||||
rslt = bmi270_set_sensor_config(&config, 1, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
}
|
||||
|
||||
return rslt;
|
||||
}
|
|
@ -0,0 +1,23 @@
|
|||
COINES_INSTALL_PATH ?= ../../../..
|
||||
|
||||
EXAMPLE_FILE ?= read_aux_data_mode.c
|
||||
|
||||
API_LOCATION ?= ../..
|
||||
|
||||
COMMON_LOCATION ?= ..
|
||||
|
||||
BMM150_SOURCE ?= ../../../bmm150
|
||||
|
||||
C_SRCS += \
|
||||
$(API_LOCATION)/bmi2.c \
|
||||
$(API_LOCATION)/bmi270.c \
|
||||
$(BMM150_SOURCE)/bmm150.c \
|
||||
$(COMMON_LOCATION)/common/common.c
|
||||
|
||||
INCLUDEPATHS += \
|
||||
$(API_LOCATION) \
|
||||
$(BMM150_SOURCE) \
|
||||
$(COMMON_LOCATION)/common
|
||||
|
||||
|
||||
include $(COINES_INSTALL_PATH)/coines.mk
|
|
@ -0,0 +1,327 @@
|
|||
/**\
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
**/
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Header Files */
|
||||
#include <stdio.h>
|
||||
#include "bmi270.h"
|
||||
#include "bmm150.h"
|
||||
#include "coines.h"
|
||||
#include "common.h"
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Macro definition */
|
||||
|
||||
/*! Macros to select the sensors */
|
||||
#define ACCEL UINT8_C(0x00)
|
||||
#define GYRO UINT8_C(0x01)
|
||||
#define AUX UINT8_C(0x02)
|
||||
|
||||
/*! Earth's gravity in m/s^2 */
|
||||
#define GRAVITY_EARTH (9.80665f)
|
||||
|
||||
/*****************************************************************************/
|
||||
/*! Structure declaration */
|
||||
|
||||
/* Sensor initialization configuration. */
|
||||
struct bmi2_dev bmi2_dev;
|
||||
|
||||
/*******************************************************************************/
|
||||
/*! Functions */
|
||||
|
||||
/**
|
||||
* user_aux_read - Reads data from auxiliary sensor in manual mode.
|
||||
*
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[out] aux_data : Aux data pointer to store the read data.
|
||||
* @param[in] length : No of bytes to read.
|
||||
* @param[in] intf_ptr : Interface pointer
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Failure Info
|
||||
*/
|
||||
static int8_t user_aux_read(uint8_t reg_addr, uint8_t *aux_data, uint32_t len, void *intf_ptr);
|
||||
|
||||
/**
|
||||
* user_aux_write - Writes data to the auxiliary sensor in manual mode.
|
||||
*
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[out] aux_data : Aux data pointer to store the data being written.
|
||||
* @param[in] length : No of bytes to read.
|
||||
* @param[in] intf_ptr : Interface pointer
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Failure Info
|
||||
*/
|
||||
static int8_t user_aux_write(uint8_t reg_addr, const uint8_t *aux_data, uint32_t len, void *intf_ptr);
|
||||
|
||||
/*!
|
||||
* @brief This function provides the delay for required time (Microsecond) as per the input provided in some of the
|
||||
* APIs.
|
||||
*
|
||||
* @param[in] period_us : The required wait time in microsecond.
|
||||
* @param[in] intf_ptr : Interface pointer
|
||||
*
|
||||
* @return void.
|
||||
*/
|
||||
static void user_delay_us(uint32_t period, void *intf_ptr);
|
||||
|
||||
/*!
|
||||
* @brief This function converts lsb to meter per second squared for 16 bit accelerometer at
|
||||
* range 2G, 4G, 8G or 16G.
|
||||
*
|
||||
* @param[in] val : LSB from each axis.
|
||||
* @param[in] g_range : Gravity range.
|
||||
* @param[in] bit_width : Resolution for accel.
|
||||
*
|
||||
* @return Gravity.
|
||||
*/
|
||||
static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width);
|
||||
|
||||
/*!
|
||||
* @brief This function converts lsb to degree per second for 16 bit gyro at
|
||||
* range 125, 250, 500, 1000 or 2000dps.
|
||||
*
|
||||
* @param[in] val : LSB from each axis.
|
||||
* @param[in] dps : Degree per second.
|
||||
* @param[in] bit_width : Resolution for gyro.
|
||||
*
|
||||
* @return Degree per second.
|
||||
*/
|
||||
static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width);
|
||||
|
||||
/* This function starts the execution of program. */
|
||||
int main(void)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Variable to select the pull-up resistor which is set to trim register */
|
||||
uint8_t regdata;
|
||||
|
||||
/* Variable to define limit to print aux data. */
|
||||
uint8_t limit = 20;
|
||||
|
||||
/* Accel, Gyro and Aux sensors are listed in array. */
|
||||
uint8_t sensor_list[3] = { BMI2_ACCEL, BMI2_GYRO, BMI2_AUX };
|
||||
|
||||
/* Structure to define the type of the sensor and its configurations. */
|
||||
struct bmi2_sens_config config[3];
|
||||
|
||||
/* Sensor initialization configuration. */
|
||||
struct bmm150_dev bmm150_dev;
|
||||
|
||||
/* bmm150 settings configuration */
|
||||
struct bmm150_settings settings;
|
||||
|
||||
/* bmm150 magnetometer data */
|
||||
struct bmm150_mag_data mag_data;
|
||||
|
||||
/* Structure to define type of sensor and their respective data. */
|
||||
struct bmi2_sensor_data sensor_data[3];
|
||||
|
||||
/* Variables to define read the accel and gyro data in float */
|
||||
float x = 0, y = 0, z = 0;
|
||||
|
||||
config[ACCEL].type = BMI2_ACCEL;
|
||||
config[GYRO].type = BMI2_GYRO;
|
||||
config[AUX].type = BMI2_AUX;
|
||||
|
||||
/* To enable the i2c interface settings for bmm150. */
|
||||
uint8_t bmm150_dev_addr = BMM150_DEFAULT_I2C_ADDRESS;
|
||||
bmm150_dev.intf_ptr = &bmm150_dev_addr;
|
||||
bmm150_dev.read = user_aux_read;
|
||||
bmm150_dev.write = user_aux_write;
|
||||
bmm150_dev.delay_us = user_delay_us;
|
||||
|
||||
/* As per datasheet, aux interface with bmi270 will support only for I2C */
|
||||
bmm150_dev.intf = BMM150_I2C_INTF;
|
||||
|
||||
sensor_data[ACCEL].type = BMI2_ACCEL;
|
||||
sensor_data[GYRO].type = BMI2_GYRO;
|
||||
sensor_data[AUX].type = BMI2_AUX;
|
||||
|
||||
/* Interface reference is given as a parameter
|
||||
* For I2C : BMI2_I2C_INTF
|
||||
* For SPI : BMI2_SPI_INTF
|
||||
*/
|
||||
rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initialize bmi270. */
|
||||
rslt = bmi270_init(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Pull-up resistor 2k is set to the trim regiter */
|
||||
regdata = BMI2_ASDA_PUPSEL_2K;
|
||||
rslt = bmi2_set_regs(BMI2_AUX_IF_TRIM, ®data, 1, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Enable the accel, gyro and aux sensor. */
|
||||
rslt = bmi270_sensor_enable(sensor_list, 3, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Get default configurations for the type of feature selected. */
|
||||
rslt = bmi270_get_sensor_config(config, 3, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Configurations for accel. */
|
||||
config[ACCEL].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE;
|
||||
config[ACCEL].cfg.acc.bwp = BMI2_ACC_OSR2_AVG2;
|
||||
config[ACCEL].cfg.acc.odr = BMI2_ACC_ODR_100HZ;
|
||||
config[ACCEL].cfg.acc.range = BMI2_ACC_RANGE_2G;
|
||||
|
||||
/* Configurations for gyro. */
|
||||
config[GYRO].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE;
|
||||
config[GYRO].cfg.gyr.noise_perf = BMI2_GYR_RANGE_2000;
|
||||
config[GYRO].cfg.gyr.bwp = BMI2_GYR_OSR2_MODE;
|
||||
config[GYRO].cfg.gyr.odr = BMI2_GYR_ODR_100HZ;
|
||||
config[GYRO].cfg.gyr.range = BMI2_GYR_RANGE_2000;
|
||||
config[GYRO].cfg.gyr.ois_range = BMI2_GYR_OIS_2000;
|
||||
|
||||
/* Configurations for aux. */
|
||||
config[AUX].cfg.aux.odr = BMI2_AUX_ODR_100HZ;
|
||||
config[AUX].cfg.aux.aux_en = BMI2_ENABLE;
|
||||
config[AUX].cfg.aux.i2c_device_addr = BMM150_DEFAULT_I2C_ADDRESS;
|
||||
config[AUX].cfg.aux.manual_en = BMI2_ENABLE;
|
||||
config[AUX].cfg.aux.fcu_write_en = BMI2_ENABLE;
|
||||
config[AUX].cfg.aux.man_rd_burst = BMI2_AUX_READ_LEN_3;
|
||||
config[AUX].cfg.aux.read_addr = BMM150_REG_DATA_X_LSB;
|
||||
|
||||
/* Set new configurations for accel, gyro and aux. */
|
||||
rslt = bmi270_set_sensor_config(config, 3, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initialize bmm150. */
|
||||
rslt = bmm150_init(&bmm150_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Set the power mode to normal mode. */
|
||||
settings.pwr_mode = BMM150_POWERMODE_NORMAL;
|
||||
rslt = bmm150_set_op_mode(&settings, &bmm150_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
rslt = bmi270_get_sensor_config(config, 3, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Disable manual mode so that the data mode is enabled. */
|
||||
config[AUX].cfg.aux.manual_en = BMI2_DISABLE;
|
||||
|
||||
/* Set the aux configurations. */
|
||||
rslt = bmi270_set_sensor_config(config, 3, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
printf("MAGNETOMETER, ACCEL, AND GYRO DATA IN DATA MODE\n");
|
||||
|
||||
if (bmm150_dev.chip_id == BMM150_CHIP_ID)
|
||||
{
|
||||
while (limit--)
|
||||
{
|
||||
/* Delay has been added to get aux, accel and gyro data at the interval of every 0.05 second. */
|
||||
bmi2_dev.delay_us(50000, bmi2_dev.intf_ptr);
|
||||
|
||||
rslt = bmi270_get_sensor_data(sensor_data, 3, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
printf("\nAcc_x = %d\t", sensor_data[ACCEL].sens_data.acc.x);
|
||||
printf("Acc_y = %d\t", sensor_data[ACCEL].sens_data.acc.y);
|
||||
printf("Acc_z = %d", sensor_data[ACCEL].sens_data.acc.z);
|
||||
|
||||
/* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */
|
||||
x = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.x, 2, bmi2_dev.resolution);
|
||||
y = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.y, 2, bmi2_dev.resolution);
|
||||
z = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.z, 2, bmi2_dev.resolution);
|
||||
|
||||
/* Print the data in m/s2. */
|
||||
printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z);
|
||||
|
||||
printf("\nGyr_X = %d\t", sensor_data[GYRO].sens_data.gyr.x);
|
||||
printf("Gyr_Y = %d\t", sensor_data[GYRO].sens_data.gyr.y);
|
||||
printf("Gyr_Z= %d", sensor_data[GYRO].sens_data.gyr.z);
|
||||
|
||||
/* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */
|
||||
x = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.x, 2000, bmi2_dev.resolution);
|
||||
y = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.y, 2000, bmi2_dev.resolution);
|
||||
z = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.z, 2000, bmi2_dev.resolution);
|
||||
|
||||
/* Print the data in dps. */
|
||||
printf("\nGyro_DPS_X = %4.2f, Gyro_DPS_Y = %4.2f, Gyro_DPS_Z = %4.2f\n", x, y, z);
|
||||
|
||||
/* Compensating the raw auxiliary data available from the BMM150 API. */
|
||||
rslt = bmm150_aux_mag_data(sensor_data[AUX].sens_data.aux_data, &mag_data, &bmm150_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
printf("\nMag_x_axis = %d \t Mag_y_axis = %d \t Mag_z_axis = %d \t\n",
|
||||
mag_data.x,
|
||||
mag_data.y,
|
||||
mag_data.z);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bmi2_coines_deinit();
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This function reads the data from auxiliary sensor in manual mode.
|
||||
*/
|
||||
static int8_t user_aux_read(uint8_t reg_addr, uint8_t *aux_data, uint32_t len, void *intf_ptr)
|
||||
{
|
||||
int8_t rslt;
|
||||
|
||||
/* Discarding the parameter id as it is redundant */
|
||||
rslt = bmi2_read_aux_man_mode(reg_addr, aux_data, len, &bmi2_dev);
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This function writes the data to auxiliary sensor in manual mode.
|
||||
*/
|
||||
static int8_t user_aux_write(uint8_t reg_addr, const uint8_t *aux_data, uint32_t len, void *intf_ptr)
|
||||
{
|
||||
int8_t rslt;
|
||||
|
||||
/* Discarding the parameter id as it is redundant */
|
||||
rslt = bmi2_write_aux_man_mode(reg_addr, aux_data, len, &bmi2_dev);
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* Delay function map to COINES platform
|
||||
*/
|
||||
static void user_delay_us(uint32_t period, void *intf_ptr)
|
||||
{
|
||||
coines_delay_usec(period);
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This function converts lsb to meter per second squared for 16 bit accelerometer at
|
||||
* range 2G, 4G, 8G or 16G.
|
||||
*/
|
||||
static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width)
|
||||
{
|
||||
float half_scale = ((float)(1 << bit_width) / 2.0f);
|
||||
|
||||
return (GRAVITY_EARTH * val * g_range) / half_scale;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This function converts lsb to degree per second for 16 bit gyro at
|
||||
* range 125, 250, 500, 1000 or 2000dps.
|
||||
*/
|
||||
static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width)
|
||||
{
|
||||
float half_scale = ((float)(1 << bit_width) / 2.0f);
|
||||
|
||||
return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val);
|
||||
}
|
|
@ -0,0 +1,23 @@
|
|||
COINES_INSTALL_PATH ?= ../../../..
|
||||
|
||||
EXAMPLE_FILE ?= read_aux_manual_mode.c
|
||||
|
||||
API_LOCATION ?= ../..
|
||||
|
||||
COMMON_LOCATION ?= ..
|
||||
|
||||
BMM150_SOURCE ?= ../../../bmm150
|
||||
|
||||
C_SRCS += \
|
||||
$(API_LOCATION)/bmi2.c \
|
||||
$(API_LOCATION)/bmi270.c \
|
||||
$(BMM150_SOURCE)/bmm150.c \
|
||||
$(COMMON_LOCATION)/common/common.c
|
||||
|
||||
INCLUDEPATHS += \
|
||||
$(API_LOCATION) \
|
||||
$(BMM150_SOURCE) \
|
||||
$(COMMON_LOCATION)/common
|
||||
|
||||
|
||||
include $(COINES_INSTALL_PATH)/coines.mk
|
|
@ -0,0 +1,322 @@
|
|||
/**\
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
**/
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Header Files */
|
||||
#include <stdio.h>
|
||||
#include "bmi270.h"
|
||||
#include "bmm150.h"
|
||||
#include "coines.h"
|
||||
#include "common.h"
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Macro definition */
|
||||
|
||||
/*! Macros to select the sensors */
|
||||
#define ACCEL UINT8_C(0x00)
|
||||
#define GYRO UINT8_C(0x01)
|
||||
#define AUX UINT8_C(0x02)
|
||||
|
||||
/*! Earth's gravity in m/s^2 */
|
||||
#define GRAVITY_EARTH (9.80665f)
|
||||
|
||||
/*****************************************************************************/
|
||||
/*! Structure declaration */
|
||||
|
||||
/* Sensor initialization configuration. */
|
||||
struct bmi2_dev bmi2_dev;
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Functions */
|
||||
|
||||
/**
|
||||
* user_aux_read - Reads data from auxiliary sensor in manual mode.
|
||||
*
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[out] aux_data : Aux data pointer to store the read data.
|
||||
* @param[in] length : No of bytes to read.
|
||||
* @param[in] intf_ptr : Interface pointer
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Failure Info
|
||||
*/
|
||||
static int8_t user_aux_read(uint8_t reg_addr, uint8_t *aux_data, uint32_t len, void *intf_ptr);
|
||||
|
||||
/**
|
||||
* user_aux_write - Writes data to the auxiliary sensor in manual mode.
|
||||
*
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[out] aux_data : Aux data pointer to store the data being written.
|
||||
* @param[in] length : No of bytes to read.
|
||||
* @param[in] intf_ptr : Interface pointer
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Failure Info
|
||||
*/
|
||||
static int8_t user_aux_write(uint8_t reg_addr, const uint8_t *aux_data, uint32_t len, void *intf_ptr);
|
||||
|
||||
/*!
|
||||
* @brief This function provides the delay for required time (Microsecond) as per the input provided in some of the
|
||||
* APIs.
|
||||
*
|
||||
* @param[in] period_us : The required wait time in microsecond.
|
||||
* @param[in] intf_ptr : Interface pointer
|
||||
*
|
||||
* @return void.
|
||||
*/
|
||||
static void user_delay_us(uint32_t period, void *intf_ptr);
|
||||
|
||||
/*!
|
||||
* @brief This function converts lsb to meter per second squared for 16 bit accelerometer at
|
||||
* range 2G, 4G, 8G or 16G.
|
||||
*
|
||||
* @param[in] val : LSB from each axis.
|
||||
* @param[in] g_range : Gravity range.
|
||||
* @param[in] bit_width : Resolution for accel.
|
||||
*
|
||||
* @return Gravity.
|
||||
*/
|
||||
static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width);
|
||||
|
||||
/*!
|
||||
* @brief This function converts lsb to degree per second for 16 bit gyro at
|
||||
* range 125, 250, 500, 1000 or 2000dps.
|
||||
*
|
||||
* @param[in] val : LSB from each axis.
|
||||
* @param[in] dps : Degree per second.
|
||||
* @param[in] bit_width : Resolution for gyro.
|
||||
*
|
||||
* @return Degree per second.
|
||||
*/
|
||||
static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width);
|
||||
|
||||
/* This function starts the execution of program. */
|
||||
int main(void)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Variable to select the pull-up resistor which is set to trim register */
|
||||
uint8_t regdata;
|
||||
|
||||
/* Variable to define limit to print aux data. */
|
||||
uint8_t limit = 20;
|
||||
|
||||
/* Accel, Gyro and Aux sensors are listed in array. */
|
||||
uint8_t sensor_list[3] = { BMI2_ACCEL, BMI2_GYRO, BMI2_AUX };
|
||||
|
||||
/* Sensor initialization configuration. */
|
||||
struct bmm150_dev bmm150_dev;
|
||||
|
||||
/* bmm150 settings configuration */
|
||||
struct bmm150_settings settings;
|
||||
|
||||
/* bmm150 magnetometer data */
|
||||
struct bmm150_mag_data mag_data;
|
||||
|
||||
/* Structure to define the type of the sensor and its configurations. */
|
||||
struct bmi2_sens_config config[3];
|
||||
|
||||
/* Structure to define type of sensor and their respective data. */
|
||||
struct bmi2_sensor_data sensor_data[2];
|
||||
|
||||
/* Variables to define read the accel and gyro data in float */
|
||||
float x = 0, y = 0, z = 0;
|
||||
|
||||
config[ACCEL].type = BMI2_ACCEL;
|
||||
config[GYRO].type = BMI2_GYRO;
|
||||
config[AUX].type = BMI2_AUX;
|
||||
|
||||
sensor_data[ACCEL].type = BMI2_ACCEL;
|
||||
sensor_data[GYRO].type = BMI2_GYRO;
|
||||
|
||||
/* Array of eight bytes to store x, y, z and r axis aux data. */
|
||||
uint8_t aux_data[8] = { 0 };
|
||||
|
||||
/* To enable the i2c interface settings for bmm150. */
|
||||
uint8_t bmm150_dev_addr = BMM150_DEFAULT_I2C_ADDRESS;
|
||||
bmm150_dev.intf_ptr = &bmm150_dev_addr;
|
||||
bmm150_dev.read = user_aux_read;
|
||||
bmm150_dev.write = user_aux_write;
|
||||
bmm150_dev.delay_us = user_delay_us;
|
||||
|
||||
/* As per datasheet, aux interface with bmi270 will support only for I2C */
|
||||
bmm150_dev.intf = BMM150_I2C_INTF;
|
||||
|
||||
/* Interface reference is given as a parameter
|
||||
* For I2C : BMI2_I2C_INTF
|
||||
* For SPI : BMI2_SPI_INTF
|
||||
*/
|
||||
rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initialize bmi270. */
|
||||
rslt = bmi270_init(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Pull-up resistor 2k is set to the trim register */
|
||||
regdata = BMI2_ASDA_PUPSEL_2K;
|
||||
rslt = bmi2_set_regs(BMI2_AUX_IF_TRIM, ®data, 1, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Enable the accel, gyro and aux sensor. */
|
||||
rslt = bmi270_sensor_enable(sensor_list, 3, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Get default configurations for the type of feature selected. */
|
||||
rslt = bmi270_get_sensor_config(config, 3, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Configurations for accel. */
|
||||
config[ACCEL].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE;
|
||||
config[ACCEL].cfg.acc.bwp = BMI2_ACC_OSR2_AVG2;
|
||||
config[ACCEL].cfg.acc.odr = BMI2_ACC_ODR_100HZ;
|
||||
config[ACCEL].cfg.acc.range = BMI2_ACC_RANGE_2G;
|
||||
|
||||
/* Configurations for gyro. */
|
||||
config[GYRO].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE;
|
||||
config[GYRO].cfg.gyr.noise_perf = BMI2_GYR_RANGE_2000;
|
||||
config[GYRO].cfg.gyr.bwp = BMI2_GYR_OSR2_MODE;
|
||||
config[GYRO].cfg.gyr.odr = BMI2_GYR_ODR_100HZ;
|
||||
config[GYRO].cfg.gyr.range = BMI2_GYR_RANGE_2000;
|
||||
config[GYRO].cfg.gyr.ois_range = BMI2_GYR_OIS_2000;
|
||||
|
||||
/* Configurations for aux. */
|
||||
config[AUX].cfg.aux.odr = BMI2_AUX_ODR_100HZ;
|
||||
config[AUX].cfg.aux.aux_en = BMI2_ENABLE;
|
||||
config[AUX].cfg.aux.i2c_device_addr = BMM150_DEFAULT_I2C_ADDRESS;
|
||||
config[AUX].cfg.aux.manual_en = BMI2_ENABLE;
|
||||
config[AUX].cfg.aux.fcu_write_en = BMI2_ENABLE;
|
||||
config[AUX].cfg.aux.man_rd_burst = BMI2_AUX_READ_LEN_3;
|
||||
|
||||
/* Set new configurations for accel, gyro and aux. */
|
||||
rslt = bmi270_set_sensor_config(config, 3, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initialize bmm150. */
|
||||
rslt = bmm150_init(&bmm150_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Set the power mode to normal mode. */
|
||||
settings.pwr_mode = BMM150_POWERMODE_NORMAL;
|
||||
rslt = bmm150_set_op_mode(&settings, &bmm150_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
printf("MAGNETOMETER, ACCEL AND GYRO DATA IN MANUAL MODE\n");
|
||||
|
||||
if (bmm150_dev.chip_id == BMM150_CHIP_ID)
|
||||
{
|
||||
while (limit--)
|
||||
{
|
||||
/* Delay has been added to get aux, accel and gyro data at the interval of every 0.05 second. */
|
||||
bmi2_dev.delay_us(50000, bmi2_dev.intf_ptr);
|
||||
|
||||
rslt = bmi270_get_sensor_data(sensor_data, 2, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
printf("\nAcc_x = %d\t", sensor_data[ACCEL].sens_data.acc.x);
|
||||
printf("Acc_y = %d\t", sensor_data[ACCEL].sens_data.acc.y);
|
||||
printf("Acc_z = %d", sensor_data[ACCEL].sens_data.acc.z);
|
||||
|
||||
/* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */
|
||||
x = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.x, 2, bmi2_dev.resolution);
|
||||
y = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.y, 2, bmi2_dev.resolution);
|
||||
z = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.z, 2, bmi2_dev.resolution);
|
||||
|
||||
/* Print the data in m/s2. */
|
||||
printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z);
|
||||
|
||||
printf("\nGyr_X = %d\t", sensor_data[GYRO].sens_data.gyr.x);
|
||||
printf("Gyr_Y = %d\t", sensor_data[GYRO].sens_data.gyr.y);
|
||||
printf("Gyr_Z = %d", sensor_data[GYRO].sens_data.gyr.z);
|
||||
|
||||
/* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */
|
||||
x = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.x, 2000, bmi2_dev.resolution);
|
||||
y = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.y, 2000, bmi2_dev.resolution);
|
||||
z = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.z, 2000, bmi2_dev.resolution);
|
||||
|
||||
/* Print the data in dps. */
|
||||
printf("\nGyro_DPS_X = %4.2f, Gyro_DPS_Y = %4.2f, Gyro_DPS_Z = %4.2f\n", x, y, z);
|
||||
}
|
||||
|
||||
/* Read aux data from the bmm150 data registers. */
|
||||
rslt = bmi2_read_aux_man_mode(BMM150_REG_DATA_X_LSB, aux_data, 8, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Compensating the raw auxiliary data available from the BMM150 API. */
|
||||
rslt = bmm150_aux_mag_data(aux_data, &mag_data, &bmm150_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
printf("Mag_x_axis = %d \t Mag_y_axis = %d \t Mag_z_axis = %d \t\n", mag_data.x, mag_data.y,
|
||||
mag_data.z);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bmi2_coines_deinit();
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This function reads the data from auxiliary sensor in manual mode.
|
||||
*/
|
||||
int8_t user_aux_read(uint8_t reg_addr, uint8_t *aux_data, uint32_t len, void *intf_ptr)
|
||||
{
|
||||
int8_t rslt;
|
||||
|
||||
/* Discarding the parameter id as it is redundant */
|
||||
rslt = bmi2_read_aux_man_mode(reg_addr, aux_data, len, &bmi2_dev);
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This function writes the data to auxiliary sensor in manual mode.
|
||||
*/
|
||||
int8_t user_aux_write(uint8_t reg_addr, const uint8_t *aux_data, uint32_t len, void *intf_ptr)
|
||||
{
|
||||
int8_t rslt;
|
||||
|
||||
/* Discarding the parameter id as it is redundant */
|
||||
rslt = bmi2_write_aux_man_mode(reg_addr, aux_data, len, &bmi2_dev);
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* Delay function map to COINES platform
|
||||
*/
|
||||
static void user_delay_us(uint32_t period, void *intf_ptr)
|
||||
{
|
||||
coines_delay_usec(period);
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This function converts lsb to meter per second squared for 16 bit accelerometer at
|
||||
* range 2G, 4G, 8G or 16G.
|
||||
*/
|
||||
static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width)
|
||||
{
|
||||
float half_scale = ((float)(1 << bit_width) / 2.0f);
|
||||
|
||||
return (GRAVITY_EARTH * val * g_range) / half_scale;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This function converts lsb to degree per second for 16 bit gyro at
|
||||
* range 125, 250, 500, 1000 or 2000dps.
|
||||
*/
|
||||
static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width)
|
||||
{
|
||||
float half_scale = ((float)(1 << bit_width) / 2.0f);
|
||||
|
||||
return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val);
|
||||
}
|
|
@ -0,0 +1,18 @@
|
|||
COINES_INSTALL_PATH ?= ../../../..
|
||||
|
||||
EXAMPLE_FILE ?= sig_motion.c
|
||||
|
||||
API_LOCATION ?= ../..
|
||||
|
||||
COMMON_LOCATION ?= ..
|
||||
|
||||
C_SRCS += \
|
||||
$(API_LOCATION)/bmi2.c \
|
||||
$(API_LOCATION)/bmi270.c \
|
||||
$(COMMON_LOCATION)/common/common.c
|
||||
|
||||
INCLUDEPATHS += \
|
||||
$(API_LOCATION) \
|
||||
$(COMMON_LOCATION)/common
|
||||
|
||||
include $(COINES_INSTALL_PATH)/coines.mk
|
|
@ -0,0 +1,93 @@
|
|||
/**\
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
**/
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Header Files */
|
||||
#include <stdio.h>
|
||||
#include "bmi270.h"
|
||||
#include "common.h"
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Functions */
|
||||
|
||||
/* This function starts the execution of program. */
|
||||
int main(void)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Structure to define the type of sensor and its configurations. */
|
||||
struct bmi2_sens_config config;
|
||||
|
||||
/* Accel sensor and significant-motion feature are listed in array. */
|
||||
uint8_t sens_list[2] = { BMI2_ACCEL, BMI2_SIG_MOTION };
|
||||
|
||||
/* Variable to get significant-motion interrupt status. */
|
||||
uint16_t int_status = 0;
|
||||
|
||||
/* Sensor initialization configuration. */
|
||||
struct bmi2_dev bmi2_dev;
|
||||
|
||||
/* Select features and their pins to be mapped to. */
|
||||
struct bmi2_sens_int_config sens_int = { .type = BMI2_SIG_MOTION, .hw_int_pin = BMI2_INT1 };
|
||||
|
||||
/* Interface reference is given as a parameter
|
||||
* For I2C : BMI2_I2C_INTF
|
||||
* For SPI : BMI2_SPI_INTF
|
||||
*/
|
||||
rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initialize bmi270. */
|
||||
rslt = bmi270_init(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Enable the selected sensors. */
|
||||
rslt = bmi270_sensor_enable(sens_list, 2, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Configure the type of feature. */
|
||||
config.type = BMI2_SIG_MOTION;
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Get default configurations for the type of feature selected. */
|
||||
rslt = bmi270_get_sensor_config(&config, 1, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Map the feature interrupt for significant-motion. */
|
||||
rslt = bmi270_map_feat_int(&sens_int, 1, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* By default the significant-motion interrupt occurs when the sensor is in motion for about 5 seconds.
|
||||
* */
|
||||
printf("Move the board for 5 secs in any direction\n");
|
||||
|
||||
do
|
||||
{
|
||||
/* To get the interrupt status of significant-motion. */
|
||||
rslt = bmi2_get_int_status(&int_status, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* To check the interrupt status of significant-motion. */
|
||||
if (int_status & BMI270_SIG_MOT_STATUS_MASK)
|
||||
{
|
||||
printf("Significant motion interrupt is generated\n");
|
||||
break;
|
||||
}
|
||||
} while (rslt == BMI2_OK);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bmi2_coines_deinit();
|
||||
|
||||
return rslt;
|
||||
}
|
|
@ -0,0 +1,18 @@
|
|||
COINES_INSTALL_PATH ?= ../../../..
|
||||
|
||||
EXAMPLE_FILE ?= step_activity.c
|
||||
|
||||
API_LOCATION ?= ../..
|
||||
|
||||
COMMON_LOCATION ?= ..
|
||||
|
||||
C_SRCS += \
|
||||
$(API_LOCATION)/bmi2.c \
|
||||
$(API_LOCATION)/bmi270.c \
|
||||
$(COMMON_LOCATION)/common/common.c
|
||||
|
||||
INCLUDEPATHS += \
|
||||
$(API_LOCATION) \
|
||||
$(COMMON_LOCATION)/common
|
||||
|
||||
include $(COINES_INSTALL_PATH)/coines.mk
|
|
@ -0,0 +1,110 @@
|
|||
/**\
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
**/
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Header Files */
|
||||
#include <stdio.h>
|
||||
#include "bmi270.h"
|
||||
#include "common.h"
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Functions */
|
||||
/* This function starts the execution of program. */
|
||||
int main(void)
|
||||
{
|
||||
/* Sensor initialization configuration. */
|
||||
struct bmi2_dev bmi2_dev;
|
||||
|
||||
/* Structure to define the type of sensor and their respective data. */
|
||||
struct bmi2_sensor_data sensor_data;
|
||||
|
||||
/* Structure to define the type of sensor and its configurations. */
|
||||
struct bmi2_sens_config config;
|
||||
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Accel sensor and step activity feature are listed in array. */
|
||||
uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_STEP_ACTIVITY };
|
||||
|
||||
/* Type of sensor to get step activity data. */
|
||||
sensor_data.type = BMI2_STEP_ACTIVITY;
|
||||
|
||||
/* Variable to get step activity interrupt status. */
|
||||
uint16_t int_status = 0;
|
||||
|
||||
uint16_t loop = 10;
|
||||
|
||||
/* Select features and their pins to be mapped to. */
|
||||
struct bmi2_sens_int_config sens_int = { .type = BMI2_STEP_ACTIVITY, .hw_int_pin = BMI2_INT2 };
|
||||
|
||||
/* The step activities are listed in array. */
|
||||
const char *activity_output[4] = { "BMI2_STILL", "BMI2_WALK", "BMI2_RUN", "BMI2_UNKNOWN" };
|
||||
|
||||
/* Interface reference is given as a parameter
|
||||
* For I2C : BMI2_I2C_INTF
|
||||
* For SPI : BMI2_SPI_INTF
|
||||
*/
|
||||
rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initialize bmi270. */
|
||||
rslt = bmi270_init(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Enable the selected sensor. */
|
||||
rslt = bmi270_sensor_enable(sensor_sel, 2, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Configure the type of sensor. */
|
||||
config.type = BMI2_STEP_ACTIVITY;
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Get default configurations for the type of feature selected. */
|
||||
rslt = bmi270_get_sensor_config(&config, 1, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Map the feature interrupt for step activity. */
|
||||
rslt = bmi270_map_feat_int(&sens_int, 1, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
printf("\nMove the board in steps to perform step activity:\n");
|
||||
|
||||
/* Loop to get step activity. */
|
||||
while (loop)
|
||||
{
|
||||
/* To get the interrupt status of step detector. */
|
||||
rslt = bmi2_get_int_status(&int_status, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* To check the interrupt status of step detector. */
|
||||
if (int_status & BMI270_STEP_ACT_STATUS_MASK)
|
||||
{
|
||||
printf("Step detected\n");
|
||||
|
||||
/* Get step activity output. */
|
||||
rslt = bmi270_get_sensor_data(&sensor_data, 1, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Print the step activity output. */
|
||||
printf("Step activity = %s\n", activity_output[sensor_data.sens_data.activity_output]);
|
||||
|
||||
loop--;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bmi2_coines_deinit();
|
||||
|
||||
return rslt;
|
||||
}
|
|
@ -0,0 +1,18 @@
|
|||
COINES_INSTALL_PATH ?= ../../../..
|
||||
|
||||
EXAMPLE_FILE ?= step_counter.c
|
||||
|
||||
API_LOCATION ?= ../..
|
||||
|
||||
COMMON_LOCATION ?= ..
|
||||
|
||||
C_SRCS += \
|
||||
$(API_LOCATION)/bmi2.c \
|
||||
$(API_LOCATION)/bmi270.c \
|
||||
$(COMMON_LOCATION)/common/common.c
|
||||
|
||||
INCLUDEPATHS += \
|
||||
$(API_LOCATION) \
|
||||
$(COMMON_LOCATION)/common
|
||||
|
||||
include $(COINES_INSTALL_PATH)/coines.mk
|
|
@ -0,0 +1,146 @@
|
|||
/**\
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
**/
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Header Files */
|
||||
#include <stdio.h>
|
||||
#include "bmi270.h"
|
||||
#include "common.h"
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Static function declaration */
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for step counter.
|
||||
*
|
||||
* @param[in] bmi2_dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @return Status of execution.
|
||||
*/
|
||||
static int8_t set_feature_config(struct bmi2_dev *bmi2_dev);
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Functions */
|
||||
|
||||
/* This function starts the execution of program. */
|
||||
int main(void)
|
||||
{
|
||||
/* Sensor initialization configuration. */
|
||||
struct bmi2_dev bmi2_dev;
|
||||
|
||||
/* Structure to define type of sensor and their respective data. */
|
||||
struct bmi2_sensor_data sensor_data;
|
||||
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Accel sensor and step counter feature are listed in array. */
|
||||
uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_STEP_COUNTER };
|
||||
|
||||
/* Variable to get step counter interrupt status. */
|
||||
uint16_t int_status = 0;
|
||||
|
||||
/* Select features and their pins to be mapped to. */
|
||||
struct bmi2_sens_int_config sens_int = { .type = BMI2_STEP_COUNTER, .hw_int_pin = BMI2_INT2 };
|
||||
|
||||
/* Type of sensor to get step counter data. */
|
||||
sensor_data.type = BMI2_STEP_COUNTER;
|
||||
|
||||
/* Interface reference is given as a parameter
|
||||
* For I2C : BMI2_I2C_INTF
|
||||
* For SPI : BMI2_SPI_INTF
|
||||
*/
|
||||
rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initialize bmi270. */
|
||||
rslt = bmi270_init(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Enable the selected sensor. */
|
||||
rslt = bmi270_sensor_enable(sensor_sel, 2, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Set the feature configuration for step counter. */
|
||||
rslt = set_feature_config(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
printf("Step counter watermark level set to 1 (20 steps)\n");
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Map the step counter feature interrupt. */
|
||||
rslt = bmi270_map_feat_int(&sens_int, 1, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Move the board in steps for 20 times to get step counter interrupt. */
|
||||
printf("Move the board in steps\n");
|
||||
|
||||
/* Loop to get number of steps counted. */
|
||||
do
|
||||
{
|
||||
/* To get the interrupt status of the step counter. */
|
||||
rslt = bmi2_get_int_status(&int_status, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* To check the interrupt status of the step counter. */
|
||||
if (int_status & BMI270_STEP_CNT_STATUS_MASK)
|
||||
{
|
||||
printf("Step counter interrupt occurred when watermark level (20 steps) is reached\n");
|
||||
|
||||
/* Get step counter output. */
|
||||
rslt = bmi270_get_sensor_data(&sensor_data, 1, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Print the step counter output. */
|
||||
printf("No of steps counted = %ld", sensor_data.sens_data.step_counter_output);
|
||||
break;
|
||||
}
|
||||
} while (rslt == BMI2_OK);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bmi2_coines_deinit();
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for step counter.
|
||||
*/
|
||||
static int8_t set_feature_config(struct bmi2_dev *bmi2_dev)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Structure to define the type of sensor and its configurations. */
|
||||
struct bmi2_sens_config config;
|
||||
|
||||
/* Configure the type of sensor. */
|
||||
config.type = BMI2_STEP_COUNTER;
|
||||
|
||||
/* Get default configurations for the type of feature selected. */
|
||||
rslt = bmi270_get_sensor_config(&config, 1, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Setting water-mark level to 1 for step counter to get interrupt after 20 step counts. Every 20 steps once
|
||||
* output triggers. */
|
||||
config.cfg.step_counter.watermark_level = 1;
|
||||
|
||||
/* Set new configuration. */
|
||||
rslt = bmi270_set_sensor_config(&config, 1, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
}
|
||||
|
||||
return rslt;
|
||||
}
|
|
@ -0,0 +1,18 @@
|
|||
COINES_INSTALL_PATH ?= ../../../..
|
||||
|
||||
EXAMPLE_FILE ?= wrist_gesture.c
|
||||
|
||||
API_LOCATION ?= ../..
|
||||
|
||||
COMMON_LOCATION ?= ..
|
||||
|
||||
C_SRCS += \
|
||||
$(API_LOCATION)/bmi2.c \
|
||||
$(API_LOCATION)/bmi270.c \
|
||||
$(COMMON_LOCATION)/common/common.c
|
||||
|
||||
INCLUDEPATHS += \
|
||||
$(API_LOCATION) \
|
||||
$(COMMON_LOCATION)/common
|
||||
|
||||
include $(COINES_INSTALL_PATH)/coines.mk
|
|
@ -0,0 +1,147 @@
|
|||
/**\
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
**/
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Header Files */
|
||||
#include <stdio.h>
|
||||
#include "bmi270.h"
|
||||
#include "common.h"
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Static function declaration */
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set the sensor configuration.
|
||||
*
|
||||
* @param[in] bmi2_dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @return Status of execution.
|
||||
*/
|
||||
static int8_t bmi2_set_config(struct bmi2_dev *bmi2_dev);
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Functions */
|
||||
|
||||
/* This function starts the execution of program */
|
||||
int main(void)
|
||||
{
|
||||
/* Variable to define result */
|
||||
int8_t rslt;
|
||||
|
||||
/* Initialize status of wrist gesture interrupt */
|
||||
uint16_t int_status = 0;
|
||||
|
||||
/* Sensor initialization configuration. */
|
||||
struct bmi2_dev bmi2_dev;
|
||||
|
||||
/* Select features and their pins to be mapped to */
|
||||
struct bmi2_sens_int_config sens_int = { .type = BMI2_WRIST_GESTURE, .hw_int_pin = BMI2_INT1 };
|
||||
|
||||
/* Sensor data structure */
|
||||
struct bmi2_sensor_data sens_data = { 0 };
|
||||
|
||||
/* The gesture movements are listed in array */
|
||||
const char *gesture_output[6] =
|
||||
{ "unknown_gesture", "push_arm_down", "pivot_up", "wrist_shake_jiggle", "flick_in", "flick_out" };
|
||||
|
||||
/* Interface reference is given as a parameter
|
||||
* For I2C : BMI2_I2C_INTF
|
||||
* For SPI : BMI2_SPI_INTF
|
||||
*/
|
||||
rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initialize bmi270. */
|
||||
rslt = bmi270_init(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
sens_data.type = BMI2_WRIST_GESTURE;
|
||||
|
||||
/* Set the sensor configuration */
|
||||
rslt = bmi2_set_config(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Map the feature interrupt */
|
||||
rslt = bmi270_map_feat_int(&sens_int, 1, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
printf("Flip the board in portrait/landscape mode:\n");
|
||||
|
||||
/* Loop to print the wrist gesture data when interrupt occurs */
|
||||
while (1)
|
||||
{
|
||||
/* Get the interrupt status of the wrist gesture */
|
||||
rslt = bmi2_get_int_status(&int_status, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if ((rslt == BMI2_OK) && (int_status & BMI270_WRIST_GEST_STATUS_MASK))
|
||||
{
|
||||
printf("Wrist gesture detected\n");
|
||||
|
||||
/* Get wrist gesture output */
|
||||
rslt = bmi270_get_sensor_data(&sens_data, 1, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
printf("Wrist gesture = %d\r\n", sens_data.sens_data.wrist_gesture_output);
|
||||
|
||||
printf("Gesture output = %s\n", gesture_output[sens_data.sens_data.wrist_gesture_output]);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bmi2_coines_deinit();
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This internal API sets the sensor configuration
|
||||
*/
|
||||
static int8_t bmi2_set_config(struct bmi2_dev *bmi2_dev)
|
||||
{
|
||||
/* Variable to define result */
|
||||
int8_t rslt;
|
||||
|
||||
/* List the sensors which are required to enable */
|
||||
uint8_t sens_list[2] = { BMI2_ACCEL, BMI2_WRIST_GESTURE };
|
||||
|
||||
/* Structure to define the type of the sensor and its configurations */
|
||||
struct bmi2_sens_config config;
|
||||
|
||||
/* Configure type of feature */
|
||||
config.type = BMI2_WRIST_GESTURE;
|
||||
|
||||
/* Enable the selected sensors */
|
||||
rslt = bmi270_sensor_enable(sens_list, 2, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Get default configurations for the type of feature selected */
|
||||
rslt = bmi270_get_sensor_config(&config, 1, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
config.cfg.wrist_gest.wearable_arm = BMI2_ARM_LEFT;
|
||||
|
||||
/* Set the new configuration along with interrupt mapping */
|
||||
rslt = bmi270_set_sensor_config(&config, 1, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
}
|
||||
}
|
||||
|
||||
return rslt;
|
||||
}
|
|
@ -0,0 +1,18 @@
|
|||
COINES_INSTALL_PATH ?= ../../../..
|
||||
|
||||
EXAMPLE_FILE ?= wrist_wear_wakeup.c
|
||||
|
||||
API_LOCATION ?= ../..
|
||||
|
||||
COMMON_LOCATION ?= ..
|
||||
|
||||
C_SRCS += \
|
||||
$(API_LOCATION)/bmi2.c \
|
||||
$(API_LOCATION)/bmi270.c \
|
||||
$(COMMON_LOCATION)/common/common.c
|
||||
|
||||
INCLUDEPATHS += \
|
||||
$(API_LOCATION) \
|
||||
$(COMMON_LOCATION)/common
|
||||
|
||||
include $(COINES_INSTALL_PATH)/coines.mk
|
|
@ -0,0 +1,131 @@
|
|||
/**\
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
**/
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Header Files */
|
||||
#include <stdio.h>
|
||||
#include "bmi270.h"
|
||||
#include "common.h"
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Static function declaration */
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set the sensor configuration.
|
||||
*
|
||||
* @param[in] bmi2_dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @return Status of execution.
|
||||
*/
|
||||
static int8_t bmi2_set_config(struct bmi2_dev *bmi2_dev);
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Functions */
|
||||
|
||||
/* This function starts the execution of program */
|
||||
int main(void)
|
||||
{
|
||||
/* Variable to define result */
|
||||
int8_t rslt;
|
||||
|
||||
/* Initialize status of wrist wear wakeup interrupt */
|
||||
uint16_t int_status = 0;
|
||||
|
||||
/* Sensor initialization configuration. */
|
||||
struct bmi2_dev bmi2_dev;
|
||||
|
||||
/* Select features and their pins to be mapped to */
|
||||
struct bmi2_sens_int_config sens_int = { .type = BMI2_WRIST_WEAR_WAKE_UP, .hw_int_pin = BMI2_INT1 };
|
||||
|
||||
/* Interface reference is given as a parameter
|
||||
* For I2C : BMI2_I2C_INTF
|
||||
* For SPI : BMI2_SPI_INTF
|
||||
*/
|
||||
rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initialize bmi270. */
|
||||
rslt = bmi270_init(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Set the sensor configuration */
|
||||
rslt = bmi2_set_config(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Map the feature interrupt */
|
||||
rslt = bmi270_map_feat_int(&sens_int, 1, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
printf("Tilt the board in landscape position to trigger wrist wear wakeup\n");
|
||||
|
||||
/* Loop to print the wrist wear wakeup data when interrupt occurs */
|
||||
while (1)
|
||||
{
|
||||
int_status = 0;
|
||||
|
||||
/* To get the interrupt status of the wrist wear wakeup */
|
||||
rslt = bmi2_get_int_status(&int_status, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* To check the interrupt status of the wrist gesture */
|
||||
if ((rslt == BMI2_OK) && (int_status & BMI270_WRIST_WAKE_UP_STATUS_MASK))
|
||||
{
|
||||
printf("Wrist wear wakeup detected\n");
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bmi2_coines_deinit();
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This internal API sets the sensor configuration
|
||||
*/
|
||||
static int8_t bmi2_set_config(struct bmi2_dev *bmi2_dev)
|
||||
{
|
||||
/* Variable to define result */
|
||||
int8_t rslt;
|
||||
|
||||
/* List the sensors which are required to enable */
|
||||
uint8_t sens_list[2] = { BMI2_ACCEL, BMI2_WRIST_WEAR_WAKE_UP };
|
||||
|
||||
/* Structure to define the type of the sensor and its configurations */
|
||||
struct bmi2_sens_config config;
|
||||
|
||||
/* Configure type of feature */
|
||||
config.type = BMI2_WRIST_WEAR_WAKE_UP;
|
||||
|
||||
/* Enable the selected sensors */
|
||||
rslt = bmi270_sensor_enable(sens_list, 2, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Get default configurations for the type of feature selected */
|
||||
rslt = bmi270_get_sensor_config(&config, 1, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Set the new configuration */
|
||||
rslt = bmi270_set_sensor_config(&config, 1, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
}
|
||||
}
|
||||
|
||||
return rslt;
|
||||
}
|
|
@ -0,0 +1,18 @@
|
|||
COINES_INSTALL_PATH ?= ../../../..
|
||||
|
||||
EXAMPLE_FILE ?= accel.c
|
||||
|
||||
API_LOCATION ?= ../..
|
||||
|
||||
COMMON_LOCATION ?= ..
|
||||
|
||||
C_SRCS += \
|
||||
$(API_LOCATION)/bmi2.c \
|
||||
$(API_LOCATION)/bmi270_context.c \
|
||||
$(COMMON_LOCATION)/common/common.c
|
||||
|
||||
INCLUDEPATHS += \
|
||||
$(API_LOCATION) \
|
||||
$(COMMON_LOCATION)/common
|
||||
|
||||
include $(COINES_INSTALL_PATH)/coines.mk
|
|
@ -0,0 +1,200 @@
|
|||
/**\
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
**/
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Header Files */
|
||||
#include <stdio.h>
|
||||
#include "bmi270_context.h"
|
||||
#include "common.h"
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Macro definition */
|
||||
|
||||
/*! Earth's gravity in m/s^2 */
|
||||
#define GRAVITY_EARTH (9.80665f)
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Static Function Declaration */
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for accel.
|
||||
*
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @return Status of execution.
|
||||
*/
|
||||
static int8_t set_accel_config(struct bmi2_dev *bmi2_dev);
|
||||
|
||||
/*!
|
||||
* @brief This function converts lsb to meter per second squared for 16 bit accelerometer at
|
||||
* range 2G, 4G, 8G or 16G.
|
||||
*
|
||||
* @param[in] val : LSB from each axis.
|
||||
* @param[in] g_range : Gravity range.
|
||||
* @param[in] bit_width : Resolution for accel.
|
||||
*
|
||||
* @return Gravity.
|
||||
*/
|
||||
static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width);
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Functions */
|
||||
|
||||
/* This function starts the execution of program. */
|
||||
int main(void)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Variable to define limit to print accel data. */
|
||||
uint8_t limit = 20;
|
||||
|
||||
/* Assign accel sensor to variable. */
|
||||
uint8_t sensor_list = BMI2_ACCEL;
|
||||
|
||||
/* Sensor initialization configuration. */
|
||||
struct bmi2_dev bmi2_dev;
|
||||
|
||||
/* Create an instance of sensor data structure. */
|
||||
struct bmi2_sensor_data sensor_data = { 0 };
|
||||
|
||||
/* Initialize the interrupt status of accel. */
|
||||
uint16_t int_status = 0;
|
||||
|
||||
uint8_t indx = 0;
|
||||
float x = 0, y = 0, z = 0;
|
||||
|
||||
/* Interface reference is given as a parameter
|
||||
* For I2C : BMI2_I2C_INTF
|
||||
* For SPI : BMI2_SPI_INTF
|
||||
*/
|
||||
rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initialize bmi270_context. */
|
||||
rslt = bmi270_context_init(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Enable the accel sensor. */
|
||||
rslt = bmi270_context_sensor_enable(&sensor_list, 1, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Accel configuration settings. */
|
||||
rslt = set_accel_config(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Assign accel sensor. */
|
||||
sensor_data.type = BMI2_ACCEL;
|
||||
printf("Accel and m/s2 data \n");
|
||||
printf("Accel data collected at 2G Range with 16-bit resolution\n");
|
||||
|
||||
/* Loop to print the accel data when interrupt occurs. */
|
||||
while (indx <= limit)
|
||||
{
|
||||
/* To get the status of accel data ready interrupt. */
|
||||
rslt = bmi2_get_int_status(&int_status, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* To check the accel data ready interrupt status and print the status for 10 samples. */
|
||||
if (int_status & BMI2_ACC_DRDY_INT_MASK)
|
||||
{
|
||||
/* Get accelerometer data for x, y and z axis. */
|
||||
rslt = bmi270_context_get_sensor_data(&sensor_data, 1, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
printf("\nAcc_X = %d\t", sensor_data.sens_data.acc.x);
|
||||
printf("Acc_Y = %d\t", sensor_data.sens_data.acc.y);
|
||||
printf("Acc_Z = %d\r\n", sensor_data.sens_data.acc.z);
|
||||
|
||||
/* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */
|
||||
x = lsb_to_mps2(sensor_data.sens_data.acc.x, 2, bmi2_dev.resolution);
|
||||
y = lsb_to_mps2(sensor_data.sens_data.acc.y, 2, bmi2_dev.resolution);
|
||||
z = lsb_to_mps2(sensor_data.sens_data.acc.z, 2, bmi2_dev.resolution);
|
||||
|
||||
/* Print the data in m/s2. */
|
||||
printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z);
|
||||
|
||||
indx++;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bmi2_coines_deinit();
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for accel.
|
||||
*/
|
||||
static int8_t set_accel_config(struct bmi2_dev *bmi2_dev)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Structure to define accelerometer configuration. */
|
||||
struct bmi2_sens_config config;
|
||||
|
||||
/* Configure the type of feature. */
|
||||
config.type = BMI2_ACCEL;
|
||||
|
||||
/* Get default configurations for the type of feature selected. */
|
||||
rslt = bmi270_context_get_sensor_config(&config, 1, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* NOTE: The user can change the following configuration parameters according to their requirement. */
|
||||
/* Set Output Data Rate */
|
||||
config.cfg.acc.odr = BMI2_ACC_ODR_200HZ;
|
||||
|
||||
/* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */
|
||||
config.cfg.acc.range = BMI2_ACC_RANGE_2G;
|
||||
|
||||
/* The bandwidth parameter is used to configure the number of sensor samples that are averaged
|
||||
* if it is set to 2, then 2^(bandwidth parameter) samples
|
||||
* are averaged, resulting in 4 averaged samples.
|
||||
* Note1 : For more information, refer the datasheet.
|
||||
* Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but
|
||||
* this has an adverse effect on the power consumed.
|
||||
*/
|
||||
config.cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4;
|
||||
|
||||
/* Enable the filter performance mode where averaging of samples
|
||||
* will be done based on above set bandwidth and ODR.
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode
|
||||
* 1 -> High performance mode(Default)
|
||||
* For more info refer datasheet.
|
||||
*/
|
||||
config.cfg.acc.filter_perf = BMI2_PERF_OPT_MODE;
|
||||
|
||||
/* Set the accel configurations. */
|
||||
rslt = bmi270_context_set_sensor_config(&config, 1, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Map data ready interrupt to interrupt pin. */
|
||||
rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
}
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This function converts lsb to meter per second squared for 16 bit accelerometer at
|
||||
* range 2G, 4G, 8G or 16G.
|
||||
*/
|
||||
static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width)
|
||||
{
|
||||
float half_scale = ((float)(1 << bit_width) / 2.0f);
|
||||
|
||||
return (GRAVITY_EARTH * val * g_range) / half_scale;
|
||||
}
|
|
@ -0,0 +1,18 @@
|
|||
COINES_INSTALL_PATH ?= ../../../..
|
||||
|
||||
EXAMPLE_FILE ?= accel_gyro.c
|
||||
|
||||
API_LOCATION ?= ../..
|
||||
|
||||
COMMON_LOCATION ?= ..
|
||||
|
||||
C_SRCS += \
|
||||
$(API_LOCATION)/bmi2.c \
|
||||
$(API_LOCATION)/bmi270_context.c \
|
||||
$(COMMON_LOCATION)/common/common.c
|
||||
|
||||
INCLUDEPATHS += \
|
||||
$(API_LOCATION) \
|
||||
$(COMMON_LOCATION)/common
|
||||
|
||||
include $(COINES_INSTALL_PATH)/coines.mk
|
|
@ -0,0 +1,268 @@
|
|||
/**\
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
**/
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Header Files */
|
||||
#include <stdio.h>
|
||||
#include "bmi270_context.h"
|
||||
#include "common.h"
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Macro definition */
|
||||
|
||||
/*! Earth's gravity in m/s^2 */
|
||||
#define GRAVITY_EARTH (9.80665f)
|
||||
|
||||
/*! Macros to select the sensors */
|
||||
#define ACCEL UINT8_C(0x00)
|
||||
#define GYRO UINT8_C(0x01)
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Static Function Declaration */
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for accel.
|
||||
*
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @return Status of execution.
|
||||
*/
|
||||
static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev);
|
||||
|
||||
/*!
|
||||
* @brief This function converts lsb to meter per second squared for 16 bit accelerometer at
|
||||
* range 2G, 4G, 8G or 16G.
|
||||
*
|
||||
* @param[in] val : LSB from each axis.
|
||||
* @param[in] g_range : Gravity range.
|
||||
* @param[in] bit_width : Resolution for accel.
|
||||
*
|
||||
* @return Gravity.
|
||||
*/
|
||||
static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width);
|
||||
|
||||
/*!
|
||||
* @brief This function converts lsb to degree per second for 16 bit gyro at
|
||||
* range 125, 250, 500, 1000 or 2000dps.
|
||||
*
|
||||
* @param[in] val : LSB from each axis.
|
||||
* @param[in] dps : Degree per second.
|
||||
* @param[in] bit_width : Resolution for gyro.
|
||||
*
|
||||
* @return Degree per second.
|
||||
*/
|
||||
static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width);
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Functions */
|
||||
|
||||
/* This function starts the execution of program. */
|
||||
int main(void)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Variable to define limit to print accel data. */
|
||||
uint8_t limit = 10;
|
||||
|
||||
/* Assign accel and gyro sensor to variable. */
|
||||
uint8_t sensor_list[2] = { BMI2_ACCEL, BMI2_GYRO };
|
||||
|
||||
/* Sensor initialization configuration. */
|
||||
struct bmi2_dev bmi2_dev;
|
||||
|
||||
/* Create an instance of sensor data structure. */
|
||||
struct bmi2_sensor_data sensor_data[2] = { { 0 } };
|
||||
|
||||
/* Initialize the interrupt status of accel and gyro. */
|
||||
uint16_t int_status = 0;
|
||||
|
||||
uint8_t indx = 1;
|
||||
|
||||
float x = 0, y = 0, z = 0;
|
||||
|
||||
/* Interface reference is given as a parameter
|
||||
* For I2C : BMI2_I2C_INTF
|
||||
* For SPI : BMI2_SPI_INTF
|
||||
*/
|
||||
rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initialize bmi270_context. */
|
||||
rslt = bmi270_context_init(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Enable the accel and gyro sensor. */
|
||||
rslt = bmi270_context_sensor_enable(sensor_list, 2, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Accel and gyro configuration settings. */
|
||||
rslt = set_accel_gyro_config(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Assign accel and gyro sensor. */
|
||||
sensor_data[ACCEL].type = BMI2_ACCEL;
|
||||
sensor_data[GYRO].type = BMI2_GYRO;
|
||||
|
||||
/* Loop to print accel and gyro data when interrupt occurs. */
|
||||
while (indx <= limit)
|
||||
{
|
||||
/* To get the data ready interrupt status of accel and gyro. */
|
||||
rslt = bmi2_get_int_status(&int_status, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* To check the data ready interrupt status and print the status for 10 samples. */
|
||||
if ((int_status & BMI2_ACC_DRDY_INT_MASK) && (int_status & BMI2_GYR_DRDY_INT_MASK))
|
||||
{
|
||||
/* Get accel and gyro data for x, y and z axis. */
|
||||
rslt = bmi270_context_get_sensor_data(sensor_data, 2, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
printf("\n******* Accel(Raw and m/s2) Gyro(Raw and dps) data : %d *******\n", indx);
|
||||
|
||||
printf("\nAcc_x = %d\t", sensor_data[ACCEL].sens_data.acc.x);
|
||||
printf("Acc_y = %d\t", sensor_data[ACCEL].sens_data.acc.y);
|
||||
printf("Acc_z = %d", sensor_data[ACCEL].sens_data.acc.z);
|
||||
|
||||
/* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */
|
||||
x = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.x, 2, bmi2_dev.resolution);
|
||||
y = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.y, 2, bmi2_dev.resolution);
|
||||
z = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.z, 2, bmi2_dev.resolution);
|
||||
|
||||
/* Print the data in m/s2. */
|
||||
printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z);
|
||||
|
||||
printf("\nGyr_X = %d\t", sensor_data[GYRO].sens_data.gyr.x);
|
||||
printf("Gyr_Y = %d\t", sensor_data[GYRO].sens_data.gyr.y);
|
||||
printf("Gyr_Z = %d\n", sensor_data[GYRO].sens_data.gyr.z);
|
||||
|
||||
/* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */
|
||||
x = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.x, 2000, bmi2_dev.resolution);
|
||||
y = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.y, 2000, bmi2_dev.resolution);
|
||||
z = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.z, 2000, bmi2_dev.resolution);
|
||||
|
||||
/* Print the data in dps. */
|
||||
printf("Gyro_DPS_X = %4.2f, Gyro_DPS_Y = %4.2f, Gyro_DPS_Z = %4.2f\n", x, y, z);
|
||||
|
||||
indx++;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bmi2_coines_deinit();
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for accel and gyro.
|
||||
*/
|
||||
static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Structure to define accelerometer and gyro configuration. */
|
||||
struct bmi2_sens_config config[2];
|
||||
|
||||
/* Configure the type of feature. */
|
||||
config[ACCEL].type = BMI2_ACCEL;
|
||||
config[GYRO].type = BMI2_GYRO;
|
||||
|
||||
/* Get default configurations for the type of feature selected. */
|
||||
rslt = bmi270_context_get_sensor_config(config, 2, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Map data ready interrupt to interrupt pin. */
|
||||
rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* NOTE: The user can change the following configuration parameters according to their requirement. */
|
||||
/* Set Output Data Rate */
|
||||
config[ACCEL].cfg.acc.odr = BMI2_ACC_ODR_200HZ;
|
||||
|
||||
/* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */
|
||||
config[ACCEL].cfg.acc.range = BMI2_ACC_RANGE_2G;
|
||||
|
||||
/* The bandwidth parameter is used to configure the number of sensor samples that are averaged
|
||||
* if it is set to 2, then 2^(bandwidth parameter) samples
|
||||
* are averaged, resulting in 4 averaged samples.
|
||||
* Note1 : For more information, refer the datasheet.
|
||||
* Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but
|
||||
* this has an adverse effect on the power consumed.
|
||||
*/
|
||||
config[ACCEL].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4;
|
||||
|
||||
/* Enable the filter performance mode where averaging of samples
|
||||
* will be done based on above set bandwidth and ODR.
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode
|
||||
* 1 -> High performance mode(Default)
|
||||
* For more info refer datasheet.
|
||||
*/
|
||||
config[ACCEL].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE;
|
||||
|
||||
/* The user can change the following configuration parameters according to their requirement. */
|
||||
/* Set Output Data Rate */
|
||||
config[GYRO].cfg.gyr.odr = BMI2_GYR_ODR_200HZ;
|
||||
|
||||
/* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */
|
||||
config[GYRO].cfg.gyr.range = BMI2_GYR_RANGE_2000;
|
||||
|
||||
/* Gyroscope bandwidth parameters. By default the gyro bandwidth is in normal mode. */
|
||||
config[GYRO].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE;
|
||||
|
||||
/* Enable/Disable the noise performance mode for precision yaw rate sensing
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode(Default)
|
||||
* 1 -> High performance mode
|
||||
*/
|
||||
config[GYRO].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE;
|
||||
|
||||
/* Enable/Disable the filter performance mode where averaging of samples
|
||||
* will be done based on above set bandwidth and ODR.
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode
|
||||
* 1 -> High performance mode(Default)
|
||||
*/
|
||||
config[GYRO].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE;
|
||||
|
||||
/* Set the accel and gyro configurations. */
|
||||
rslt = bmi270_context_set_sensor_config(config, 2, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
}
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This function converts lsb to meter per second squared for 16 bit accelerometer at
|
||||
* range 2G, 4G, 8G or 16G.
|
||||
*/
|
||||
static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width)
|
||||
{
|
||||
float half_scale = ((float)(1 << bit_width) / 2.0f);
|
||||
|
||||
return (GRAVITY_EARTH * val * g_range) / half_scale;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This function converts lsb to degree per second for 16 bit gyro at
|
||||
* range 125, 250, 500, 1000 or 2000dps.
|
||||
*/
|
||||
static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width)
|
||||
{
|
||||
float half_scale = ((float)(1 << bit_width) / 2.0f);
|
||||
|
||||
return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val);
|
||||
}
|
|
@ -0,0 +1,18 @@
|
|||
COINES_INSTALL_PATH ?= ../../../..
|
||||
|
||||
EXAMPLE_FILE ?= activity_recognition.c
|
||||
|
||||
API_LOCATION ?= ../..
|
||||
|
||||
COMMON_LOCATION ?= ..
|
||||
|
||||
C_SRCS += \
|
||||
$(API_LOCATION)/bmi2.c \
|
||||
$(API_LOCATION)/bmi270_context.c \
|
||||
$(COMMON_LOCATION)/common/common.c
|
||||
|
||||
INCLUDEPATHS += \
|
||||
$(API_LOCATION) \
|
||||
$(COMMON_LOCATION)/common
|
||||
|
||||
include $(COINES_INSTALL_PATH)/coines.mk
|
|
@ -0,0 +1,154 @@
|
|||
/**\
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
**/
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Header Files */
|
||||
#include <stdio.h>
|
||||
#include "bmi270_context.h"
|
||||
#include "common.h"
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Static function declaration */
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for step counter.
|
||||
*
|
||||
* @param[in] bmi2_dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @return Status of execution.
|
||||
*/
|
||||
static int8_t set_config(struct bmi2_dev *bmi2_dev);
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Functions */
|
||||
/* This function starts the execution of program. */
|
||||
int main(void)
|
||||
{
|
||||
/* Sensor initialization configuration. */
|
||||
struct bmi2_dev bmi2_dev;
|
||||
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Variable to define index for FIFO data */
|
||||
uint8_t count;
|
||||
|
||||
/* Various Activity recognitions are listed in array. */
|
||||
const char *activity_reg_output[6] = { "OTHERS", "STILL", "WALKING", "RUNNING", "ON_BICYCLE", "IN_VEHICLE" };
|
||||
|
||||
/* Accel sensor and step activity feature are listed in array. */
|
||||
uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_ACTIVITY_RECOGNITION };
|
||||
|
||||
/* Array to define FIFO data */
|
||||
uint8_t fifo_data[516] = { 0 };
|
||||
|
||||
/* Variable to store number of activity frames */
|
||||
uint16_t act_frames;
|
||||
|
||||
/* Structure to define a FIFO */
|
||||
struct bmi2_fifo_frame fifoframe = { 0 };
|
||||
|
||||
/* Structure to define output for activity recognition */
|
||||
struct bmi2_act_recog_output act_recog_data[5] = { { 0 } };
|
||||
|
||||
uint16_t fifo_length;
|
||||
|
||||
/* Interface reference is given as a parameter
|
||||
* For I2C : BMI2_I2C_INTF
|
||||
* For SPI : BMI2_SPI_INTF
|
||||
*/
|
||||
rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initialize bmi270_context. */
|
||||
rslt = bmi270_context_init(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Enable the selected sensor. */
|
||||
rslt = bmi270_context_sensor_enable(sensor_sel, 2, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
rslt = set_config(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Update FIFO structure */
|
||||
fifoframe.data = fifo_data;
|
||||
fifoframe.length = 516;
|
||||
|
||||
rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev);
|
||||
printf("Fifo length = %d \n", fifo_length);
|
||||
|
||||
/* Read FIFO data */
|
||||
rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
while (rslt != BMI2_W_FIFO_EMPTY)
|
||||
{
|
||||
/* Provide the number of frames to be read */
|
||||
act_frames = 5;
|
||||
|
||||
printf("Requested FIFO data frames : %d\n", act_frames);
|
||||
|
||||
/* Get the activity output */
|
||||
rslt = bmi270_context_get_act_recog_output(act_recog_data, &act_frames, &fifoframe, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
printf("Parsed FIFO data frames : %d\r\n", act_frames);
|
||||
|
||||
for (count = 0; count < act_frames; count++)
|
||||
{
|
||||
printf(
|
||||
"Activity Recognition output[%d]:Sensor time: %lu\t Previous activity: %s\t current: %s\n",
|
||||
count,
|
||||
act_recog_data[count].time_stamp,
|
||||
activity_reg_output[act_recog_data[count].prev_act],
|
||||
activity_reg_output[act_recog_data[count].curr_act]);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bmi2_coines_deinit();
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
static int8_t set_config(struct bmi2_dev *bmi2_dev)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
uint8_t temp_data[2];
|
||||
|
||||
/* Disable advanced power save */
|
||||
rslt = bmi2_set_adv_power_save(BMI2_DISABLE, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Reset FIFO */
|
||||
rslt = bmi2_set_command_register(BMI2_FIFO_FLUSH_CMD, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
printf("Move the board for activity recognition for 30 sec :\n");
|
||||
bmi2_dev->delay_us(30000000, bmi2_dev->intf_ptr);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
rslt = bmi2_get_regs(BMI2_FIFO_CONFIG_0_ADDR, temp_data, 2, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
}
|
||||
}
|
||||
|
||||
return rslt;
|
||||
}
|
|
@ -0,0 +1,372 @@
|
|||
/**
|
||||
* Copyright (C) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include "coines.h"
|
||||
#include "bmi2_defs.h"
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Macro definitions */
|
||||
#define BMI2XY_SHUTTLE_ID UINT16_C(0x1B8)
|
||||
|
||||
/*! Macro that defines read write length */
|
||||
#define READ_WRITE_LEN UINT8_C(46)
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Static variable definition */
|
||||
|
||||
/*! Variable that holds the I2C device address or SPI chip selection */
|
||||
static uint8_t dev_addr;
|
||||
|
||||
/******************************************************************************/
|
||||
/*! User interface functions */
|
||||
|
||||
/*!
|
||||
* I2C read function map to COINES platform
|
||||
*/
|
||||
BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr)
|
||||
{
|
||||
uint8_t dev_addr = *(uint8_t*)intf_ptr;
|
||||
|
||||
return coines_read_i2c(dev_addr, reg_addr, reg_data, (uint16_t)len);
|
||||
}
|
||||
|
||||
/*!
|
||||
* I2C write function map to COINES platform
|
||||
*/
|
||||
BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr)
|
||||
{
|
||||
uint8_t dev_addr = *(uint8_t*)intf_ptr;
|
||||
|
||||
return coines_write_i2c(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len);
|
||||
}
|
||||
|
||||
/*!
|
||||
* SPI read function map to COINES platform
|
||||
*/
|
||||
BMI2_INTF_RETURN_TYPE bmi2_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr)
|
||||
{
|
||||
uint8_t dev_addr = *(uint8_t*)intf_ptr;
|
||||
|
||||
return coines_read_spi(dev_addr, reg_addr, reg_data, (uint16_t)len);
|
||||
}
|
||||
|
||||
/*!
|
||||
* SPI write function map to COINES platform
|
||||
*/
|
||||
BMI2_INTF_RETURN_TYPE bmi2_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr)
|
||||
{
|
||||
uint8_t dev_addr = *(uint8_t*)intf_ptr;
|
||||
|
||||
return coines_write_spi(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len);
|
||||
}
|
||||
|
||||
/*!
|
||||
* Delay function map to COINES platform
|
||||
*/
|
||||
void bmi2_delay_us(uint32_t period, void *intf_ptr)
|
||||
{
|
||||
coines_delay_usec(period);
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Function to select the interface between SPI and I2C.
|
||||
* Also to initialize coines platform
|
||||
*/
|
||||
int8_t bmi2_interface_init(struct bmi2_dev *bmi, uint8_t intf)
|
||||
{
|
||||
int8_t rslt = BMI2_OK;
|
||||
struct coines_board_info board_info;
|
||||
|
||||
if (bmi != NULL)
|
||||
{
|
||||
int16_t result = coines_open_comm_intf(COINES_COMM_INTF_USB);
|
||||
if (result < COINES_SUCCESS)
|
||||
{
|
||||
printf(
|
||||
"\n Unable to connect with Application Board ! \n" " 1. Check if the board is connected and powered on. \n" " 2. Check if Application Board USB driver is installed. \n"
|
||||
" 3. Check if board is in use by another application. (Insufficient permissions to access USB) \n");
|
||||
exit(result);
|
||||
}
|
||||
|
||||
result = coines_get_board_info(&board_info);
|
||||
|
||||
#if defined(PC)
|
||||
setbuf(stdout, NULL);
|
||||
#endif
|
||||
|
||||
if (result == COINES_SUCCESS)
|
||||
{
|
||||
if ((board_info.shuttle_id != BMI2XY_SHUTTLE_ID))
|
||||
{
|
||||
printf("! Warning invalid sensor shuttle \n ," "This application will not support this sensor \n");
|
||||
exit(COINES_E_FAILURE);
|
||||
}
|
||||
}
|
||||
|
||||
coines_set_shuttleboard_vdd_vddio_config(0, 0);
|
||||
coines_delay_msec(100);
|
||||
|
||||
/* Bus configuration : I2C */
|
||||
if (intf == BMI2_I2C_INTF)
|
||||
{
|
||||
printf("I2C Interface \n");
|
||||
|
||||
/* To initialize the user I2C function */
|
||||
dev_addr = BMI2_I2C_PRIM_ADDR;
|
||||
bmi->intf = BMI2_I2C_INTF;
|
||||
bmi->read = bmi2_i2c_read;
|
||||
bmi->write = bmi2_i2c_write;
|
||||
coines_config_i2c_bus(COINES_I2C_BUS_0, COINES_I2C_FAST_MODE);
|
||||
}
|
||||
/* Bus configuration : SPI */
|
||||
else if (intf == BMI2_SPI_INTF)
|
||||
{
|
||||
printf("SPI Interface \n");
|
||||
|
||||
/* To initialize the user SPI function */
|
||||
dev_addr = COINES_SHUTTLE_PIN_7;
|
||||
bmi->intf = BMI2_SPI_INTF;
|
||||
bmi->read = bmi2_spi_read;
|
||||
bmi->write = bmi2_spi_write;
|
||||
coines_config_spi_bus(COINES_SPI_BUS_0, COINES_SPI_SPEED_5_MHZ, COINES_SPI_MODE3);
|
||||
coines_set_pin_config(COINES_SHUTTLE_PIN_7, COINES_PIN_DIRECTION_OUT, COINES_PIN_VALUE_HIGH);
|
||||
}
|
||||
|
||||
/* Assign device address to interface pointer */
|
||||
bmi->intf_ptr = &dev_addr;
|
||||
|
||||
/* Configure delay in microseconds */
|
||||
bmi->delay_us = bmi2_delay_us;
|
||||
|
||||
/* Configure max read/write length (in bytes) ( Supported length depends on target machine) */
|
||||
bmi->read_write_len = READ_WRITE_LEN;
|
||||
|
||||
/* Assign to NULL to load the default config file. */
|
||||
bmi->config_file_ptr = NULL;
|
||||
|
||||
coines_delay_usec(10000);
|
||||
|
||||
coines_set_shuttleboard_vdd_vddio_config(3300, 3300);
|
||||
|
||||
coines_delay_usec(10000);
|
||||
}
|
||||
else
|
||||
{
|
||||
rslt = BMI2_E_NULL_PTR;
|
||||
}
|
||||
|
||||
return rslt;
|
||||
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Prints the execution status of the APIs.
|
||||
*/
|
||||
void bmi2_error_codes_print_result(int8_t rslt)
|
||||
{
|
||||
switch (rslt)
|
||||
{
|
||||
case BMI2_OK:
|
||||
|
||||
/* Do nothing */
|
||||
break;
|
||||
|
||||
case BMI2_W_FIFO_EMPTY:
|
||||
printf("Warning [%d] : FIFO empty\r\n", rslt);
|
||||
break;
|
||||
case BMI2_W_PARTIAL_READ:
|
||||
printf("Warning [%d] : FIFO partial read\r\n", rslt);
|
||||
break;
|
||||
case BMI2_E_NULL_PTR:
|
||||
printf(
|
||||
"Error [%d] : Null pointer error. It occurs when the user tries to assign value (not address) to a pointer," " which has been initialized to NULL.\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_COM_FAIL:
|
||||
printf(
|
||||
"Error [%d] : Communication failure error. It occurs due to read/write operation failure and also due " "to power failure during communication\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_DEV_NOT_FOUND:
|
||||
printf("Error [%d] : Device not found error. It occurs when the device chip id is incorrectly read\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_INVALID_SENSOR:
|
||||
printf(
|
||||
"Error [%d] : Invalid sensor error. It occurs when there is a mismatch in the requested feature with the " "available one\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_SELF_TEST_FAIL:
|
||||
printf(
|
||||
"Error [%d] : Self-test failed error. It occurs when the validation of accel self-test data is " "not satisfied\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_INVALID_INT_PIN:
|
||||
printf(
|
||||
"Error [%d] : Invalid interrupt pin error. It occurs when the user tries to configure interrupt pins " "apart from INT1 and INT2\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_OUT_OF_RANGE:
|
||||
printf(
|
||||
"Error [%d] : Out of range error. It occurs when the data exceeds from filtered or unfiltered data from " "fifo and also when the range exceeds the maximum range for accel and gyro while performing FOC\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_ACC_INVALID_CFG:
|
||||
printf(
|
||||
"Error [%d] : Invalid Accel configuration error. It occurs when there is an error in accel configuration" " register which could be one among range, BW or filter performance in reg address 0x40\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_GYRO_INVALID_CFG:
|
||||
printf(
|
||||
"Error [%d] : Invalid Gyro configuration error. It occurs when there is a error in gyro configuration" "register which could be one among range, BW or filter performance in reg address 0x42\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_ACC_GYR_INVALID_CFG:
|
||||
printf(
|
||||
"Error [%d] : Invalid Accel-Gyro configuration error. It occurs when there is a error in accel and gyro" " configuration registers which could be one among range, BW or filter performance in reg address 0x40 " "and 0x42\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_CONFIG_LOAD:
|
||||
printf(
|
||||
"Error [%d] : Configuration load error. It occurs when failure observed while loading the configuration " "into the sensor\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_INVALID_PAGE:
|
||||
printf(
|
||||
"Error [%d] : Invalid page error. It occurs due to failure in writing the correct feature configuration " "from selected page\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_SET_APS_FAIL:
|
||||
printf(
|
||||
"Error [%d] : APS failure error. It occurs due to failure in write of advance power mode configuration " "register\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_AUX_INVALID_CFG:
|
||||
printf(
|
||||
"Error [%d] : Invalid AUX configuration error. It occurs when the auxiliary interface settings are not " "enabled properly\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_AUX_BUSY:
|
||||
printf(
|
||||
"Error [%d] : AUX busy error. It occurs when the auxiliary interface buses are engaged while configuring" " the AUX\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_REMAP_ERROR:
|
||||
printf(
|
||||
"Error [%d] : Remap error. It occurs due to failure in assigning the remap axes data for all the axes " "after change in axis position\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_GYR_USER_GAIN_UPD_FAIL:
|
||||
printf(
|
||||
"Error [%d] : Gyro user gain update fail error. It occurs when the reading of user gain update status " "fails\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_SELF_TEST_NOT_DONE:
|
||||
printf(
|
||||
"Error [%d] : Self-test not done error. It occurs when the self-test process is ongoing or not " "completed\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_INVALID_INPUT:
|
||||
printf("Error [%d] : Invalid input error. It occurs when the sensor input validity fails\r\n", rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_INVALID_STATUS:
|
||||
printf("Error [%d] : Invalid status error. It occurs when the feature/sensor validity fails\r\n", rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_CRT_ERROR:
|
||||
printf("Error [%d] : CRT error. It occurs when the CRT test has failed\r\n", rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_ST_ALREADY_RUNNING:
|
||||
printf(
|
||||
"Error [%d] : Self-test already running error. It occurs when the self-test is already running and " "another has been initiated\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_CRT_READY_FOR_DL_FAIL_ABORT:
|
||||
printf(
|
||||
"Error [%d] : CRT ready for download fail abort error. It occurs when download in CRT fails due to wrong " "address location\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_DL_ERROR:
|
||||
printf(
|
||||
"Error [%d] : Download error. It occurs when write length exceeds that of the maximum burst length\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_PRECON_ERROR:
|
||||
printf(
|
||||
"Error [%d] : Pre-conditional error. It occurs when precondition to start the feature was not " "completed\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_ABORT_ERROR:
|
||||
printf("Error [%d] : Abort error. It occurs when the device was shaken during CRT test\r\n", rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_WRITE_CYCLE_ONGOING:
|
||||
printf(
|
||||
"Error [%d] : Write cycle ongoing error. It occurs when the write cycle is already running and another " "has been initiated\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_ST_NOT_RUNING:
|
||||
printf(
|
||||
"Error [%d] : Self-test is not running error. It occurs when self-test running is disabled while it's " "running\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_DATA_RDY_INT_FAILED:
|
||||
printf(
|
||||
"Error [%d] : Data ready interrupt error. It occurs when the sample count exceeds the FOC sample limit " "and data ready status is not updated\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_INVALID_FOC_POSITION:
|
||||
printf(
|
||||
"Error [%d] : Invalid FOC position error. It occurs when average FOC data is obtained for the wrong" " axes\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
default:
|
||||
printf("Error [%d] : Unknown error code\r\n", rslt);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Deinitializes coines platform
|
||||
*
|
||||
* @return void.
|
||||
*/
|
||||
void bmi2_coines_deinit(void)
|
||||
{
|
||||
coines_close_comm_intf(COINES_COMM_INTF_USB);
|
||||
}
|
|
@ -0,0 +1,122 @@
|
|||
/**\
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
**/
|
||||
|
||||
#ifndef _COMMON_H
|
||||
#define _COMMON_H
|
||||
|
||||
/*! CPP guard */
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include <stdio.h>
|
||||
#include "bmi2.h"
|
||||
|
||||
/*!
|
||||
* @brief Function for reading the sensor's registers through I2C bus.
|
||||
*
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[out] reg_data : Pointer to the data buffer to store the read data.
|
||||
* @param[in] length : No of bytes to read.
|
||||
* @param[in] intf_ptr : Interface pointer
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval = BMI2_INTF_RET_SUCCESS -> Success
|
||||
* @retval != BMI2_INTF_RET_SUCCESS -> Failure Info
|
||||
*
|
||||
*/
|
||||
BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr);
|
||||
|
||||
/*!
|
||||
* @brief Function for writing the sensor's registers through I2C bus.
|
||||
*
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[in] reg_data : Pointer to the data buffer whose value is to be written.
|
||||
* @param[in] length : No of bytes to write.
|
||||
* @param[in] intf_ptr : Interface pointer
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval = BMI2_INTF_RET_SUCCESS -> Success
|
||||
* @retval != BMI2_INTF_RET_SUCCESS -> Failure Info
|
||||
*
|
||||
*/
|
||||
BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr);
|
||||
|
||||
/*!
|
||||
* @brief Function for reading the sensor's registers through SPI bus.
|
||||
*
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[out] reg_data : Pointer to the data buffer to store the read data.
|
||||
* @param[in] length : No of bytes to read.
|
||||
* @param[in] intf_ptr : Interface pointer
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval = BMI2_INTF_RET_SUCCESS -> Success
|
||||
* @retval != BMI2_INTF_RET_SUCCESS -> Failure Info
|
||||
*
|
||||
*/
|
||||
BMI2_INTF_RETURN_TYPE bmi2_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr);
|
||||
|
||||
/*!
|
||||
* @brief Function for writing the sensor's registers through SPI bus.
|
||||
*
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[in] reg_data : Pointer to the data buffer whose data has to be written.
|
||||
* @param[in] length : No of bytes to write.
|
||||
* @param[in] intf_ptr : Interface pointer
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval = BMI2_INTF_RET_SUCCESS -> Success
|
||||
* @retval != BMI2_INTF_RET_SUCCESS -> Failure Info
|
||||
*
|
||||
*/
|
||||
BMI2_INTF_RETURN_TYPE bmi2_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr);
|
||||
|
||||
/*!
|
||||
* @brief This function provides the delay for required time (Microsecond) as per the input provided in some of the
|
||||
* APIs.
|
||||
*
|
||||
* @param[in] period_us : The required wait time in microsecond.
|
||||
* @param[in] intf_ptr : Interface pointer
|
||||
*
|
||||
* @return void.
|
||||
*
|
||||
*/
|
||||
void bmi2_delay_us(uint32_t period, void *intf_ptr);
|
||||
|
||||
/*!
|
||||
* @brief Function to select the interface between SPI and I2C.
|
||||
*
|
||||
* @param[in] bma : Structure instance of bmi2_dev
|
||||
* @param[in] intf : Interface selection parameter
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Failure Info
|
||||
*/
|
||||
int8_t bmi2_interface_init(struct bmi2_dev *bma, uint8_t intf);
|
||||
|
||||
/*!
|
||||
* @brief Prints the execution status of the APIs.
|
||||
*
|
||||
* @param[in] rslt : Error code returned by the API whose execution status has to be printed.
|
||||
*
|
||||
* @return void.
|
||||
*/
|
||||
void bmi2_error_codes_print_result(int8_t rslt);
|
||||
|
||||
/*!
|
||||
* @brief Deinitializes coines platform
|
||||
*
|
||||
* @return void.
|
||||
*/
|
||||
void bmi2_coines_deinit(void);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif /* End of CPP guard */
|
||||
|
||||
#endif /* _COMMON_H */
|
|
@ -0,0 +1,18 @@
|
|||
COINES_INSTALL_PATH ?= ../../../..
|
||||
|
||||
EXAMPLE_FILE ?= component_retrim.c
|
||||
|
||||
API_LOCATION ?= ../..
|
||||
|
||||
COMMON_LOCATION ?= ..
|
||||
|
||||
C_SRCS += \
|
||||
$(API_LOCATION)/bmi2.c \
|
||||
$(API_LOCATION)/bmi270_context.c \
|
||||
$(COMMON_LOCATION)/common/common.c
|
||||
|
||||
INCLUDEPATHS += \
|
||||
$(API_LOCATION) \
|
||||
$(COMMON_LOCATION)/common
|
||||
|
||||
include $(COINES_INSTALL_PATH)/coines.mk
|
|
@ -0,0 +1,52 @@
|
|||
/**\
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
**/
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Header Files */
|
||||
#include <stdio.h>
|
||||
#include "bmi270_context.h"
|
||||
#include "common.h"
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Functions */
|
||||
|
||||
/* This function starts the execution of program. */
|
||||
int main(void)
|
||||
{
|
||||
/* Sensor initialization configuration. */
|
||||
struct bmi2_dev bmi2_dev;
|
||||
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Interface reference is given as a parameter
|
||||
* For I2C : BMI2_I2C_INTF
|
||||
* For SPI : BMI2_SPI_INTF
|
||||
*/
|
||||
rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initialize bmi270_context. */
|
||||
rslt = bmi270_context_init(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* This API runs the CRT process. */
|
||||
rslt = bmi2_do_crt(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Do not move the board while doing CRT. If so, it will throw an abort error. */
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
printf("CRT successfully completed.");
|
||||
}
|
||||
}
|
||||
|
||||
bmi2_coines_deinit();
|
||||
|
||||
return rslt;
|
||||
}
|
|
@ -0,0 +1,18 @@
|
|||
COINES_INSTALL_PATH ?= ../../../..
|
||||
|
||||
EXAMPLE_FILE ?= fifo_full_header_mode.c
|
||||
|
||||
API_LOCATION ?= ../..
|
||||
|
||||
COMMON_LOCATION ?= ..
|
||||
|
||||
C_SRCS += \
|
||||
$(API_LOCATION)/bmi2.c \
|
||||
$(API_LOCATION)/bmi270_context.c \
|
||||
$(COMMON_LOCATION)/common/common.c
|
||||
|
||||
INCLUDEPATHS += \
|
||||
$(API_LOCATION) \
|
||||
$(COMMON_LOCATION)/common
|
||||
|
||||
include $(COINES_INSTALL_PATH)/coines.mk
|
|
@ -0,0 +1,315 @@
|
|||
/**\
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
**/
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Header Files */
|
||||
#include <stdio.h>
|
||||
#include "bmi270_context.h"
|
||||
#include "common.h"
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Macros */
|
||||
|
||||
/*! Buffer size allocated to store raw FIFO data. */
|
||||
#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048)
|
||||
|
||||
/*! Length of data to be read from FIFO. */
|
||||
#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048)
|
||||
|
||||
/*! Number of accel frames to be extracted from FIFO. */
|
||||
|
||||
/*! Calculation for frame count: Total frame count = Fifo buffer size(2048)/ Total frames(6 Accel, 6 Gyro and 1 header,
|
||||
* totaling to 13) which equals to 157.
|
||||
*/
|
||||
#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(157)
|
||||
|
||||
/*! Number of gyro frames to be extracted from FIFO. */
|
||||
#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(157)
|
||||
|
||||
/*! Macro to read sensortime byte in FIFO. */
|
||||
#define SENSORTIME_OVERHEAD_BYTE UINT8_C(3)
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Static Function Declaration */
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for accel and gyro.
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
* @return Status of execution.
|
||||
*/
|
||||
static int8_t set_accel_gyro_config(struct bmi2_dev *dev);
|
||||
|
||||
/******************************************************************************/
|
||||
|
||||
/* This function starts the execution of program. */
|
||||
int main(void)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
uint16_t index = 0;
|
||||
uint16_t fifo_length = 0;
|
||||
uint16_t config = 0;
|
||||
|
||||
/* To read sensortime, extra 3 bytes are added to fifo buffer. */
|
||||
uint16_t fifo_buffer_size = BMI2_FIFO_RAW_DATA_BUFFER_SIZE + SENSORTIME_OVERHEAD_BYTE;
|
||||
|
||||
/* Variable to get fifo full interrupt status. */
|
||||
uint16_t int_status = 0;
|
||||
|
||||
uint16_t accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT;
|
||||
|
||||
uint16_t gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT;
|
||||
|
||||
/* Variable to store the available frame length count in FIFO */
|
||||
uint8_t frame_count;
|
||||
|
||||
int8_t try = 1;
|
||||
|
||||
/* Number of bytes of FIFO data. */
|
||||
uint8_t fifo_data[fifo_buffer_size];
|
||||
|
||||
/* Sensor initialization configuration. */
|
||||
struct bmi2_dev bmi2_dev;
|
||||
|
||||
/* Array of accelerometer frames -> Total bytes =
|
||||
* 157 * (6 axes + 1 header bytes) = 1099 bytes */
|
||||
struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } };
|
||||
|
||||
/* Array of gyro frames -> Total bytes =
|
||||
* 157 * (6 axes + 1 header bytes) = 1099 bytes */
|
||||
struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } };
|
||||
|
||||
/* Initialize FIFO frame structure. */
|
||||
struct bmi2_fifo_frame fifoframe = { 0 };
|
||||
|
||||
/* Accel and gyro sensor are listed in array. */
|
||||
uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO };
|
||||
|
||||
/* Interface reference is given as a parameter
|
||||
* For I2C : BMI2_I2C_INTF
|
||||
* For SPI : BMI2_SPI_INTF
|
||||
*/
|
||||
rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initialize bmi270_context. */
|
||||
rslt = bmi270_context_init(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Enable accel and gyro sensor. */
|
||||
rslt = bmi270_context_sensor_enable(sensor_sel, 2, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Configuration settings for accel and gyro. */
|
||||
rslt = set_accel_gyro_config(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Before setting FIFO, disable the advance power save mode. */
|
||||
rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initially disable all configurations in fifo. */
|
||||
rslt = bmi2_get_fifo_config(&config, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initially disable all configurations in fifo. */
|
||||
rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Update FIFO structure. */
|
||||
/* Mapping the buffer to store the fifo data. */
|
||||
fifoframe.data = fifo_data;
|
||||
|
||||
/* Length of FIFO frame. */
|
||||
/* To read sensortime, extra 3 bytes are added to fifo user length. */
|
||||
fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH + SENSORTIME_OVERHEAD_BYTE;
|
||||
|
||||
/* Set FIFO configuration by enabling accel, gyro and timestamp.
|
||||
* NOTE 1: The header mode is enabled by default.
|
||||
* NOTE 2: By default the FIFO operating mode is in FIFO mode.
|
||||
* NOTE 3: Sensortime is enabled by default */
|
||||
printf("FIFO is configured in header mode\n");
|
||||
rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Map FIFO full interrupt. */
|
||||
fifoframe.data_int_map = BMI2_FFULL_INT;
|
||||
rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
while (try <= 10)
|
||||
{
|
||||
/* Read FIFO data on interrupt. */
|
||||
rslt = bmi2_get_int_status(&int_status, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if ((rslt == BMI2_OK) && (int_status & BMI2_FFULL_INT_STATUS_MASK))
|
||||
{
|
||||
printf("\nIteration : %d\n", try);
|
||||
|
||||
accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT;
|
||||
|
||||
gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT;
|
||||
|
||||
rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
printf("\nFIFO data bytes available : %d \n", fifo_length);
|
||||
printf("\nFIFO data bytes requested : %d \n", fifoframe.length);
|
||||
|
||||
/* Read FIFO data. */
|
||||
rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Read FIFO data on interrupt. */
|
||||
rslt = bmi2_get_int_status(&int_status, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
printf("\nFIFO accel frames requested : %d \n", accel_frame_length);
|
||||
|
||||
/* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */
|
||||
rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev);
|
||||
printf("\nFIFO accel frames extracted : %d \n", accel_frame_length);
|
||||
|
||||
printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length);
|
||||
|
||||
/* Parse the FIFO data to extract gyro data from the FIFO buffer. */
|
||||
rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev);
|
||||
printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length);
|
||||
|
||||
/* Calculating the frame count from the available bytes in FIFO
|
||||
* frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len + header_byte)) */
|
||||
frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH + 1));
|
||||
printf("\nAvailable frame count from available bytes: %d\n", frame_count);
|
||||
|
||||
printf("\nExtracted accel frames\n");
|
||||
|
||||
if (accel_frame_length > frame_count)
|
||||
{
|
||||
accel_frame_length = frame_count;
|
||||
}
|
||||
|
||||
/* Print the parsed accelerometer data from the FIFO buffer. */
|
||||
for (index = 0; index < accel_frame_length; index++)
|
||||
{
|
||||
printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x,
|
||||
fifo_accel_data[index].y, fifo_accel_data[index].z);
|
||||
}
|
||||
|
||||
printf("\nExtracted gyro frames\n");
|
||||
|
||||
if (gyro_frame_length > frame_count)
|
||||
{
|
||||
gyro_frame_length = frame_count;
|
||||
}
|
||||
|
||||
/* Print the parsed gyro data from the FIFO buffer. */
|
||||
for (index = 0; index < gyro_frame_length; index++)
|
||||
{
|
||||
printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n",
|
||||
index,
|
||||
fifo_gyro_data[index].x,
|
||||
fifo_gyro_data[index].y,
|
||||
fifo_gyro_data[index].z);
|
||||
}
|
||||
|
||||
/* Print control frames like sensor time and skipped frame count. */
|
||||
printf("\nSkipped frame count = %d\n", fifoframe.skipped_frame_count);
|
||||
|
||||
printf("Sensor time = %lu\r\n", fifoframe.sensor_time);
|
||||
|
||||
try++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bmi2_coines_deinit();
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for accel and gyro.
|
||||
*/
|
||||
static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Structure to define accel and gyro configurations. */
|
||||
struct bmi2_sens_config config[2];
|
||||
|
||||
/* Configure the type of feature. */
|
||||
config[0].type = BMI2_ACCEL;
|
||||
config[1].type = BMI2_GYRO;
|
||||
|
||||
/* Get default configurations for the type of feature selected. */
|
||||
rslt = bmi270_context_get_sensor_config(config, 2, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* NOTE: The user can change the following configuration parameters according to their requirement. */
|
||||
/* Accel configuration settings. */
|
||||
/* Set Output Data Rate */
|
||||
config[0].cfg.acc.odr = BMI2_ACC_ODR_25HZ;
|
||||
|
||||
/* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */
|
||||
config[0].cfg.acc.range = BMI2_ACC_RANGE_2G;
|
||||
|
||||
/* The bandwidth parameter is used to configure the number of sensor samples that are averaged
|
||||
* if it is set to 2, then 2^(bandwidth parameter) samples
|
||||
* are averaged, resulting in 4 averaged samples
|
||||
* Note1 : For more information, refer the datasheet.
|
||||
* Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but
|
||||
* this has an adverse effect on the power consumed.
|
||||
*/
|
||||
config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4;
|
||||
|
||||
/* Enable the filter performance mode where averaging of samples
|
||||
* will be done based on above set bandwidth and ODR.
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode
|
||||
* 1 -> High performance mode(Default)
|
||||
* For more info refer datasheet.
|
||||
*/
|
||||
config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE;
|
||||
|
||||
/* Gyro configuration settings. */
|
||||
/* Set Output Data Rate */
|
||||
config[1].cfg.gyr.odr = BMI2_GYR_ODR_50HZ;
|
||||
|
||||
/* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */
|
||||
config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000;
|
||||
|
||||
/* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */
|
||||
config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE;
|
||||
|
||||
/* Enable/Disable the noise performance mode for precision yaw rate sensing
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode(Default)
|
||||
* 1 -> High performance mode
|
||||
*/
|
||||
config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE;
|
||||
|
||||
/* Enable/Disable the filter performance mode where averaging of samples
|
||||
* will be done based on above set bandwidth and ODR.
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode
|
||||
* 1 -> High performance mode(Default)
|
||||
*/
|
||||
config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE;
|
||||
|
||||
/* Set new configurations. */
|
||||
rslt = bmi270_context_set_sensor_config(config, 2, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
}
|
||||
|
||||
return rslt;
|
||||
}
|
|
@ -0,0 +1,18 @@
|
|||
COINES_INSTALL_PATH ?= ../../../..
|
||||
|
||||
EXAMPLE_FILE ?= fifo_full_headerless_mode.c
|
||||
|
||||
API_LOCATION ?= ../..
|
||||
|
||||
COMMON_LOCATION ?= ..
|
||||
|
||||
C_SRCS += \
|
||||
$(API_LOCATION)/bmi2.c \
|
||||
$(API_LOCATION)/bmi270_context.c \
|
||||
$(COMMON_LOCATION)/common/common.c
|
||||
|
||||
INCLUDEPATHS += \
|
||||
$(API_LOCATION) \
|
||||
$(COMMON_LOCATION)/common
|
||||
|
||||
include $(COINES_INSTALL_PATH)/coines.mk
|
|
@ -0,0 +1,304 @@
|
|||
/**\
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
**/
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Header Files */
|
||||
#include <stdio.h>
|
||||
#include "bmi270_context.h"
|
||||
#include "common.h"
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Macros */
|
||||
|
||||
/*! Buffer size allocated to store raw FIFO data */
|
||||
#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048)
|
||||
|
||||
/*! Length of data to be read from FIFO */
|
||||
#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048)
|
||||
|
||||
/*! Number of accel frames to be extracted from FIFO */
|
||||
|
||||
/*! Calculation for frame count: Total frame count = Fifo buffer size(2048)/ Total frames(6 Accel, 6 Gyro totaling to
|
||||
* 12) which equals to 170.
|
||||
*/
|
||||
#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(169)
|
||||
|
||||
/*! Number of gyro frames to be extracted from FIFO */
|
||||
#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(169)
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Static Function Declaration */
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for accel and gyro.
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
* @return Status of execution.
|
||||
*/
|
||||
static int8_t set_accel_gyro_config(struct bmi2_dev *dev);
|
||||
|
||||
/******************************************************************************/
|
||||
|
||||
/* This function starts the execution of program. */
|
||||
int main(void)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
uint16_t index = 0;
|
||||
|
||||
uint16_t fifo_length = 0;
|
||||
|
||||
uint8_t try = 1;
|
||||
|
||||
/* Variable to get fifo full interrupt status. */
|
||||
uint16_t int_status = 0;
|
||||
|
||||
uint16_t accel_frame_length;
|
||||
|
||||
uint16_t gyro_frame_length;
|
||||
|
||||
/* Variable to store the available frame length count in FIFO */
|
||||
uint8_t frame_count;
|
||||
|
||||
/* Number of bytes of FIFO data. */
|
||||
uint8_t fifo_data[BMI2_FIFO_RAW_DATA_BUFFER_SIZE] = { 0 };
|
||||
|
||||
/* Sensor initialization configuration. */
|
||||
struct bmi2_dev bmi2_dev;
|
||||
|
||||
/* Array of accelerometer frames -> Total bytes =
|
||||
* 170 * (6 axes bytes) = 1020 bytes */
|
||||
struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } };
|
||||
|
||||
/* Array of gyro frames -> Total bytes =
|
||||
* 170 * (6 axes bytes) = 1020 bytes */
|
||||
struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } };
|
||||
|
||||
/* Initialize FIFO frame structure. */
|
||||
struct bmi2_fifo_frame fifoframe = { 0 };
|
||||
|
||||
/* Accel and gyro sensor are listed in array. */
|
||||
uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO };
|
||||
|
||||
/* Interface reference is given as a parameter
|
||||
* For I2C : BMI2_I2C_INTF
|
||||
* For SPI : BMI2_SPI_INTF
|
||||
*/
|
||||
rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initialize bmi270_context. */
|
||||
rslt = bmi270_context_init(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Enable accel and gyro sensor. */
|
||||
rslt = bmi270_context_sensor_enable(sensor_sel, 2, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Configuration settings for accel and gyro. */
|
||||
rslt = set_accel_gyro_config(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Before setting FIFO, disable the advance power save mode. */
|
||||
rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initially disable all configurations in fifo. */
|
||||
rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Update FIFO structure. */
|
||||
/* Mapping the buffer to store the fifo data. */
|
||||
fifoframe.data = fifo_data;
|
||||
|
||||
/* Length of FIFO frame. */
|
||||
fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH;
|
||||
|
||||
/* Set FIFO configuration by enabling accel, gyro.
|
||||
* NOTE 1: The header mode is enabled by default.
|
||||
* NOTE 2: By default the FIFO operating mode is FIFO mode. */
|
||||
rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
printf("FIFO is configured in headerless mode\n");
|
||||
|
||||
/* To enable headerless mode, disable the header. */
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
rslt = bmi2_set_fifo_config(BMI2_FIFO_HEADER_EN, BMI2_DISABLE, &bmi2_dev);
|
||||
}
|
||||
|
||||
/* Map FIFO full interrupt. */
|
||||
fifoframe.data_int_map = BMI2_FFULL_INT;
|
||||
rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
while (try <= 10)
|
||||
{
|
||||
/* Read FIFO data on interrupt. */
|
||||
rslt = bmi2_get_int_status(&int_status, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if ((rslt == BMI2_OK) && (int_status & BMI2_FFULL_INT_STATUS_MASK))
|
||||
{
|
||||
printf("\nIteration : %d\n", try);
|
||||
|
||||
rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT;
|
||||
gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT;
|
||||
|
||||
printf("\nFIFO data bytes available : %d \n", fifo_length);
|
||||
printf("\nFIFO data bytes requested : %d \n", fifoframe.length);
|
||||
|
||||
/* Read FIFO data. */
|
||||
rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Read FIFO data on interrupt. */
|
||||
rslt = bmi2_get_int_status(&int_status, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
printf("\nFIFO accel frames requested : %d \n", accel_frame_length);
|
||||
|
||||
/* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */
|
||||
rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev);
|
||||
printf("\nFIFO accel frames extracted : %d \n", accel_frame_length);
|
||||
|
||||
printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length);
|
||||
|
||||
/* Parse the FIFO data to extract gyro data from the FIFO buffer. */
|
||||
rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev);
|
||||
printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length);
|
||||
|
||||
/* Calculating the frame count from the available bytes in FIFO
|
||||
* frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len)) */
|
||||
frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH));
|
||||
printf("\nAvailable frame count from available bytes: %d\n", frame_count);
|
||||
|
||||
printf("\nExtracted accel frames\n");
|
||||
|
||||
if (accel_frame_length > frame_count)
|
||||
{
|
||||
accel_frame_length = frame_count;
|
||||
}
|
||||
|
||||
/* Print the parsed accelerometer data from the FIFO buffer. */
|
||||
for (index = 0; index < accel_frame_length; index++)
|
||||
{
|
||||
printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x,
|
||||
fifo_accel_data[index].y, fifo_accel_data[index].z);
|
||||
}
|
||||
|
||||
printf("\nExtracted gyro frames\n");
|
||||
|
||||
if (gyro_frame_length > frame_count)
|
||||
{
|
||||
gyro_frame_length = frame_count;
|
||||
}
|
||||
|
||||
/* Print the parsed gyro data from the FIFO buffer. */
|
||||
for (index = 0; index < gyro_frame_length; index++)
|
||||
{
|
||||
printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n",
|
||||
index,
|
||||
fifo_gyro_data[index].x,
|
||||
fifo_gyro_data[index].y,
|
||||
fifo_gyro_data[index].z);
|
||||
}
|
||||
}
|
||||
|
||||
try++;
|
||||
}
|
||||
}
|
||||
|
||||
bmi2_coines_deinit();
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for accel and gyro.
|
||||
*/
|
||||
static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Structure to define accel and gyro configurations. */
|
||||
struct bmi2_sens_config config[2];
|
||||
|
||||
/* Configure the type of feature. */
|
||||
config[0].type = BMI2_ACCEL;
|
||||
config[1].type = BMI2_GYRO;
|
||||
|
||||
/* Get default configurations for the type of feature selected. */
|
||||
rslt = bmi270_context_get_sensor_config(config, 2, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* NOTE: The user can change the following configuration parameters according to their requirement. */
|
||||
/* Accel configuration settings. */
|
||||
/* Set Output Data Rate */
|
||||
config[0].cfg.acc.odr = BMI2_ACC_ODR_100HZ;
|
||||
|
||||
/* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */
|
||||
config[0].cfg.acc.range = BMI2_ACC_RANGE_2G;
|
||||
|
||||
/* The bandwidth parameter is used to configure the number of sensor samples that are averaged
|
||||
* if it is set to 2, then 2^(bandwidth parameter) samples
|
||||
* are averaged, resulting in 4 averaged samples
|
||||
* Note1 : For more information, refer the datasheet.
|
||||
* Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but
|
||||
* this has an adverse effect on the power consumed.
|
||||
*/
|
||||
config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4;
|
||||
|
||||
/* Enable the filter performance mode where averaging of samples
|
||||
* will be done based on above set bandwidth and ODR.
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode
|
||||
* 1 -> High performance mode(Default)
|
||||
* For more info refer datasheet.
|
||||
*/
|
||||
config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE;
|
||||
|
||||
/* Gyro configuration settings. */
|
||||
/* Set Output Data Rate */
|
||||
config[1].cfg.gyr.odr = BMI2_GYR_ODR_100HZ;
|
||||
|
||||
/* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */
|
||||
config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000;
|
||||
|
||||
/* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */
|
||||
config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE;
|
||||
|
||||
/* Enable/Disable the noise performance mode for precision yaw rate sensing
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode(Default)
|
||||
* 1 -> High performance mode
|
||||
*/
|
||||
config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE;
|
||||
|
||||
/* Enable/Disable the filter performance mode where averaging of samples
|
||||
* will be done based on above set bandwidth and ODR.
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode
|
||||
* 1 -> High performance mode(Default)
|
||||
*/
|
||||
config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE;
|
||||
|
||||
/* Set new configurations. */
|
||||
rslt = bmi270_context_set_sensor_config(config, 2, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
}
|
||||
|
||||
return rslt;
|
||||
}
|
|
@ -0,0 +1,18 @@
|
|||
COINES_INSTALL_PATH ?= ../../../..
|
||||
|
||||
EXAMPLE_FILE ?= fifo_watermark_header_mode.c
|
||||
|
||||
API_LOCATION ?= ../..
|
||||
|
||||
COMMON_LOCATION ?= ..
|
||||
|
||||
C_SRCS += \
|
||||
$(API_LOCATION)/bmi2.c \
|
||||
$(API_LOCATION)/bmi270_context.c \
|
||||
$(COMMON_LOCATION)/common/common.c
|
||||
|
||||
INCLUDEPATHS += \
|
||||
$(API_LOCATION) \
|
||||
$(COMMON_LOCATION)/common
|
||||
|
||||
include $(COINES_INSTALL_PATH)/coines.mk
|
|
@ -0,0 +1,335 @@
|
|||
/**\
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
**/
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Header Files */
|
||||
#include <stdio.h>
|
||||
#include "bmi270_context.h"
|
||||
#include "common.h"
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Macros */
|
||||
|
||||
/*! Buffer size allocated to store raw FIFO data */
|
||||
#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048)
|
||||
|
||||
/*! Length of data to be read from FIFO */
|
||||
#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048)
|
||||
|
||||
/*! Number of accel frames to be extracted from FIFO */
|
||||
|
||||
/*!
|
||||
* Calculation:
|
||||
* fifo_watermark_level = 650, accel_frame_len = 6, gyro_frame_len = 6 header_byte = 1.
|
||||
* fifo_accel_frame_count = (650 / (6 + 6 + 1 )) = 50 frames
|
||||
* NOTE: Extra frames are read in order to get sensor time
|
||||
*/
|
||||
#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(55)
|
||||
|
||||
/*! Number of gyro frames to be extracted from FIFO */
|
||||
#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(55)
|
||||
|
||||
/*! Setting a watermark level in FIFO */
|
||||
#define BMI270_FIFO_WATERMARK_LEVEL UINT16_C(650)
|
||||
|
||||
/*! Macro to read sensortime byte in FIFO */
|
||||
#define SENSORTIME_OVERHEAD_BYTE UINT8_C(3)
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Static Function Declaration */
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for accel and gyro.
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
* @return Status of execution.
|
||||
*/
|
||||
static int8_t set_accel_gyro_config(struct bmi2_dev *dev);
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Functions */
|
||||
|
||||
/* This function starts the execution of program. */
|
||||
int main(void)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Variable to store the available frame length count in FIFO */
|
||||
uint8_t frame_count;
|
||||
|
||||
/* Variable to index bytes. */
|
||||
uint16_t index = 0;
|
||||
|
||||
uint16_t fifo_length = 0;
|
||||
|
||||
uint16_t accel_frame_length;
|
||||
|
||||
uint16_t gyro_frame_length;
|
||||
|
||||
uint8_t try = 1;
|
||||
|
||||
/* To read sensortime, extra 3 bytes are added to fifo buffer */
|
||||
uint16_t fifo_buffer_size = BMI2_FIFO_RAW_DATA_BUFFER_SIZE + SENSORTIME_OVERHEAD_BYTE;
|
||||
|
||||
/* Number of bytes of FIFO data. */
|
||||
uint8_t fifo_data[fifo_buffer_size];
|
||||
|
||||
/* Sensor initialization configuration. */
|
||||
struct bmi2_dev bmi2_dev;
|
||||
|
||||
/* Array of accelerometer frames -> Total bytes =
|
||||
* 55 * (6 axes bytes) = 330 bytes */
|
||||
struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } };
|
||||
|
||||
/* Array of gyro frames -> Total bytes =
|
||||
* 55 * (6 axes bytes) = 330 bytes */
|
||||
struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } };
|
||||
|
||||
/* Initialize FIFO frame structure. */
|
||||
struct bmi2_fifo_frame fifoframe = { 0 };
|
||||
|
||||
/* Accel and gyro sensor are listed in array. */
|
||||
uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO };
|
||||
|
||||
/* Variable to get fifo water-mark interrupt status. */
|
||||
uint16_t int_status = 0;
|
||||
|
||||
uint16_t watermark = 0;
|
||||
|
||||
/* Interface reference is given as a parameter
|
||||
* For I2C : BMI2_I2C_INTF
|
||||
* For SPI : BMI2_SPI_INTF
|
||||
*/
|
||||
rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initialize bmi270_context. */
|
||||
rslt = bmi270_context_init(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Enable accel and gyro sensor. */
|
||||
rslt = bmi270_context_sensor_enable(sensor_sel, 2, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Configuration settings for accel and gyro. */
|
||||
rslt = set_accel_gyro_config(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Before setting FIFO, disable the advance power save mode. */
|
||||
rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initially disable all configurations in fifo. */
|
||||
rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Update FIFO structure. */
|
||||
/* Mapping the buffer to store the fifo data. */
|
||||
fifoframe.data = fifo_data;
|
||||
|
||||
/* Length of FIFO frame. */
|
||||
/* To read sensortime, extra 3 bytes are added to fifo user length. */
|
||||
fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH + SENSORTIME_OVERHEAD_BYTE;
|
||||
|
||||
/* Set FIFO configuration by enabling accel, gyro and timestamp.
|
||||
* NOTE 1: The header mode is enabled by default.
|
||||
* NOTE 2: By default the FIFO operating mode is FIFO mode.
|
||||
* NOTE 3: Sensortime is enabled by default */
|
||||
printf("FIFO is configured in header mode\n");
|
||||
rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* FIFO water-mark interrupt is enabled. */
|
||||
fifoframe.data_int_map = BMI2_FWM_INT;
|
||||
|
||||
/* Map water-mark interrupt to the required interrupt pin. */
|
||||
rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Set water-mark level. */
|
||||
fifoframe.wm_lvl = BMI270_FIFO_WATERMARK_LEVEL;
|
||||
|
||||
/* Set the water-mark level if water-mark interrupt is mapped. */
|
||||
rslt = bmi2_set_fifo_wm(fifoframe.wm_lvl, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
rslt = bmi2_get_fifo_wm(&watermark, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
printf("\nFIFO watermark level is %d\n", watermark);
|
||||
|
||||
while (try <= 10)
|
||||
{
|
||||
/* Read FIFO data on interrupt. */
|
||||
rslt = bmi2_get_int_status(&int_status, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* To check the status of FIFO watermark interrupt. */
|
||||
if ((rslt == BMI2_OK) && (int_status & BMI2_FWM_INT_STATUS_MASK))
|
||||
{
|
||||
printf("\nIteration : %d\n", try);
|
||||
|
||||
printf("\nWatermark interrupt occurred\n");
|
||||
|
||||
rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT;
|
||||
gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT;
|
||||
|
||||
printf("\nFIFO data bytes available : %d \n", fifo_length);
|
||||
printf("\nFIFO data bytes requested : %d \n", fifoframe.length);
|
||||
|
||||
/* Read FIFO data. */
|
||||
rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Read FIFO data on interrupt. */
|
||||
rslt = bmi2_get_int_status(&int_status, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
printf("\nFIFO accel frames requested : %d \n", accel_frame_length);
|
||||
|
||||
/* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */
|
||||
rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev);
|
||||
printf("\nFIFO accel frames extracted : %d \n", accel_frame_length);
|
||||
|
||||
printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length);
|
||||
|
||||
/* Parse the FIFO data to extract gyro data from the FIFO buffer. */
|
||||
rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev);
|
||||
printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length);
|
||||
|
||||
/* Calculating the frame count from the available bytes in FIFO
|
||||
* frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len + header_byte)) */
|
||||
frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH + 1));
|
||||
printf("\nAvailable frame count from available bytes: %d\n", frame_count);
|
||||
|
||||
printf("\nExracted accel frames\n");
|
||||
|
||||
if (accel_frame_length > frame_count)
|
||||
{
|
||||
accel_frame_length = frame_count;
|
||||
}
|
||||
|
||||
/* Print the parsed accelerometer data from the FIFO buffer. */
|
||||
for (index = 0; index < accel_frame_length; index++)
|
||||
{
|
||||
printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x,
|
||||
fifo_accel_data[index].y, fifo_accel_data[index].z);
|
||||
}
|
||||
|
||||
printf("\nExtracted gyro frames\n");
|
||||
|
||||
if (gyro_frame_length > frame_count)
|
||||
{
|
||||
gyro_frame_length = frame_count;
|
||||
}
|
||||
|
||||
/* Print the parsed gyro data from the FIFO buffer. */
|
||||
for (index = 0; index < gyro_frame_length; index++)
|
||||
{
|
||||
printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n",
|
||||
index,
|
||||
fifo_gyro_data[index].x,
|
||||
fifo_gyro_data[index].y,
|
||||
fifo_gyro_data[index].z);
|
||||
}
|
||||
|
||||
/* Print control frames like sensor time and skipped frame count. */
|
||||
printf("Skipped frame count = %d\n", fifoframe.skipped_frame_count);
|
||||
printf("Sensor time = %lu\r\n", fifoframe.sensor_time);
|
||||
|
||||
try++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bmi2_coines_deinit();
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for accel and gyro.
|
||||
*/
|
||||
static int8_t set_accel_gyro_config(struct bmi2_dev *dev)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Structure to define accel and gyro configurations. */
|
||||
struct bmi2_sens_config config[2];
|
||||
|
||||
/* Configure the type of feature. */
|
||||
config[0].type = BMI2_ACCEL;
|
||||
config[1].type = BMI2_GYRO;
|
||||
|
||||
/* Get default configurations for the type of feature selected. */
|
||||
rslt = bmi270_context_get_sensor_config(config, 2, dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* NOTE: The user can change the following configuration parameters according to their requirement. */
|
||||
/* Accel configuration settings. */
|
||||
/* Set Output Data Rate */
|
||||
config[0].cfg.acc.odr = BMI2_ACC_ODR_25HZ;
|
||||
|
||||
/* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */
|
||||
config[0].cfg.acc.range = BMI2_ACC_RANGE_2G;
|
||||
|
||||
/* The bandwidth parameter is used to configure the number of sensor samples that are averaged
|
||||
* if it is set to 2, then 2^(bandwidth parameter) samples
|
||||
* are averaged, resulting in 4 averaged samples
|
||||
* Note1 : For more information, refer the datasheet.
|
||||
* Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but
|
||||
* this has an adverse effect on the power consumed.
|
||||
*/
|
||||
config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4;
|
||||
|
||||
/* Enable the filter performance mode where averaging of samples
|
||||
* will be done based on above set bandwidth and ODR.
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode
|
||||
* 1 -> High performance mode(Default)
|
||||
* For more info refer datasheet.
|
||||
*/
|
||||
config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE;
|
||||
|
||||
/* Gyro configuration settings. */
|
||||
/* Set Output Data Rate */
|
||||
config[1].cfg.gyr.odr = BMI2_GYR_ODR_25HZ;
|
||||
|
||||
/* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */
|
||||
config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000;
|
||||
|
||||
/* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */
|
||||
config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE;
|
||||
|
||||
/* Enable/Disable the noise performance mode for precision yaw rate sensing
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode(Default)
|
||||
* 1 -> High performance mode
|
||||
*/
|
||||
config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE;
|
||||
|
||||
/* Enable/Disable the filter performance mode where averaging of samples
|
||||
* will be done based on above set bandwidth and ODR.
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode
|
||||
* 1 -> High performance mode(Default)
|
||||
*/
|
||||
config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE;
|
||||
|
||||
/* Set new configurations. */
|
||||
rslt = bmi270_context_set_sensor_config(config, 2, dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
}
|
||||
|
||||
return rslt;
|
||||
}
|
|
@ -0,0 +1,18 @@
|
|||
COINES_INSTALL_PATH ?= ../../../..
|
||||
|
||||
EXAMPLE_FILE ?= fifo_watermark_headerless_mode.c
|
||||
|
||||
API_LOCATION ?= ../..
|
||||
|
||||
COMMON_LOCATION ?= ..
|
||||
|
||||
C_SRCS += \
|
||||
$(API_LOCATION)/bmi2.c \
|
||||
$(API_LOCATION)/bmi270_context.c \
|
||||
$(COMMON_LOCATION)/common/common.c
|
||||
|
||||
INCLUDEPATHS += \
|
||||
$(API_LOCATION) \
|
||||
$(COMMON_LOCATION)/common
|
||||
|
||||
include $(COINES_INSTALL_PATH)/coines.mk
|
|
@ -0,0 +1,331 @@
|
|||
/**\
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
**/
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Header Files */
|
||||
#include <stdio.h>
|
||||
#include "bmi270_context.h"
|
||||
#include "common.h"
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Macros */
|
||||
|
||||
/*! Buffer size allocated to store raw FIFO data */
|
||||
#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048)
|
||||
|
||||
/*! Length of data to be read from FIFO */
|
||||
#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048)
|
||||
|
||||
/*! Number of accel frames to be extracted from FIFO */
|
||||
|
||||
/*!
|
||||
* Calculation:
|
||||
* fifo_watermark_level = 650, accel_frame_len = 6, gyro_frame_len = 6.
|
||||
* fifo_accel_frame_count = (650 / (6 + 6 )) = 54 frames
|
||||
*/
|
||||
#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(55)
|
||||
|
||||
/*! Number of gyro frames to be extracted from FIFO */
|
||||
#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(55)
|
||||
|
||||
/*! Setting a watermark level in FIFO */
|
||||
#define BMI270_FIFO_WATERMARK_LEVEL UINT16_C(650)
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Static Function Declaration */
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for accel and gyro.
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
* @return Status of execution.
|
||||
*/
|
||||
static int8_t set_accel_gyro_config(struct bmi2_dev *dev);
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Functions */
|
||||
|
||||
/* This function starts the execution of program. */
|
||||
int main(void)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Variable to index bytes. */
|
||||
uint16_t index = 0;
|
||||
|
||||
/* Variable to store the available frame length count in FIFO */
|
||||
uint8_t frame_count;
|
||||
|
||||
uint16_t fifo_length = 0;
|
||||
|
||||
uint16_t accel_frame_length;
|
||||
|
||||
uint16_t gyro_frame_length;
|
||||
|
||||
uint8_t try = 1;
|
||||
|
||||
/* Number of bytes of FIFO data. */
|
||||
uint8_t fifo_data[BMI2_FIFO_RAW_DATA_BUFFER_SIZE] = { 0 };
|
||||
|
||||
/* Sensor initialization configuration. */
|
||||
struct bmi2_dev bmi2_dev;
|
||||
|
||||
/* Array of accelerometer frames -> Total bytes =
|
||||
* 50 * (6 axes bytes) = 300 bytes */
|
||||
struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } };
|
||||
|
||||
/* Array of gyro frames -> Total bytes =
|
||||
* 50 * (6 axes bytes) = 300 bytes */
|
||||
struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } };
|
||||
|
||||
/* Initialize FIFO frame structure. */
|
||||
struct bmi2_fifo_frame fifoframe = { 0 };
|
||||
|
||||
/* Accel and gyro sensor are listed in array. */
|
||||
uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO };
|
||||
|
||||
/* Variable to get fifo water-mark interrupt status. */
|
||||
uint16_t int_status = 0;
|
||||
|
||||
uint16_t watermark = 0;
|
||||
|
||||
/* Interface reference is given as a parameter
|
||||
* For I2C : BMI2_I2C_INTF
|
||||
* For SPI : BMI2_SPI_INTF
|
||||
*/
|
||||
rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initialize bmi270_context. */
|
||||
rslt = bmi270_context_init(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Enable accel and gyro sensor. */
|
||||
rslt = bmi270_context_sensor_enable(sensor_sel, 2, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Configuration settings for accel and gyro. */
|
||||
rslt = set_accel_gyro_config(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Before setting FIFO, disable the advance power save mode. */
|
||||
rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initially disable all configurations in fifo. */
|
||||
rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Update FIFO structure. */
|
||||
/* Mapping the buffer to store the fifo data. */
|
||||
fifoframe.data = fifo_data;
|
||||
|
||||
/* Length of FIFO frame. */
|
||||
fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH;
|
||||
|
||||
/* Set FIFO configuration by enabling accel, gyro.
|
||||
* NOTE 1: The header mode is enabled by default.
|
||||
* NOTE 2: By default the FIFO operating mode is FIFO mode. */
|
||||
rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
printf("FIFO is configured in headerless mode\n");
|
||||
|
||||
/* To enable headerless mode, disable the header. */
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
rslt = bmi2_set_fifo_config(BMI2_FIFO_HEADER_EN, BMI2_DISABLE, &bmi2_dev);
|
||||
}
|
||||
|
||||
/* FIFO water-mark interrupt is enabled. */
|
||||
fifoframe.data_int_map = BMI2_FWM_INT;
|
||||
|
||||
/* Map water-mark interrupt to the required interrupt pin. */
|
||||
rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Set water-mark level. */
|
||||
fifoframe.wm_lvl = BMI270_FIFO_WATERMARK_LEVEL;
|
||||
|
||||
fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH;
|
||||
|
||||
/* Set the water-mark level if water-mark interrupt is mapped. */
|
||||
rslt = bmi2_set_fifo_wm(fifoframe.wm_lvl, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
rslt = bmi2_get_fifo_wm(&watermark, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
printf("\nFIFO watermark level is %d\n", watermark);
|
||||
|
||||
while (try <= 10)
|
||||
{
|
||||
/* Read FIFO data on interrupt. */
|
||||
rslt = bmi2_get_int_status(&int_status, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* To check the status of FIFO watermark interrupt. */
|
||||
if ((rslt == BMI2_OK) && (int_status & BMI2_FWM_INT_STATUS_MASK))
|
||||
{
|
||||
printf("\nIteration : %d\n", try);
|
||||
|
||||
printf("\nWatermark interrupt occurred\n");
|
||||
|
||||
rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT;
|
||||
gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT;
|
||||
|
||||
printf("\nFIFO data bytes available : %d \n", fifo_length);
|
||||
printf("\nFIFO data bytes requested : %d \n", fifoframe.length);
|
||||
|
||||
/* Read FIFO data. */
|
||||
rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Read FIFO data on interrupt. */
|
||||
rslt = bmi2_get_int_status(&int_status, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
printf("\nFIFO accel frames requested : %d \n", accel_frame_length);
|
||||
|
||||
/* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */
|
||||
rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev);
|
||||
printf("\nFIFO accel frames extracted : %d \n", accel_frame_length);
|
||||
|
||||
printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length);
|
||||
|
||||
/* Parse the FIFO data to extract gyro data from the FIFO buffer. */
|
||||
rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev);
|
||||
printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length);
|
||||
|
||||
/* Calculating the frame count from the available bytes in FIFO
|
||||
* frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len)) */
|
||||
frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH));
|
||||
printf("\nAvailable frame count from available bytes: %d\n", frame_count);
|
||||
|
||||
printf("\nExracted accel frames\n");
|
||||
|
||||
if (accel_frame_length > frame_count)
|
||||
{
|
||||
accel_frame_length = frame_count;
|
||||
}
|
||||
|
||||
/* Print the parsed accelerometer data from the FIFO buffer. */
|
||||
for (index = 0; index < accel_frame_length; index++)
|
||||
{
|
||||
printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x,
|
||||
fifo_accel_data[index].y, fifo_accel_data[index].z);
|
||||
}
|
||||
|
||||
printf("\nExtracted gyro frames\n");
|
||||
|
||||
if (gyro_frame_length > frame_count)
|
||||
{
|
||||
gyro_frame_length = frame_count;
|
||||
}
|
||||
|
||||
/* Print the parsed gyro data from the FIFO buffer. */
|
||||
for (index = 0; index < gyro_frame_length; index++)
|
||||
{
|
||||
printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n",
|
||||
index,
|
||||
fifo_gyro_data[index].x,
|
||||
fifo_gyro_data[index].y,
|
||||
fifo_gyro_data[index].z);
|
||||
}
|
||||
}
|
||||
|
||||
try++;
|
||||
}
|
||||
}
|
||||
|
||||
bmi2_coines_deinit();
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for accel.
|
||||
*/
|
||||
static int8_t set_accel_gyro_config(struct bmi2_dev *dev)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Structure to define accel and gyro configurations. */
|
||||
struct bmi2_sens_config config[2];
|
||||
|
||||
/* Configure the type of feature. */
|
||||
config[0].type = BMI2_ACCEL;
|
||||
config[1].type = BMI2_GYRO;
|
||||
|
||||
/* Get default configurations for the type of feature selected. */
|
||||
rslt = bmi270_context_get_sensor_config(config, 2, dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* NOTE: The user can change the following configuration parameter according to their requirement. */
|
||||
/* Accel configuration settings. */
|
||||
/* Set Output Data Rate */
|
||||
config[0].cfg.acc.odr = BMI2_ACC_ODR_100HZ;
|
||||
|
||||
/* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */
|
||||
config[0].cfg.acc.range = BMI2_ACC_RANGE_2G;
|
||||
|
||||
/* The bandwidth parameter is used to configure the number of sensor samples that are averaged
|
||||
* if it is set to 2, then 2^(bandwidth parameter) samples
|
||||
* are averaged, resulting in 4 averaged samples
|
||||
* Note1 : For more information, refer the datasheet.
|
||||
* Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but
|
||||
* this has an adverse effect on the power consumed.
|
||||
*/
|
||||
config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4;
|
||||
|
||||
/* Enable the filter performance mode where averaging of samples
|
||||
* will be done based on above set bandwidth and ODR.
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode
|
||||
* 1 -> High performance mode(Default)
|
||||
* For more info refer datasheet.
|
||||
*/
|
||||
config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE;
|
||||
|
||||
/* Gyro configuration settings. */
|
||||
/* Set Output Data Rate */
|
||||
config[1].cfg.gyr.odr = BMI2_GYR_ODR_100HZ;
|
||||
|
||||
/* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */
|
||||
config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000;
|
||||
|
||||
/* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */
|
||||
config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE;
|
||||
|
||||
/* Enable/Disable the noise performance mode for precision yaw rate sensing
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode(Default)
|
||||
* 1 -> High performance mode
|
||||
*/
|
||||
config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE;
|
||||
|
||||
/* Enable/Disable the filter performance mode where averaging of samples
|
||||
* will be done based on above set bandwidth and ODR.
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode
|
||||
* 1 -> High performance mode(Default)
|
||||
*/
|
||||
config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE;
|
||||
|
||||
/* Set new configurations. */
|
||||
rslt = bmi270_context_set_sensor_config(config, 2, dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
}
|
||||
|
||||
return rslt;
|
||||
}
|
|
@ -0,0 +1,18 @@
|
|||
COINES_INSTALL_PATH ?= ../../../..
|
||||
|
||||
EXAMPLE_FILE ?= gyro.c
|
||||
|
||||
API_LOCATION ?= ../..
|
||||
|
||||
COMMON_LOCATION ?= ..
|
||||
|
||||
C_SRCS += \
|
||||
$(API_LOCATION)/bmi2.c \
|
||||
$(API_LOCATION)/bmi270_context.c \
|
||||
$(COMMON_LOCATION)/common/common.c
|
||||
|
||||
INCLUDEPATHS += \
|
||||
$(API_LOCATION) \
|
||||
$(COMMON_LOCATION)/common
|
||||
|
||||
include $(COINES_INSTALL_PATH)/coines.mk
|
|
@ -0,0 +1,195 @@
|
|||
/**\
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
**/
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Header Files */
|
||||
#include <stdio.h>
|
||||
#include "bmi270_context.h"
|
||||
#include "common.h"
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Static Function Declaration */
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for gyro.
|
||||
*
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @return Status of execution.
|
||||
*/
|
||||
static int8_t set_gyro_config(struct bmi2_dev *dev);
|
||||
|
||||
/*!
|
||||
* @brief This function converts lsb to degree per second for 16 bit gyro at
|
||||
* range 125, 250, 500, 1000 or 2000dps.
|
||||
*
|
||||
* @param[in] val : LSB from each axis.
|
||||
* @param[in] dps : Degree per second.
|
||||
* @param[in] bit_width : Resolution for gyro.
|
||||
*
|
||||
* @return Degree per second.
|
||||
*/
|
||||
static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width);
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Functions */
|
||||
|
||||
/* This function starts the execution of program. */
|
||||
int main(void)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Variable to define limit to print gyro data. */
|
||||
uint8_t limit = 10;
|
||||
|
||||
uint8_t indx = 0;
|
||||
|
||||
float x = 0, y = 0, z = 0;
|
||||
|
||||
/* Sensor initialization configuration. */
|
||||
struct bmi2_dev bmi2_dev;
|
||||
|
||||
/* Create an instance of sensor data structure. */
|
||||
struct bmi2_sensor_data sensor_data = { 0 };
|
||||
|
||||
/* Assign gyro sensor to variable. */
|
||||
uint8_t sens_list = BMI2_GYRO;
|
||||
|
||||
/* Initialize the interrupt status of gyro. */
|
||||
uint16_t int_status = 0;
|
||||
|
||||
/* Interface reference is given as a parameter
|
||||
* For I2C : BMI2_I2C_INTF
|
||||
* For SPI : BMI2_SPI_INTF
|
||||
*/
|
||||
rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initialize bmi270_context. */
|
||||
rslt = bmi270_context_init(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Enable the selected sensors. */
|
||||
rslt = bmi270_context_sensor_enable(&sens_list, 1, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Gyro configuration settings. */
|
||||
rslt = set_gyro_config(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Select gyro sensor. */
|
||||
sensor_data.type = BMI2_GYRO;
|
||||
printf("Gyro and DPS data\n");
|
||||
printf("Gyro data collected at Range 2000 dps with 16-bit resolution\n");
|
||||
|
||||
/* Loop to print gyro data when interrupt occurs. */
|
||||
while (indx <= limit)
|
||||
{
|
||||
/* To get the data ready interrupt status of gyro. */
|
||||
rslt = bmi2_get_int_status(&int_status, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* To check the data ready interrupt status and print the status for 10 samples. */
|
||||
if (int_status & BMI2_GYR_DRDY_INT_MASK)
|
||||
{
|
||||
/* Get gyro data for x, y and z axis. */
|
||||
rslt = bmi270_context_get_sensor_data(&sensor_data, 1, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
printf("\nGyr_X = %d\t", sensor_data.sens_data.gyr.x);
|
||||
printf("Gyr_Y = %d\t", sensor_data.sens_data.gyr.y);
|
||||
printf("Gyr_Z = %d\r\n", sensor_data.sens_data.gyr.z);
|
||||
|
||||
/* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */
|
||||
x = lsb_to_dps(sensor_data.sens_data.gyr.x, 2000, bmi2_dev.resolution);
|
||||
y = lsb_to_dps(sensor_data.sens_data.gyr.y, 2000, bmi2_dev.resolution);
|
||||
z = lsb_to_dps(sensor_data.sens_data.gyr.z, 2000, bmi2_dev.resolution);
|
||||
|
||||
/* Print the data in dps. */
|
||||
printf("Gyr_DPS_X = %4.2f, Gyr_DPS_Y = %4.2f, Gyr_DPS_Z = %4.2f\n", x, y, z);
|
||||
|
||||
indx++;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bmi2_coines_deinit();
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for gyro.
|
||||
*/
|
||||
static int8_t set_gyro_config(struct bmi2_dev *dev)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Structure to define the type of sensor and its configurations. */
|
||||
struct bmi2_sens_config config;
|
||||
|
||||
/* Configure the type of feature. */
|
||||
config.type = BMI2_GYRO;
|
||||
|
||||
/* Get default configurations for the type of feature selected. */
|
||||
rslt = bmi270_context_get_sensor_config(&config, 1, dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Map data ready interrupt to interrupt pin. */
|
||||
rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT2, dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* The user can change the following configuration parameters according to their requirement. */
|
||||
/* Set Output Data Rate */
|
||||
config.cfg.gyr.odr = BMI2_GYR_ODR_200HZ;
|
||||
|
||||
/* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */
|
||||
config.cfg.gyr.range = BMI2_GYR_RANGE_2000;
|
||||
|
||||
/* Gyroscope bandwidth parameters. By default the gyro bandwidth is in normal mode. */
|
||||
config.cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE;
|
||||
|
||||
/* Enable/Disable the noise performance mode for precision yaw rate sensing
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode(Default)
|
||||
* 1 -> High performance mode
|
||||
*/
|
||||
config.cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE;
|
||||
|
||||
/* Enable/Disable the filter performance mode where averaging of samples
|
||||
* will be done based on above set bandwidth and ODR.
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode
|
||||
* 1 -> High performance mode(Default)
|
||||
*/
|
||||
config.cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE;
|
||||
|
||||
/* Set the gyro configurations. */
|
||||
rslt = bmi270_context_set_sensor_config(&config, 1, dev);
|
||||
}
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This function converts lsb to degree per second for 16 bit gyro at
|
||||
* range 125, 250, 500, 1000 or 2000dps.
|
||||
*/
|
||||
static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width)
|
||||
{
|
||||
float half_scale = ((float)(1 << bit_width) / 2.0f);
|
||||
|
||||
return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val);
|
||||
}
|
|
@ -0,0 +1,18 @@
|
|||
COINES_INSTALL_PATH ?= ../../../..
|
||||
|
||||
EXAMPLE_FILE ?= step_counter.c
|
||||
|
||||
API_LOCATION ?= ../..
|
||||
|
||||
COMMON_LOCATION ?= ..
|
||||
|
||||
C_SRCS += \
|
||||
$(API_LOCATION)/bmi2.c \
|
||||
$(API_LOCATION)/bmi270_context.c \
|
||||
$(COMMON_LOCATION)/common/common.c
|
||||
|
||||
INCLUDEPATHS += \
|
||||
$(API_LOCATION) \
|
||||
$(COMMON_LOCATION)/common
|
||||
|
||||
include $(COINES_INSTALL_PATH)/coines.mk
|
|
@ -0,0 +1,146 @@
|
|||
/**\
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
**/
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Header Files */
|
||||
#include <stdio.h>
|
||||
#include "bmi270_context.h"
|
||||
#include "common.h"
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Static function declaration */
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for step counter.
|
||||
*
|
||||
* @param[in] bmi2_dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @return Status of execution.
|
||||
*/
|
||||
static int8_t set_feature_config(struct bmi2_dev *bmi2_dev);
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Functions */
|
||||
|
||||
/* This function starts the execution of program. */
|
||||
int main(void)
|
||||
{
|
||||
/* Sensor initialization configuration. */
|
||||
struct bmi2_dev bmi2_dev;
|
||||
|
||||
/* Structure to define type of sensor and their respective data. */
|
||||
struct bmi2_sensor_data sensor_data;
|
||||
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Accel sensor and step counter feature are listed in array. */
|
||||
uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_STEP_COUNTER };
|
||||
|
||||
/* Variable to get step counter interrupt status. */
|
||||
uint16_t int_status = 0;
|
||||
|
||||
/* Select features and their pins to be mapped to. */
|
||||
struct bmi2_sens_int_config sens_int = { .type = BMI2_STEP_COUNTER, .hw_int_pin = BMI2_INT2 };
|
||||
|
||||
/* Type of sensor to get step counter data. */
|
||||
sensor_data.type = BMI2_STEP_COUNTER;
|
||||
|
||||
/* Interface reference is given as a parameter
|
||||
* For I2C : BMI2_I2C_INTF
|
||||
* For SPI : BMI2_SPI_INTF
|
||||
*/
|
||||
rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initialize bmi270_context. */
|
||||
rslt = bmi270_context_init(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Enable the selected sensor. */
|
||||
rslt = bmi270_context_sensor_enable(sensor_sel, 2, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Set the feature configuration for step counter. */
|
||||
rslt = set_feature_config(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
printf("Step counter watermark level set to 1 (20 steps)\n");
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Map the step counter feature interrupt. */
|
||||
rslt = bmi270_context_map_feat_int(&sens_int, 1, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Move the board in steps for 20 times to get step counter interrupt. */
|
||||
printf("Move the board in steps\n");
|
||||
|
||||
/* Loop to get number of steps counted. */
|
||||
do
|
||||
{
|
||||
/* To get the interrupt status of the step counter. */
|
||||
rslt = bmi2_get_int_status(&int_status, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* To check the interrupt status of the step counter. */
|
||||
if (int_status & BMI270_CONTEXT_STEP_CNT_STATUS_MASK)
|
||||
{
|
||||
printf("Step counter interrupt occurred when watermark level (20 steps) is reached\n");
|
||||
|
||||
/* Get step counter output. */
|
||||
rslt = bmi270_context_get_sensor_data(&sensor_data, 1, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Print the step counter output. */
|
||||
printf("No of steps counted = %ld", sensor_data.sens_data.step_counter_output);
|
||||
break;
|
||||
}
|
||||
} while (rslt == BMI2_OK);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bmi2_coines_deinit();
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for step counter.
|
||||
*/
|
||||
static int8_t set_feature_config(struct bmi2_dev *bmi2_dev)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Structure to define the type of sensor and its configurations. */
|
||||
struct bmi2_sens_config config;
|
||||
|
||||
/* Configure the type of sensor. */
|
||||
config.type = BMI2_STEP_COUNTER;
|
||||
|
||||
/* Get default configurations for the type of feature selected. */
|
||||
rslt = bmi270_context_get_sensor_config(&config, 1, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Setting water-mark level to 1 for step counter to get interrupt after 20 step counts. Every 20 steps once
|
||||
* output triggers. */
|
||||
config.cfg.step_counter.watermark_level = 1;
|
||||
|
||||
/* Set new configuration. */
|
||||
rslt = bmi270_context_set_sensor_config(&config, 1, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
}
|
||||
|
||||
return rslt;
|
||||
}
|
|
@ -0,0 +1,18 @@
|
|||
COINES_INSTALL_PATH ?= ../../../..
|
||||
|
||||
EXAMPLE_FILE ?= accel.c
|
||||
|
||||
API_LOCATION ?= ../..
|
||||
|
||||
COMMON_LOCATION ?= ..
|
||||
|
||||
C_SRCS += \
|
||||
$(API_LOCATION)/bmi2.c \
|
||||
$(API_LOCATION)/bmi270_hc.c \
|
||||
$(COMMON_LOCATION)/common/common.c
|
||||
|
||||
INCLUDEPATHS += \
|
||||
$(API_LOCATION) \
|
||||
$(COMMON_LOCATION)/common
|
||||
|
||||
include $(COINES_INSTALL_PATH)/coines.mk
|
|
@ -0,0 +1,201 @@
|
|||
/**\
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
**/
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Header Files */
|
||||
#include <stdio.h>
|
||||
#include "bmi270_hc.h"
|
||||
#include "common.h"
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Macro definition */
|
||||
|
||||
/*! Earth's gravity in m/s^2 */
|
||||
#define GRAVITY_EARTH (9.80665f)
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Static Function Declaration */
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for accel.
|
||||
*
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @return Status of execution.
|
||||
*/
|
||||
static int8_t set_accel_config(struct bmi2_dev *bmi2_dev);
|
||||
|
||||
/*!
|
||||
* @brief This function converts lsb to meter per second squared for 16 bit accelerometer at
|
||||
* range 2G, 4G, 8G or 16G.
|
||||
*
|
||||
* @param[in] val : LSB from each axis.
|
||||
* @param[in] g_range : Gravity range.
|
||||
* @param[in] bit_width : Resolution for accel.
|
||||
*
|
||||
* @return Gravity.
|
||||
*/
|
||||
static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width);
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Functions */
|
||||
|
||||
/* This function starts the execution of program. */
|
||||
int main(void)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Variable to define limit to print accel data. */
|
||||
uint8_t limit = 20;
|
||||
|
||||
/* Assign accel sensor to variable. */
|
||||
uint8_t sensor_list = BMI2_ACCEL;
|
||||
|
||||
/* Sensor initialization configuration. */
|
||||
struct bmi2_dev bmi2_dev;
|
||||
|
||||
/* Create an instance of sensor data structure. */
|
||||
struct bmi2_sensor_data sensor_data = { 0 };
|
||||
|
||||
/* Initialize the interrupt status of accel. */
|
||||
uint16_t int_status = 0;
|
||||
|
||||
uint8_t indx = 0;
|
||||
float x = 0, y = 0, z = 0;
|
||||
|
||||
/* Interface reference is given as a parameter
|
||||
* For I2C : BMI2_I2C_INTF
|
||||
* For SPI : BMI2_SPI_INTF
|
||||
*/
|
||||
rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initialize bmi270_hc. */
|
||||
rslt = bmi270_hc_init(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Enable the accel sensor. */
|
||||
rslt = bmi270_hc_sensor_enable(&sensor_list, 1, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Accel configuration settings. */
|
||||
rslt = set_accel_config(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Assign accel sensor. */
|
||||
sensor_data.type = BMI2_ACCEL;
|
||||
printf("Accel and m/s2 data \n");
|
||||
printf("Accel data collected at 2G Range with 16-bit resolution\n");
|
||||
|
||||
/* Loop to print the accel data when interrupt occurs. */
|
||||
while (indx <= limit)
|
||||
{
|
||||
/* To get the status of accel data ready interrupt. */
|
||||
rslt = bmi2_get_int_status(&int_status, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* To check the accel data ready interrupt status and print the status for 10 samples. */
|
||||
if (int_status & BMI2_ACC_DRDY_INT_MASK)
|
||||
{
|
||||
/* Get accelerometer data for x, y and z axis. */
|
||||
rslt = bmi270_hc_get_sensor_data(&sensor_data, 1, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
printf("\nAcc_X = %d\t", sensor_data.sens_data.acc.x);
|
||||
printf("Acc_Y = %d\t", sensor_data.sens_data.acc.y);
|
||||
printf("Acc_Z = %d\r\n", sensor_data.sens_data.acc.z);
|
||||
|
||||
/* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */
|
||||
x = lsb_to_mps2(sensor_data.sens_data.acc.x, 2, bmi2_dev.resolution);
|
||||
y = lsb_to_mps2(sensor_data.sens_data.acc.y, 2, bmi2_dev.resolution);
|
||||
z = lsb_to_mps2(sensor_data.sens_data.acc.z, 2, bmi2_dev.resolution);
|
||||
|
||||
/* Print the data in m/s2. */
|
||||
printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z);
|
||||
|
||||
indx++;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bmi2_coines_deinit();
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for accel.
|
||||
*/
|
||||
static int8_t set_accel_config(struct bmi2_dev *bmi2_dev)
|
||||
{
|
||||
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Structure to define accelerometer configuration. */
|
||||
struct bmi2_sens_config config;
|
||||
|
||||
/* Configure the type of feature. */
|
||||
config.type = BMI2_ACCEL;
|
||||
|
||||
/* Get default configurations for the type of feature selected. */
|
||||
rslt = bmi270_hc_get_sensor_config(&config, 1, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* NOTE: The user can change the following configuration parameters according to their requirement. */
|
||||
/* Set Output Data Rate */
|
||||
config.cfg.acc.odr = BMI2_ACC_ODR_200HZ;
|
||||
|
||||
/* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */
|
||||
config.cfg.acc.range = BMI2_ACC_RANGE_2G;
|
||||
|
||||
/* The bandwidth parameter is used to configure the number of sensor samples that are averaged
|
||||
* if it is set to 2, then 2^(bandwidth parameter) samples
|
||||
* are averaged, resulting in 4 averaged samples.
|
||||
* Note1 : For more information, refer the datasheet.
|
||||
* Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but
|
||||
* this has an adverse effect on the power consumed.
|
||||
*/
|
||||
config.cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4;
|
||||
|
||||
/* Enable the filter performance mode where averaging of samples
|
||||
* will be done based on above set bandwidth and ODR.
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode
|
||||
* 1 -> High performance mode(Default)
|
||||
* For more info refer datasheet.
|
||||
*/
|
||||
config.cfg.acc.filter_perf = BMI2_PERF_OPT_MODE;
|
||||
|
||||
/* Set the accel configurations. */
|
||||
rslt = bmi270_hc_set_sensor_config(&config, 1, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Map data ready interrupt to interrupt pin. */
|
||||
rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
}
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This function converts lsb to meter per second squared for 16 bit accelerometer at
|
||||
* range 2G, 4G, 8G or 16G.
|
||||
*/
|
||||
static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width)
|
||||
{
|
||||
float half_scale = ((float)(1 << bit_width) / 2.0f);
|
||||
|
||||
return (GRAVITY_EARTH * val * g_range) / half_scale;
|
||||
}
|
|
@ -0,0 +1,18 @@
|
|||
COINES_INSTALL_PATH ?= ../../../..
|
||||
|
||||
EXAMPLE_FILE ?= accel_gyro.c
|
||||
|
||||
API_LOCATION ?= ../..
|
||||
|
||||
COMMON_LOCATION ?= ..
|
||||
|
||||
C_SRCS += \
|
||||
$(API_LOCATION)/bmi2.c \
|
||||
$(API_LOCATION)/bmi270_hc.c \
|
||||
$(COMMON_LOCATION)/common/common.c
|
||||
|
||||
INCLUDEPATHS += \
|
||||
$(API_LOCATION) \
|
||||
$(COMMON_LOCATION)/common
|
||||
|
||||
include $(COINES_INSTALL_PATH)/coines.mk
|
|
@ -0,0 +1,268 @@
|
|||
/**\
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
**/
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Header Files */
|
||||
#include <stdio.h>
|
||||
#include "bmi270_hc.h"
|
||||
#include "common.h"
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Macro definition */
|
||||
|
||||
/*! Earth's gravity in m/s^2 */
|
||||
#define GRAVITY_EARTH (9.80665f)
|
||||
|
||||
/*! Macros to select the sensors */
|
||||
#define ACCEL UINT8_C(0x00)
|
||||
#define GYRO UINT8_C(0x01)
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Static Function Declaration */
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for accel.
|
||||
*
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @return Status of execution.
|
||||
*/
|
||||
static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev);
|
||||
|
||||
/*!
|
||||
* @brief This function converts lsb to meter per second squared for 16 bit accelerometer at
|
||||
* range 2G, 4G, 8G or 16G.
|
||||
*
|
||||
* @param[in] val : LSB from each axis.
|
||||
* @param[in] g_range : Gravity range.
|
||||
* @param[in] bit_width : Resolution for accel.
|
||||
*
|
||||
* @return Gravity.
|
||||
*/
|
||||
static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width);
|
||||
|
||||
/*!
|
||||
* @brief This function converts lsb to degree per second for 16 bit gyro at
|
||||
* range 125, 250, 500, 1000 or 2000dps.
|
||||
*
|
||||
* @param[in] val : LSB from each axis.
|
||||
* @param[in] dps : Degree per second.
|
||||
* @param[in] bit_width : Resolution for gyro.
|
||||
*
|
||||
* @return Degree per second.
|
||||
*/
|
||||
static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width);
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Functions */
|
||||
|
||||
/* This function starts the execution of program. */
|
||||
int main(void)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Variable to define limit to print accel data. */
|
||||
uint8_t limit = 10;
|
||||
|
||||
/* Assign accel and gyro sensor to variable. */
|
||||
uint8_t sensor_list[2] = { BMI2_ACCEL, BMI2_GYRO };
|
||||
|
||||
/* Sensor initialization configuration. */
|
||||
struct bmi2_dev bmi2_dev;
|
||||
|
||||
/* Create an instance of sensor data structure. */
|
||||
struct bmi2_sensor_data sensor_data[2] = { { 0 } };
|
||||
|
||||
/* Initialize the interrupt status of accel and gyro. */
|
||||
uint16_t int_status = 0;
|
||||
|
||||
uint8_t indx = 1;
|
||||
|
||||
float x = 0, y = 0, z = 0;
|
||||
|
||||
/* Interface reference is given as a parameter
|
||||
* For I2C : BMI2_I2C_INTF
|
||||
* For SPI : BMI2_SPI_INTF
|
||||
*/
|
||||
rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initialize bmi270_hc. */
|
||||
rslt = bmi270_hc_init(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Enable the accel and gyro sensor. */
|
||||
rslt = bmi270_hc_sensor_enable(sensor_list, 2, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Accel and gyro configuration settings. */
|
||||
rslt = set_accel_gyro_config(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Assign accel and gyro sensor. */
|
||||
sensor_data[ACCEL].type = BMI2_ACCEL;
|
||||
sensor_data[GYRO].type = BMI2_GYRO;
|
||||
|
||||
/* Loop to print accel and gyro data when interrupt occurs. */
|
||||
while (indx <= limit)
|
||||
{
|
||||
/* To get the data ready interrupt status of accel and gyro. */
|
||||
rslt = bmi2_get_int_status(&int_status, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* To check the data ready interrupt status and print the status for 10 samples. */
|
||||
if ((int_status & BMI2_ACC_DRDY_INT_MASK) && (int_status & BMI2_GYR_DRDY_INT_MASK))
|
||||
{
|
||||
/* Get accel and gyro data for x, y and z axis. */
|
||||
rslt = bmi270_hc_get_sensor_data(sensor_data, 2, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
printf("\n******* Accel(Raw and m/s2) Gyro(Raw and dps) data : %d *******\n", indx);
|
||||
|
||||
printf("\nAcc_x = %d\t", sensor_data[ACCEL].sens_data.acc.x);
|
||||
printf("Acc_y = %d\t", sensor_data[ACCEL].sens_data.acc.y);
|
||||
printf("Acc_z = %d", sensor_data[ACCEL].sens_data.acc.z);
|
||||
|
||||
/* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */
|
||||
x = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.x, 2, bmi2_dev.resolution);
|
||||
y = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.y, 2, bmi2_dev.resolution);
|
||||
z = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.z, 2, bmi2_dev.resolution);
|
||||
|
||||
/* Print the data in m/s2. */
|
||||
printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z);
|
||||
|
||||
printf("\nGyr_X = %d\t", sensor_data[GYRO].sens_data.gyr.x);
|
||||
printf("Gyr_Y = %d\t", sensor_data[GYRO].sens_data.gyr.y);
|
||||
printf("Gyr_Z = %d\n", sensor_data[GYRO].sens_data.gyr.z);
|
||||
|
||||
/* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */
|
||||
x = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.x, 2000, bmi2_dev.resolution);
|
||||
y = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.y, 2000, bmi2_dev.resolution);
|
||||
z = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.z, 2000, bmi2_dev.resolution);
|
||||
|
||||
/* Print the data in dps. */
|
||||
printf("Gyro_DPS_X = %4.2f, Gyro_DPS_Y = %4.2f, Gyro_DPS_Z = %4.2f\n", x, y, z);
|
||||
|
||||
indx++;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bmi2_coines_deinit();
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for accel and gyro.
|
||||
*/
|
||||
static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Structure to define accelerometer and gyro configuration. */
|
||||
struct bmi2_sens_config config[2];
|
||||
|
||||
/* Configure the type of feature. */
|
||||
config[ACCEL].type = BMI2_ACCEL;
|
||||
config[GYRO].type = BMI2_GYRO;
|
||||
|
||||
/* Get default configurations for the type of feature selected. */
|
||||
rslt = bmi270_hc_get_sensor_config(config, 2, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Map data ready interrupt to interrupt pin. */
|
||||
rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* NOTE: The user can change the following configuration parameters according to their requirement. */
|
||||
/* Set Output Data Rate */
|
||||
config[ACCEL].cfg.acc.odr = BMI2_ACC_ODR_200HZ;
|
||||
|
||||
/* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */
|
||||
config[ACCEL].cfg.acc.range = BMI2_ACC_RANGE_2G;
|
||||
|
||||
/* The bandwidth parameter is used to configure the number of sensor samples that are averaged
|
||||
* if it is set to 2, then 2^(bandwidth parameter) samples
|
||||
* are averaged, resulting in 4 averaged samples.
|
||||
* Note1 : For more information, refer the datasheet.
|
||||
* Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but
|
||||
* this has an adverse effect on the power consumed.
|
||||
*/
|
||||
config[ACCEL].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4;
|
||||
|
||||
/* Enable the filter performance mode where averaging of samples
|
||||
* will be done based on above set bandwidth and ODR.
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode
|
||||
* 1 -> High performance mode(Default)
|
||||
* For more info refer datasheet.
|
||||
*/
|
||||
config[ACCEL].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE;
|
||||
|
||||
/* The user can change the following configuration parameters according to their requirement. */
|
||||
/* Set Output Data Rate */
|
||||
config[GYRO].cfg.gyr.odr = BMI2_GYR_ODR_200HZ;
|
||||
|
||||
/* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */
|
||||
config[GYRO].cfg.gyr.range = BMI2_GYR_RANGE_2000;
|
||||
|
||||
/* Gyroscope bandwidth parameters. By default the gyro bandwidth is in normal mode. */
|
||||
config[GYRO].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE;
|
||||
|
||||
/* Enable/Disable the noise performance mode for precision yaw rate sensing
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode(Default)
|
||||
* 1 -> High performance mode
|
||||
*/
|
||||
config[GYRO].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE;
|
||||
|
||||
/* Enable/Disable the filter performance mode where averaging of samples
|
||||
* will be done based on above set bandwidth and ODR.
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode
|
||||
* 1 -> High performance mode(Default)
|
||||
*/
|
||||
config[GYRO].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE;
|
||||
|
||||
/* Set the accel and gyro configurations. */
|
||||
rslt = bmi270_hc_set_sensor_config(config, 2, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
}
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This function converts lsb to meter per second squared for 16 bit accelerometer at
|
||||
* range 2G, 4G, 8G or 16G.
|
||||
*/
|
||||
static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width)
|
||||
{
|
||||
float half_scale = ((float)(1 << bit_width) / 2.0f);
|
||||
|
||||
return (GRAVITY_EARTH * val * g_range) / half_scale;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This function converts lsb to degree per second for 16 bit gyro at
|
||||
* range 125, 250, 500, 1000 or 2000dps.
|
||||
*/
|
||||
static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width)
|
||||
{
|
||||
float half_scale = ((float)(1 << bit_width) / 2.0f);
|
||||
|
||||
return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val);
|
||||
}
|
|
@ -0,0 +1,18 @@
|
|||
COINES_INSTALL_PATH ?= ../../../..
|
||||
|
||||
EXAMPLE_FILE ?= activity_recognition.c
|
||||
|
||||
API_LOCATION ?= ../..
|
||||
|
||||
COMMON_LOCATION ?= ..
|
||||
|
||||
C_SRCS += \
|
||||
$(API_LOCATION)/bmi2.c \
|
||||
$(API_LOCATION)/bmi270_hc.c \
|
||||
$(COMMON_LOCATION)/common/common.c
|
||||
|
||||
INCLUDEPATHS += \
|
||||
$(API_LOCATION) \
|
||||
$(COMMON_LOCATION)/common
|
||||
|
||||
include $(COINES_INSTALL_PATH)/coines.mk
|
|
@ -0,0 +1,154 @@
|
|||
/**\
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
**/
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Header Files */
|
||||
#include <stdio.h>
|
||||
#include "bmi270_hc.h"
|
||||
#include "common.h"
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Static function declaration */
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for step counter.
|
||||
*
|
||||
* @param[in] bmi2_dev : Structure instance of bmi2_dev.
|
||||
*
|
||||
* @return Status of execution.
|
||||
*/
|
||||
static int8_t set_config(struct bmi2_dev *bmi2_dev);
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Functions */
|
||||
/* This function starts the execution of program. */
|
||||
int main(void)
|
||||
{
|
||||
/* Sensor initialization configuration. */
|
||||
struct bmi2_dev bmi2_dev;
|
||||
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Variable to define index for FIFO data */
|
||||
uint8_t count;
|
||||
|
||||
/* Various Activity recognitions are listed in array. */
|
||||
const char *activity_reg_output[6] = { "OTHERS", "STILL", "WALKING", "RUNNING", "ON_BICYCLE", "IN_VEHICLE" };
|
||||
|
||||
/* Accel sensor and step activity feature are listed in array. */
|
||||
uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_ACTIVITY_RECOGNITION };
|
||||
|
||||
/* Array to define FIFO data */
|
||||
uint8_t fifo_data[516] = { 0 };
|
||||
|
||||
/* Variable to store number of activity frames */
|
||||
uint16_t act_frames;
|
||||
|
||||
/* Structure to define a FIFO */
|
||||
struct bmi2_fifo_frame fifoframe = { 0 };
|
||||
|
||||
/* Structure to define output for activity recognition */
|
||||
struct bmi2_act_recog_output act_recog_data[5] = { { 0 } };
|
||||
|
||||
uint16_t fifo_length;
|
||||
|
||||
/* Interface reference is given as a parameter
|
||||
* For I2C : BMI2_I2C_INTF
|
||||
* For SPI : BMI2_SPI_INTF
|
||||
*/
|
||||
rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initialize bmi270_hc. */
|
||||
rslt = bmi270_hc_init(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Enable the selected sensor. */
|
||||
rslt = bmi270_hc_sensor_enable(sensor_sel, 2, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
rslt = set_config(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Update FIFO structure */
|
||||
fifoframe.data = fifo_data;
|
||||
fifoframe.length = 516;
|
||||
|
||||
rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev);
|
||||
printf("Fifo length = %d \n", fifo_length);
|
||||
|
||||
/* Read FIFO data */
|
||||
rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
while (rslt != BMI2_W_FIFO_EMPTY)
|
||||
{
|
||||
/* Provide the number of frames to be read */
|
||||
act_frames = 5;
|
||||
|
||||
printf("Requested FIFO data frames : %d\n", act_frames);
|
||||
|
||||
/* Get the activity output */
|
||||
rslt = bmi270_hc_get_act_recog_output(act_recog_data, &act_frames, &fifoframe, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
printf("Parsed FIFO data frames : %d\r\n", act_frames);
|
||||
|
||||
for (count = 0; count < act_frames; count++)
|
||||
{
|
||||
printf(
|
||||
"Activity Recognition output[%d]:Sensor time: %lu\t Previous activity: %s\t current: %s\n",
|
||||
count,
|
||||
act_recog_data[count].time_stamp,
|
||||
activity_reg_output[act_recog_data[count].prev_act],
|
||||
activity_reg_output[act_recog_data[count].curr_act]);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bmi2_coines_deinit();
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
static int8_t set_config(struct bmi2_dev *bmi2_dev)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
uint8_t temp_data[2];
|
||||
|
||||
/* Disable advanced power save */
|
||||
rslt = bmi2_set_adv_power_save(BMI2_DISABLE, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* Reset FIFO */
|
||||
rslt = bmi2_set_command_register(BMI2_FIFO_FLUSH_CMD, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
printf("Move the board for activity recognition for 30 sec :\n");
|
||||
bmi2_dev->delay_us(30000000, bmi2_dev->intf_ptr);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
rslt = bmi2_get_regs(BMI2_FIFO_CONFIG_0_ADDR, temp_data, 2, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
}
|
||||
}
|
||||
|
||||
return rslt;
|
||||
}
|
|
@ -0,0 +1,372 @@
|
|||
/**
|
||||
* Copyright (C) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include "coines.h"
|
||||
#include "bmi2_defs.h"
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Macro definitions */
|
||||
#define BMI2XY_SHUTTLE_ID UINT16_C(0x1B8)
|
||||
|
||||
/*! Macro that defines read write length */
|
||||
#define READ_WRITE_LEN UINT8_C(46)
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Static variable definition */
|
||||
|
||||
/*! Variable that holds the I2C device address or SPI chip selection */
|
||||
static uint8_t dev_addr;
|
||||
|
||||
/******************************************************************************/
|
||||
/*! User interface functions */
|
||||
|
||||
/*!
|
||||
* I2C read function map to COINES platform
|
||||
*/
|
||||
BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr)
|
||||
{
|
||||
uint8_t dev_addr = *(uint8_t*)intf_ptr;
|
||||
|
||||
return coines_read_i2c(dev_addr, reg_addr, reg_data, (uint16_t)len);
|
||||
}
|
||||
|
||||
/*!
|
||||
* I2C write function map to COINES platform
|
||||
*/
|
||||
BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr)
|
||||
{
|
||||
uint8_t dev_addr = *(uint8_t*)intf_ptr;
|
||||
|
||||
return coines_write_i2c(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len);
|
||||
}
|
||||
|
||||
/*!
|
||||
* SPI read function map to COINES platform
|
||||
*/
|
||||
BMI2_INTF_RETURN_TYPE bmi2_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr)
|
||||
{
|
||||
uint8_t dev_addr = *(uint8_t*)intf_ptr;
|
||||
|
||||
return coines_read_spi(dev_addr, reg_addr, reg_data, (uint16_t)len);
|
||||
}
|
||||
|
||||
/*!
|
||||
* SPI write function map to COINES platform
|
||||
*/
|
||||
BMI2_INTF_RETURN_TYPE bmi2_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr)
|
||||
{
|
||||
uint8_t dev_addr = *(uint8_t*)intf_ptr;
|
||||
|
||||
return coines_write_spi(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len);
|
||||
}
|
||||
|
||||
/*!
|
||||
* Delay function map to COINES platform
|
||||
*/
|
||||
void bmi2_delay_us(uint32_t period, void *intf_ptr)
|
||||
{
|
||||
coines_delay_usec(period);
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Function to select the interface between SPI and I2C.
|
||||
* Also to initialize coines platform
|
||||
*/
|
||||
int8_t bmi2_interface_init(struct bmi2_dev *bmi, uint8_t intf)
|
||||
{
|
||||
int8_t rslt = BMI2_OK;
|
||||
struct coines_board_info board_info;
|
||||
|
||||
if (bmi != NULL)
|
||||
{
|
||||
int16_t result = coines_open_comm_intf(COINES_COMM_INTF_USB);
|
||||
if (result < COINES_SUCCESS)
|
||||
{
|
||||
printf(
|
||||
"\n Unable to connect with Application Board ! \n" " 1. Check if the board is connected and powered on. \n" " 2. Check if Application Board USB driver is installed. \n"
|
||||
" 3. Check if board is in use by another application. (Insufficient permissions to access USB) \n");
|
||||
exit(result);
|
||||
}
|
||||
|
||||
result = coines_get_board_info(&board_info);
|
||||
|
||||
#if defined(PC)
|
||||
setbuf(stdout, NULL);
|
||||
#endif
|
||||
|
||||
if (result == COINES_SUCCESS)
|
||||
{
|
||||
if ((board_info.shuttle_id != BMI2XY_SHUTTLE_ID))
|
||||
{
|
||||
printf("! Warning invalid sensor shuttle \n ," "This application will not support this sensor \n");
|
||||
exit(COINES_E_FAILURE);
|
||||
}
|
||||
}
|
||||
|
||||
coines_set_shuttleboard_vdd_vddio_config(0, 0);
|
||||
coines_delay_msec(100);
|
||||
|
||||
/* Bus configuration : I2C */
|
||||
if (intf == BMI2_I2C_INTF)
|
||||
{
|
||||
printf("I2C Interface \n");
|
||||
|
||||
/* To initialize the user I2C function */
|
||||
dev_addr = BMI2_I2C_PRIM_ADDR;
|
||||
bmi->intf = BMI2_I2C_INTF;
|
||||
bmi->read = bmi2_i2c_read;
|
||||
bmi->write = bmi2_i2c_write;
|
||||
coines_config_i2c_bus(COINES_I2C_BUS_0, COINES_I2C_FAST_MODE);
|
||||
}
|
||||
/* Bus configuration : SPI */
|
||||
else if (intf == BMI2_SPI_INTF)
|
||||
{
|
||||
printf("SPI Interface \n");
|
||||
|
||||
/* To initialize the user SPI function */
|
||||
dev_addr = COINES_SHUTTLE_PIN_7;
|
||||
bmi->intf = BMI2_SPI_INTF;
|
||||
bmi->read = bmi2_spi_read;
|
||||
bmi->write = bmi2_spi_write;
|
||||
coines_config_spi_bus(COINES_SPI_BUS_0, COINES_SPI_SPEED_5_MHZ, COINES_SPI_MODE3);
|
||||
coines_set_pin_config(COINES_SHUTTLE_PIN_7, COINES_PIN_DIRECTION_OUT, COINES_PIN_VALUE_HIGH);
|
||||
}
|
||||
|
||||
/* Assign device address to interface pointer */
|
||||
bmi->intf_ptr = &dev_addr;
|
||||
|
||||
/* Configure delay in microseconds */
|
||||
bmi->delay_us = bmi2_delay_us;
|
||||
|
||||
/* Configure max read/write length (in bytes) ( Supported length depends on target machine) */
|
||||
bmi->read_write_len = READ_WRITE_LEN;
|
||||
|
||||
/* Assign to NULL to load the default config file. */
|
||||
bmi->config_file_ptr = NULL;
|
||||
|
||||
coines_delay_usec(10000);
|
||||
|
||||
coines_set_shuttleboard_vdd_vddio_config(3300, 3300);
|
||||
|
||||
coines_delay_usec(10000);
|
||||
}
|
||||
else
|
||||
{
|
||||
rslt = BMI2_E_NULL_PTR;
|
||||
}
|
||||
|
||||
return rslt;
|
||||
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Prints the execution status of the APIs.
|
||||
*/
|
||||
void bmi2_error_codes_print_result(int8_t rslt)
|
||||
{
|
||||
switch (rslt)
|
||||
{
|
||||
case BMI2_OK:
|
||||
|
||||
/* Do nothing */
|
||||
break;
|
||||
|
||||
case BMI2_W_FIFO_EMPTY:
|
||||
printf("Warning [%d] : FIFO empty\r\n", rslt);
|
||||
break;
|
||||
case BMI2_W_PARTIAL_READ:
|
||||
printf("Warning [%d] : FIFO partial read\r\n", rslt);
|
||||
break;
|
||||
case BMI2_E_NULL_PTR:
|
||||
printf(
|
||||
"Error [%d] : Null pointer error. It occurs when the user tries to assign value (not address) to a pointer," " which has been initialized to NULL.\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_COM_FAIL:
|
||||
printf(
|
||||
"Error [%d] : Communication failure error. It occurs due to read/write operation failure and also due " "to power failure during communication\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_DEV_NOT_FOUND:
|
||||
printf("Error [%d] : Device not found error. It occurs when the device chip id is incorrectly read\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_INVALID_SENSOR:
|
||||
printf(
|
||||
"Error [%d] : Invalid sensor error. It occurs when there is a mismatch in the requested feature with the " "available one\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_SELF_TEST_FAIL:
|
||||
printf(
|
||||
"Error [%d] : Self-test failed error. It occurs when the validation of accel self-test data is " "not satisfied\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_INVALID_INT_PIN:
|
||||
printf(
|
||||
"Error [%d] : Invalid interrupt pin error. It occurs when the user tries to configure interrupt pins " "apart from INT1 and INT2\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_OUT_OF_RANGE:
|
||||
printf(
|
||||
"Error [%d] : Out of range error. It occurs when the data exceeds from filtered or unfiltered data from " "fifo and also when the range exceeds the maximum range for accel and gyro while performing FOC\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_ACC_INVALID_CFG:
|
||||
printf(
|
||||
"Error [%d] : Invalid Accel configuration error. It occurs when there is an error in accel configuration" " register which could be one among range, BW or filter performance in reg address 0x40\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_GYRO_INVALID_CFG:
|
||||
printf(
|
||||
"Error [%d] : Invalid Gyro configuration error. It occurs when there is a error in gyro configuration" "register which could be one among range, BW or filter performance in reg address 0x42\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_ACC_GYR_INVALID_CFG:
|
||||
printf(
|
||||
"Error [%d] : Invalid Accel-Gyro configuration error. It occurs when there is a error in accel and gyro" " configuration registers which could be one among range, BW or filter performance in reg address 0x40 " "and 0x42\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_CONFIG_LOAD:
|
||||
printf(
|
||||
"Error [%d] : Configuration load error. It occurs when failure observed while loading the configuration " "into the sensor\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_INVALID_PAGE:
|
||||
printf(
|
||||
"Error [%d] : Invalid page error. It occurs due to failure in writing the correct feature configuration " "from selected page\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_SET_APS_FAIL:
|
||||
printf(
|
||||
"Error [%d] : APS failure error. It occurs due to failure in write of advance power mode configuration " "register\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_AUX_INVALID_CFG:
|
||||
printf(
|
||||
"Error [%d] : Invalid AUX configuration error. It occurs when the auxiliary interface settings are not " "enabled properly\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_AUX_BUSY:
|
||||
printf(
|
||||
"Error [%d] : AUX busy error. It occurs when the auxiliary interface buses are engaged while configuring" " the AUX\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_REMAP_ERROR:
|
||||
printf(
|
||||
"Error [%d] : Remap error. It occurs due to failure in assigning the remap axes data for all the axes " "after change in axis position\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_GYR_USER_GAIN_UPD_FAIL:
|
||||
printf(
|
||||
"Error [%d] : Gyro user gain update fail error. It occurs when the reading of user gain update status " "fails\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_SELF_TEST_NOT_DONE:
|
||||
printf(
|
||||
"Error [%d] : Self-test not done error. It occurs when the self-test process is ongoing or not " "completed\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_INVALID_INPUT:
|
||||
printf("Error [%d] : Invalid input error. It occurs when the sensor input validity fails\r\n", rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_INVALID_STATUS:
|
||||
printf("Error [%d] : Invalid status error. It occurs when the feature/sensor validity fails\r\n", rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_CRT_ERROR:
|
||||
printf("Error [%d] : CRT error. It occurs when the CRT test has failed\r\n", rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_ST_ALREADY_RUNNING:
|
||||
printf(
|
||||
"Error [%d] : Self-test already running error. It occurs when the self-test is already running and " "another has been initiated\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_CRT_READY_FOR_DL_FAIL_ABORT:
|
||||
printf(
|
||||
"Error [%d] : CRT ready for download fail abort error. It occurs when download in CRT fails due to wrong " "address location\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_DL_ERROR:
|
||||
printf(
|
||||
"Error [%d] : Download error. It occurs when write length exceeds that of the maximum burst length\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_PRECON_ERROR:
|
||||
printf(
|
||||
"Error [%d] : Pre-conditional error. It occurs when precondition to start the feature was not " "completed\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_ABORT_ERROR:
|
||||
printf("Error [%d] : Abort error. It occurs when the device was shaken during CRT test\r\n", rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_WRITE_CYCLE_ONGOING:
|
||||
printf(
|
||||
"Error [%d] : Write cycle ongoing error. It occurs when the write cycle is already running and another " "has been initiated\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_ST_NOT_RUNING:
|
||||
printf(
|
||||
"Error [%d] : Self-test is not running error. It occurs when self-test running is disabled while it's " "running\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_DATA_RDY_INT_FAILED:
|
||||
printf(
|
||||
"Error [%d] : Data ready interrupt error. It occurs when the sample count exceeds the FOC sample limit " "and data ready status is not updated\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
case BMI2_E_INVALID_FOC_POSITION:
|
||||
printf(
|
||||
"Error [%d] : Invalid FOC position error. It occurs when average FOC data is obtained for the wrong" " axes\r\n",
|
||||
rslt);
|
||||
break;
|
||||
|
||||
default:
|
||||
printf("Error [%d] : Unknown error code\r\n", rslt);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Deinitializes coines platform
|
||||
*
|
||||
* @return void.
|
||||
*/
|
||||
void bmi2_coines_deinit(void)
|
||||
{
|
||||
coines_close_comm_intf(COINES_COMM_INTF_USB);
|
||||
}
|
|
@ -0,0 +1,122 @@
|
|||
/**\
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
**/
|
||||
|
||||
#ifndef _COMMON_H
|
||||
#define _COMMON_H
|
||||
|
||||
/*! CPP guard */
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include <stdio.h>
|
||||
#include "bmi2.h"
|
||||
|
||||
/*!
|
||||
* @brief Function for reading the sensor's registers through I2C bus.
|
||||
*
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[out] reg_data : Pointer to the data buffer to store the read data.
|
||||
* @param[in] length : No of bytes to read.
|
||||
* @param[in] intf_ptr : Interface pointer
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval = BMI2_INTF_RET_SUCCESS -> Success
|
||||
* @retval != BMI2_INTF_RET_SUCCESS -> Failure Info
|
||||
*
|
||||
*/
|
||||
BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr);
|
||||
|
||||
/*!
|
||||
* @brief Function for writing the sensor's registers through I2C bus.
|
||||
*
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[in] reg_data : Pointer to the data buffer whose value is to be written.
|
||||
* @param[in] length : No of bytes to write.
|
||||
* @param[in] intf_ptr : Interface pointer
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval = BMI2_INTF_RET_SUCCESS -> Success
|
||||
* @retval != BMI2_INTF_RET_SUCCESS -> Failure Info
|
||||
*
|
||||
*/
|
||||
BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr);
|
||||
|
||||
/*!
|
||||
* @brief Function for reading the sensor's registers through SPI bus.
|
||||
*
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[out] reg_data : Pointer to the data buffer to store the read data.
|
||||
* @param[in] length : No of bytes to read.
|
||||
* @param[in] intf_ptr : Interface pointer
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval = BMI2_INTF_RET_SUCCESS -> Success
|
||||
* @retval != BMI2_INTF_RET_SUCCESS -> Failure Info
|
||||
*
|
||||
*/
|
||||
BMI2_INTF_RETURN_TYPE bmi2_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr);
|
||||
|
||||
/*!
|
||||
* @brief Function for writing the sensor's registers through SPI bus.
|
||||
*
|
||||
* @param[in] reg_addr : Register address.
|
||||
* @param[in] reg_data : Pointer to the data buffer whose data has to be written.
|
||||
* @param[in] length : No of bytes to write.
|
||||
* @param[in] intf_ptr : Interface pointer
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval = BMI2_INTF_RET_SUCCESS -> Success
|
||||
* @retval != BMI2_INTF_RET_SUCCESS -> Failure Info
|
||||
*
|
||||
*/
|
||||
BMI2_INTF_RETURN_TYPE bmi2_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr);
|
||||
|
||||
/*!
|
||||
* @brief This function provides the delay for required time (Microsecond) as per the input provided in some of the
|
||||
* APIs.
|
||||
*
|
||||
* @param[in] period_us : The required wait time in microsecond.
|
||||
* @param[in] intf_ptr : Interface pointer
|
||||
*
|
||||
* @return void.
|
||||
*
|
||||
*/
|
||||
void bmi2_delay_us(uint32_t period, void *intf_ptr);
|
||||
|
||||
/*!
|
||||
* @brief Function to select the interface between SPI and I2C.
|
||||
*
|
||||
* @param[in] bma : Structure instance of bmi2_dev
|
||||
* @param[in] intf : Interface selection parameter
|
||||
*
|
||||
* @return Status of execution
|
||||
* @retval 0 -> Success
|
||||
* @retval < 0 -> Failure Info
|
||||
*/
|
||||
int8_t bmi2_interface_init(struct bmi2_dev *bma, uint8_t intf);
|
||||
|
||||
/*!
|
||||
* @brief Prints the execution status of the APIs.
|
||||
*
|
||||
* @param[in] rslt : Error code returned by the API whose execution status has to be printed.
|
||||
*
|
||||
* @return void.
|
||||
*/
|
||||
void bmi2_error_codes_print_result(int8_t rslt);
|
||||
|
||||
/*!
|
||||
* @brief Deinitializes coines platform
|
||||
*
|
||||
* @return void.
|
||||
*/
|
||||
void bmi2_coines_deinit(void);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif /* End of CPP guard */
|
||||
|
||||
#endif /* _COMMON_H */
|
|
@ -0,0 +1,18 @@
|
|||
COINES_INSTALL_PATH ?= ../../../..
|
||||
|
||||
EXAMPLE_FILE ?= component_retrim.c
|
||||
|
||||
API_LOCATION ?= ../..
|
||||
|
||||
COMMON_LOCATION ?= ..
|
||||
|
||||
C_SRCS += \
|
||||
$(API_LOCATION)/bmi2.c \
|
||||
$(API_LOCATION)/bmi270_hc.c \
|
||||
$(COMMON_LOCATION)/common/common.c
|
||||
|
||||
INCLUDEPATHS += \
|
||||
$(API_LOCATION) \
|
||||
$(COMMON_LOCATION)/common
|
||||
|
||||
include $(COINES_INSTALL_PATH)/coines.mk
|
|
@ -0,0 +1,52 @@
|
|||
/**\
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
**/
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Header Files */
|
||||
#include <stdio.h>
|
||||
#include "bmi270_hc.h"
|
||||
#include "common.h"
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Functions */
|
||||
|
||||
/* This function starts the execution of program. */
|
||||
int main(void)
|
||||
{
|
||||
/* Sensor initialization configuration. */
|
||||
struct bmi2_dev bmi2_dev;
|
||||
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Interface reference is given as a parameter
|
||||
* For I2C : BMI2_I2C_INTF
|
||||
* For SPI : BMI2_SPI_INTF
|
||||
*/
|
||||
rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initialize bmi270_hc. */
|
||||
rslt = bmi270_hc_init(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* This API runs the CRT process. */
|
||||
rslt = bmi2_do_crt(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Do not move the board while doing CRT. If so, it will throw an abort error. */
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
printf("CRT successfully completed.");
|
||||
}
|
||||
}
|
||||
|
||||
bmi2_coines_deinit();
|
||||
|
||||
return rslt;
|
||||
}
|
|
@ -0,0 +1,18 @@
|
|||
COINES_INSTALL_PATH ?= ../../../..
|
||||
|
||||
EXAMPLE_FILE ?= fifo_full_header_mode.c
|
||||
|
||||
API_LOCATION ?= ../..
|
||||
|
||||
COMMON_LOCATION ?= ..
|
||||
|
||||
C_SRCS += \
|
||||
$(API_LOCATION)/bmi2.c \
|
||||
$(API_LOCATION)/bmi270_hc.c \
|
||||
$(COMMON_LOCATION)/common/common.c
|
||||
|
||||
INCLUDEPATHS += \
|
||||
$(API_LOCATION) \
|
||||
$(COMMON_LOCATION)/common
|
||||
|
||||
include $(COINES_INSTALL_PATH)/coines.mk
|
|
@ -0,0 +1,316 @@
|
|||
/**\
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
**/
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Header Files */
|
||||
#include <stdio.h>
|
||||
#include "bmi270_hc.h"
|
||||
#include "common.h"
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Macros */
|
||||
|
||||
/*! Buffer size allocated to store raw FIFO data. */
|
||||
#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048)
|
||||
|
||||
/*! Length of data to be read from FIFO. */
|
||||
#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048)
|
||||
|
||||
/*! Number of accel frames to be extracted from FIFO. */
|
||||
|
||||
/*! Calculation for frame count: Total frame count = Fifo buffer size(2048)/ Total frames(6 Accel, 6 Gyro and 1 header,
|
||||
* totaling to 13) which equals to 157.
|
||||
*/
|
||||
#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(157)
|
||||
|
||||
/*! Number of gyro frames to be extracted from FIFO. */
|
||||
#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(157)
|
||||
|
||||
/*! Macro to read sensortime byte in FIFO. */
|
||||
#define SENSORTIME_OVERHEAD_BYTE UINT8_C(3)
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Static Function Declaration */
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for accel and gyro.
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
* @return Status of execution.
|
||||
*/
|
||||
static int8_t set_accel_gyro_config(struct bmi2_dev *dev);
|
||||
|
||||
/******************************************************************************/
|
||||
|
||||
/* This function starts the execution of program. */
|
||||
int main(void)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
uint16_t index = 0;
|
||||
uint16_t fifo_length = 0;
|
||||
uint16_t config = 0;
|
||||
|
||||
/* To read sensortime, extra 3 bytes are added to fifo buffer. */
|
||||
uint16_t fifo_buffer_size = BMI2_FIFO_RAW_DATA_BUFFER_SIZE + SENSORTIME_OVERHEAD_BYTE;
|
||||
|
||||
/* Variable to get fifo full interrupt status. */
|
||||
uint16_t int_status = 0;
|
||||
|
||||
uint16_t accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT;
|
||||
|
||||
uint16_t gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT;
|
||||
|
||||
/* Variable to store the available frame length count in FIFO */
|
||||
uint8_t frame_count;
|
||||
|
||||
int8_t try = 1;
|
||||
|
||||
/* Number of bytes of FIFO data. */
|
||||
uint8_t fifo_data[fifo_buffer_size];
|
||||
|
||||
/* Sensor initialization configuration. */
|
||||
struct bmi2_dev bmi2_dev;
|
||||
|
||||
/* Array of accelerometer frames -> Total bytes =
|
||||
* 157 * (6 axes + 1 header bytes) = 1099 bytes */
|
||||
struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } };
|
||||
|
||||
/* Array of gyro frames -> Total bytes =
|
||||
* 157 * (6 axes + 1 header bytes) = 1099 bytes */
|
||||
struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } };
|
||||
|
||||
/* Initialize FIFO frame structure. */
|
||||
struct bmi2_fifo_frame fifoframe = { 0 };
|
||||
|
||||
/* Accel and gyro sensor are listed in array. */
|
||||
uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO };
|
||||
|
||||
/* Interface reference is given as a parameter
|
||||
* For I2C : BMI2_I2C_INTF
|
||||
* For SPI : BMI2_SPI_INTF
|
||||
*/
|
||||
rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initialize bmi270_hc. */
|
||||
rslt = bmi270_hc_init(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Enable accel and gyro sensor. */
|
||||
rslt = bmi270_hc_sensor_enable(sensor_sel, 2, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Configuration settings for accel and gyro. */
|
||||
rslt = set_accel_gyro_config(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Before setting FIFO, disable the advance power save mode. */
|
||||
rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initially disable all configurations in fifo. */
|
||||
rslt = bmi2_get_fifo_config(&config, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initially disable all configurations in fifo. */
|
||||
rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Update FIFO structure. */
|
||||
/* Mapping the buffer to store the fifo data. */
|
||||
fifoframe.data = fifo_data;
|
||||
|
||||
/* Length of FIFO frame. */
|
||||
/* To read sensortime, extra 3 bytes are added to fifo user length. */
|
||||
fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH + SENSORTIME_OVERHEAD_BYTE;
|
||||
|
||||
/* Set FIFO configuration by enabling accel, gyro and timestamp.
|
||||
* NOTE 1: The header mode is enabled by default.
|
||||
* NOTE 2: By default the FIFO operating mode is in FIFO mode.
|
||||
* NOTE 3: Sensortime is enabled by default */
|
||||
printf("FIFO is configured in header mode\n");
|
||||
rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Map FIFO full interrupt. */
|
||||
fifoframe.data_int_map = BMI2_FFULL_INT;
|
||||
rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
while (try <= 10)
|
||||
{
|
||||
/* Read FIFO data on interrupt. */
|
||||
rslt = bmi2_get_int_status(&int_status, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if ((rslt == BMI2_OK) && (int_status & BMI2_FFULL_INT_STATUS_MASK))
|
||||
{
|
||||
printf("\nIteration : %d\n", try);
|
||||
|
||||
accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT;
|
||||
|
||||
gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT;
|
||||
|
||||
rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
printf("\nFIFO data bytes available : %d \n", fifo_length);
|
||||
printf("\nFIFO data bytes requested : %d \n", fifoframe.length);
|
||||
|
||||
/* Read FIFO data. */
|
||||
rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Read FIFO data on interrupt. */
|
||||
rslt = bmi2_get_int_status(&int_status, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
printf("\nFIFO accel frames requested : %d \n", accel_frame_length);
|
||||
|
||||
/* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */
|
||||
rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev);
|
||||
printf("\nFIFO accel frames extracted : %d \n", accel_frame_length);
|
||||
|
||||
printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length);
|
||||
|
||||
/* Parse the FIFO data to extract gyro data from the FIFO buffer. */
|
||||
rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev);
|
||||
printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length);
|
||||
|
||||
/* Calculating the frame count from the available bytes in FIFO
|
||||
* frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len + header_byte)) */
|
||||
frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH + 1));
|
||||
printf("\nAvailable frame count from available bytes: %d\n", frame_count);
|
||||
|
||||
printf("\nExtracted accel frames\n");
|
||||
|
||||
if (accel_frame_length > frame_count)
|
||||
{
|
||||
accel_frame_length = frame_count;
|
||||
}
|
||||
|
||||
/* Print the parsed accelerometer data from the FIFO buffer. */
|
||||
for (index = 0; index < accel_frame_length; index++)
|
||||
{
|
||||
printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x,
|
||||
fifo_accel_data[index].y, fifo_accel_data[index].z);
|
||||
}
|
||||
|
||||
printf("\nExtracted gyro frames\n");
|
||||
|
||||
if (gyro_frame_length > frame_count)
|
||||
{
|
||||
gyro_frame_length = frame_count;
|
||||
}
|
||||
|
||||
/* Print the parsed gyro data from the FIFO buffer. */
|
||||
for (index = 0; index < gyro_frame_length; index++)
|
||||
{
|
||||
printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n",
|
||||
index,
|
||||
fifo_gyro_data[index].x,
|
||||
fifo_gyro_data[index].y,
|
||||
fifo_gyro_data[index].z);
|
||||
}
|
||||
|
||||
/* Print control frames like sensor time and skipped frame count. */
|
||||
printf("\nSkipped frame count = %d\n", fifoframe.skipped_frame_count);
|
||||
|
||||
printf("Sensor time = %lu\r\n", fifoframe.sensor_time);
|
||||
|
||||
try++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bmi2_coines_deinit();
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for accel and gyro.
|
||||
*/
|
||||
static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Structure to define accel and gyro configurations. */
|
||||
struct bmi2_sens_config config[2];
|
||||
|
||||
/* Configure the type of feature. */
|
||||
config[0].type = BMI2_ACCEL;
|
||||
config[1].type = BMI2_GYRO;
|
||||
|
||||
/* Get default configurations for the type of feature selected. */
|
||||
rslt = bmi270_hc_get_sensor_config(config, 2, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
|
||||
/* NOTE: The user can change the following configuration parameters according to their requirement. */
|
||||
/* Accel configuration settings. */
|
||||
/* Set Output Data Rate */
|
||||
config[0].cfg.acc.odr = BMI2_ACC_ODR_25HZ;
|
||||
|
||||
/* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */
|
||||
config[0].cfg.acc.range = BMI2_ACC_RANGE_2G;
|
||||
|
||||
/* The bandwidth parameter is used to configure the number of sensor samples that are averaged
|
||||
* if it is set to 2, then 2^(bandwidth parameter) samples
|
||||
* are averaged, resulting in 4 averaged samples
|
||||
* Note1 : For more information, refer the datasheet.
|
||||
* Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but
|
||||
* this has an adverse effect on the power consumed.
|
||||
*/
|
||||
config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4;
|
||||
|
||||
/* Enable the filter performance mode where averaging of samples
|
||||
* will be done based on above set bandwidth and ODR.
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode
|
||||
* 1 -> High performance mode(Default)
|
||||
* For more info refer datasheet.
|
||||
*/
|
||||
config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE;
|
||||
|
||||
/* Gyro configuration settings. */
|
||||
/* Set Output Data Rate */
|
||||
config[1].cfg.gyr.odr = BMI2_GYR_ODR_25HZ;
|
||||
|
||||
/* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */
|
||||
config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000;
|
||||
|
||||
/* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */
|
||||
config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE;
|
||||
|
||||
/* Enable/Disable the noise performance mode for precision yaw rate sensing
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode(Default)
|
||||
* 1 -> High performance mode
|
||||
*/
|
||||
config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE;
|
||||
|
||||
/* Enable/Disable the filter performance mode where averaging of samples
|
||||
* will be done based on above set bandwidth and ODR.
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode
|
||||
* 1 -> High performance mode(Default)
|
||||
*/
|
||||
config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE;
|
||||
|
||||
/* Set new configurations. */
|
||||
rslt = bmi270_hc_set_sensor_config(config, 2, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
}
|
||||
|
||||
return rslt;
|
||||
}
|
|
@ -0,0 +1,18 @@
|
|||
COINES_INSTALL_PATH ?= ../../../..
|
||||
|
||||
EXAMPLE_FILE ?= fifo_full_headerless_mode.c
|
||||
|
||||
API_LOCATION ?= ../..
|
||||
|
||||
COMMON_LOCATION ?= ..
|
||||
|
||||
C_SRCS += \
|
||||
$(API_LOCATION)/bmi2.c \
|
||||
$(API_LOCATION)/bmi270_hc.c \
|
||||
$(COMMON_LOCATION)/common/common.c
|
||||
|
||||
INCLUDEPATHS += \
|
||||
$(API_LOCATION) \
|
||||
$(COMMON_LOCATION)/common
|
||||
|
||||
include $(COINES_INSTALL_PATH)/coines.mk
|
|
@ -0,0 +1,304 @@
|
|||
/**\
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
**/
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Header Files */
|
||||
#include <stdio.h>
|
||||
#include "bmi270_hc.h"
|
||||
#include "common.h"
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Macros */
|
||||
|
||||
/*! Buffer size allocated to store raw FIFO data */
|
||||
#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048)
|
||||
|
||||
/*! Length of data to be read from FIFO */
|
||||
#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048)
|
||||
|
||||
/*! Number of accel frames to be extracted from FIFO */
|
||||
|
||||
/*! Calculation for frame count: Total frame count = Fifo buffer size(2048)/ Total frames(6 Accel, 6 Gyro totaling to
|
||||
* 12) which equals to 170.
|
||||
*/
|
||||
#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(169)
|
||||
|
||||
/*! Number of gyro frames to be extracted from FIFO */
|
||||
#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(169)
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Static Function Declaration */
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for accel and gyro.
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
* @return Status of execution.
|
||||
*/
|
||||
static int8_t set_accel_gyro_config(struct bmi2_dev *dev);
|
||||
|
||||
/******************************************************************************/
|
||||
|
||||
/* This function starts the execution of program. */
|
||||
int main(void)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
uint16_t index = 0;
|
||||
|
||||
uint16_t fifo_length = 0;
|
||||
|
||||
uint8_t try = 1;
|
||||
|
||||
/* Variable to get fifo full interrupt status. */
|
||||
uint16_t int_status = 0;
|
||||
|
||||
uint16_t accel_frame_length;
|
||||
|
||||
uint16_t gyro_frame_length;
|
||||
|
||||
/* Variable to store the available frame length count in FIFO */
|
||||
uint8_t frame_count;
|
||||
|
||||
/* Number of bytes of FIFO data. */
|
||||
uint8_t fifo_data[BMI2_FIFO_RAW_DATA_BUFFER_SIZE] = { 0 };
|
||||
|
||||
/* Sensor initialization configuration. */
|
||||
struct bmi2_dev bmi2_dev;
|
||||
|
||||
/* Array of accelerometer frames -> Total bytes =
|
||||
* 170 * (6 axes bytes) = 1020 bytes */
|
||||
struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } };
|
||||
|
||||
/* Array of gyro frames -> Total bytes =
|
||||
* 170 * (6 axes bytes) = 1020 bytes */
|
||||
struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } };
|
||||
|
||||
/* Initialize FIFO frame structure. */
|
||||
struct bmi2_fifo_frame fifoframe = { 0 };
|
||||
|
||||
/* Accel and gyro sensor are listed in array. */
|
||||
uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO };
|
||||
|
||||
/* Interface reference is given as a parameter
|
||||
* For I2C : BMI2_I2C_INTF
|
||||
* For SPI : BMI2_SPI_INTF
|
||||
*/
|
||||
rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initialize bmi270_hc. */
|
||||
rslt = bmi270_hc_init(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Enable accel and gyro sensor. */
|
||||
rslt = bmi270_hc_sensor_enable(sensor_sel, 2, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Configuration settings for accel and gyro. */
|
||||
rslt = set_accel_gyro_config(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Before setting FIFO, disable the advance power save mode. */
|
||||
rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initially disable all configurations in fifo. */
|
||||
rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Update FIFO structure. */
|
||||
/* Mapping the buffer to store the fifo data. */
|
||||
fifoframe.data = fifo_data;
|
||||
|
||||
/* Length of FIFO frame. */
|
||||
fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH;
|
||||
|
||||
/* Set FIFO configuration by enabling accel, gyro.
|
||||
* NOTE 1: The header mode is enabled by default.
|
||||
* NOTE 2: By default the FIFO operating mode is FIFO mode. */
|
||||
rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
printf("FIFO is configured in headerless mode\n");
|
||||
|
||||
/* To enable headerless mode, disable the header. */
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
rslt = bmi2_set_fifo_config(BMI2_FIFO_HEADER_EN, BMI2_DISABLE, &bmi2_dev);
|
||||
}
|
||||
|
||||
/* Map FIFO full interrupt. */
|
||||
fifoframe.data_int_map = BMI2_FFULL_INT;
|
||||
rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
while (try <= 10)
|
||||
{
|
||||
/* Read FIFO data on interrupt. */
|
||||
rslt = bmi2_get_int_status(&int_status, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if ((rslt == BMI2_OK) && (int_status & BMI2_FFULL_INT_STATUS_MASK))
|
||||
{
|
||||
printf("\nIteration : %d\n", try);
|
||||
|
||||
rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT;
|
||||
gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT;
|
||||
|
||||
printf("\nFIFO data bytes available : %d \n", fifo_length);
|
||||
printf("\nFIFO data bytes requested : %d \n", fifoframe.length);
|
||||
|
||||
/* Read FIFO data. */
|
||||
rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Read FIFO data on interrupt. */
|
||||
rslt = bmi2_get_int_status(&int_status, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
printf("\nFIFO accel frames requested : %d \n", accel_frame_length);
|
||||
|
||||
/* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */
|
||||
rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev);
|
||||
printf("\nFIFO accel frames extracted : %d \n", accel_frame_length);
|
||||
|
||||
printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length);
|
||||
|
||||
/* Parse the FIFO data to extract gyro data from the FIFO buffer. */
|
||||
rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev);
|
||||
printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length);
|
||||
|
||||
/* Calculating the frame count from the available bytes in FIFO
|
||||
* frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len)) */
|
||||
frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH));
|
||||
printf("\nAvailable frame count from available bytes: %d\n", frame_count);
|
||||
|
||||
printf("\nExtracted accel frames\n");
|
||||
|
||||
if (accel_frame_length > frame_count)
|
||||
{
|
||||
accel_frame_length = frame_count;
|
||||
}
|
||||
|
||||
/* Print the parsed accelerometer data from the FIFO buffer. */
|
||||
for (index = 0; index < accel_frame_length; index++)
|
||||
{
|
||||
printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x,
|
||||
fifo_accel_data[index].y, fifo_accel_data[index].z);
|
||||
}
|
||||
|
||||
printf("\nExtracted gyro frames\n");
|
||||
|
||||
if (gyro_frame_length > frame_count)
|
||||
{
|
||||
gyro_frame_length = frame_count;
|
||||
}
|
||||
|
||||
/* Print the parsed gyro data from the FIFO buffer. */
|
||||
for (index = 0; index < gyro_frame_length; index++)
|
||||
{
|
||||
printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n",
|
||||
index,
|
||||
fifo_gyro_data[index].x,
|
||||
fifo_gyro_data[index].y,
|
||||
fifo_gyro_data[index].z);
|
||||
}
|
||||
}
|
||||
|
||||
try++;
|
||||
}
|
||||
}
|
||||
|
||||
bmi2_coines_deinit();
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for accel and gyro.
|
||||
*/
|
||||
static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Structure to define accel and gyro configurations. */
|
||||
struct bmi2_sens_config config[2];
|
||||
|
||||
/* Configure the type of feature. */
|
||||
config[0].type = BMI2_ACCEL;
|
||||
config[1].type = BMI2_GYRO;
|
||||
|
||||
/* Get default configurations for the type of feature selected. */
|
||||
rslt = bmi270_hc_get_sensor_config(config, 2, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* NOTE: The user can change the following configuration parameters according to their requirement. */
|
||||
/* Accel configuration settings. */
|
||||
/* Set Output Data Rate */
|
||||
config[0].cfg.acc.odr = BMI2_ACC_ODR_100HZ;
|
||||
|
||||
/* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */
|
||||
config[0].cfg.acc.range = BMI2_ACC_RANGE_2G;
|
||||
|
||||
/* The bandwidth parameter is used to configure the number of sensor samples that are averaged
|
||||
* if it is set to 2, then 2^(bandwidth parameter) samples
|
||||
* are averaged, resulting in 4 averaged samples
|
||||
* Note1 : For more information, refer the datasheet.
|
||||
* Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but
|
||||
* this has an adverse effect on the power consumed.
|
||||
*/
|
||||
config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4;
|
||||
|
||||
/* Enable the filter performance mode where averaging of samples
|
||||
* will be done based on above set bandwidth and ODR.
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode
|
||||
* 1 -> High performance mode(Default)
|
||||
* For more info refer datasheet.
|
||||
*/
|
||||
config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE;
|
||||
|
||||
/* Gyro configuration settings. */
|
||||
/* Set Output Data Rate */
|
||||
config[1].cfg.gyr.odr = BMI2_GYR_ODR_100HZ;
|
||||
|
||||
/* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */
|
||||
config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000;
|
||||
|
||||
/* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */
|
||||
config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE;
|
||||
|
||||
/* Enable/Disable the noise performance mode for precision yaw rate sensing
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode(Default)
|
||||
* 1 -> High performance mode
|
||||
*/
|
||||
config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE;
|
||||
|
||||
/* Enable/Disable the filter performance mode where averaging of samples
|
||||
* will be done based on above set bandwidth and ODR.
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode
|
||||
* 1 -> High performance mode(Default)
|
||||
*/
|
||||
config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE;
|
||||
|
||||
/* Set new configurations. */
|
||||
rslt = bmi270_hc_set_sensor_config(config, 2, bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
}
|
||||
|
||||
return rslt;
|
||||
}
|
|
@ -0,0 +1,18 @@
|
|||
COINES_INSTALL_PATH ?= ../../../..
|
||||
|
||||
EXAMPLE_FILE ?= fifo_watermark_header_mode.c
|
||||
|
||||
API_LOCATION ?= ../..
|
||||
|
||||
COMMON_LOCATION ?= ..
|
||||
|
||||
C_SRCS += \
|
||||
$(API_LOCATION)/bmi2.c \
|
||||
$(API_LOCATION)/bmi270_hc.c \
|
||||
$(COMMON_LOCATION)/common/common.c
|
||||
|
||||
INCLUDEPATHS += \
|
||||
$(API_LOCATION) \
|
||||
$(COMMON_LOCATION)/common
|
||||
|
||||
include $(COINES_INSTALL_PATH)/coines.mk
|
|
@ -0,0 +1,335 @@
|
|||
/**\
|
||||
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
**/
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Header Files */
|
||||
#include <stdio.h>
|
||||
#include "bmi270_hc.h"
|
||||
#include "common.h"
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Macros */
|
||||
|
||||
/*! Buffer size allocated to store raw FIFO data */
|
||||
#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048)
|
||||
|
||||
/*! Length of data to be read from FIFO */
|
||||
#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048)
|
||||
|
||||
/*! Number of accel frames to be extracted from FIFO */
|
||||
|
||||
/*!
|
||||
* Calculation:
|
||||
* fifo_watermark_level = 650, accel_frame_len = 6, gyro_frame_len = 6 header_byte = 1.
|
||||
* fifo_accel_frame_count = (650 / (6 + 6 + 1 )) = 50 frames
|
||||
* NOTE: Extra frames are read in order to get sensor time
|
||||
*/
|
||||
#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(55)
|
||||
|
||||
/*! Number of gyro frames to be extracted from FIFO */
|
||||
#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(55)
|
||||
|
||||
/*! Setting a watermark level in FIFO */
|
||||
#define BMI270_FIFO_WATERMARK_LEVEL UINT16_C(650)
|
||||
|
||||
/*! Macro to read sensortime byte in FIFO */
|
||||
#define SENSORTIME_OVERHEAD_BYTE UINT8_C(3)
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Static Function Declaration */
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for accel and gyro.
|
||||
* @param[in] dev : Structure instance of bmi2_dev.
|
||||
* @return Status of execution.
|
||||
*/
|
||||
static int8_t set_accel_gyro_config(struct bmi2_dev *dev);
|
||||
|
||||
/******************************************************************************/
|
||||
/*! Functions */
|
||||
|
||||
/* This function starts the execution of program. */
|
||||
int main(void)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Variable to store the available frame length count in FIFO */
|
||||
uint8_t frame_count;
|
||||
|
||||
/* Variable to index bytes. */
|
||||
uint16_t index = 0;
|
||||
|
||||
uint16_t fifo_length = 0;
|
||||
|
||||
uint16_t accel_frame_length;
|
||||
|
||||
uint16_t gyro_frame_length;
|
||||
|
||||
uint8_t try = 1;
|
||||
|
||||
/* To read sensortime, extra 3 bytes are added to fifo buffer */
|
||||
uint16_t fifo_buffer_size = BMI2_FIFO_RAW_DATA_BUFFER_SIZE + SENSORTIME_OVERHEAD_BYTE;
|
||||
|
||||
/* Number of bytes of FIFO data. */
|
||||
uint8_t fifo_data[fifo_buffer_size];
|
||||
|
||||
/* Sensor initialization configuration. */
|
||||
struct bmi2_dev bmi2_dev;
|
||||
|
||||
/* Array of accelerometer frames -> Total bytes =
|
||||
* 55 * (6 axes bytes) = 330 bytes */
|
||||
struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } };
|
||||
|
||||
/* Array of gyro frames -> Total bytes =
|
||||
* 55 * (6 axes bytes) = 330 bytes */
|
||||
struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } };
|
||||
|
||||
/* Initialize FIFO frame structure. */
|
||||
struct bmi2_fifo_frame fifoframe = { 0 };
|
||||
|
||||
/* Accel and gyro sensor are listed in array. */
|
||||
uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO };
|
||||
|
||||
/* Variable to get fifo water-mark interrupt status. */
|
||||
uint16_t int_status = 0;
|
||||
|
||||
uint16_t watermark = 0;
|
||||
|
||||
/* Interface reference is given as a parameter
|
||||
* For I2C : BMI2_I2C_INTF
|
||||
* For SPI : BMI2_SPI_INTF
|
||||
*/
|
||||
rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initialize bmi270_hc. */
|
||||
rslt = bmi270_hc_init(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Enable accel and gyro sensor. */
|
||||
rslt = bmi270_hc_sensor_enable(sensor_sel, 2, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Configuration settings for accel and gyro. */
|
||||
rslt = set_accel_gyro_config(&bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Before setting FIFO, disable the advance power save mode. */
|
||||
rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Initially disable all configurations in fifo. */
|
||||
rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Update FIFO structure. */
|
||||
/* Mapping the buffer to store the fifo data. */
|
||||
fifoframe.data = fifo_data;
|
||||
|
||||
/* Length of FIFO frame. */
|
||||
/* To read sensortime, extra 3 bytes are added to fifo user length. */
|
||||
fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH + SENSORTIME_OVERHEAD_BYTE;
|
||||
|
||||
/* Set FIFO configuration by enabling accel, gyro and timestamp.
|
||||
* NOTE 1: The header mode is enabled by default.
|
||||
* NOTE 2: By default the FIFO operating mode is FIFO mode.
|
||||
* NOTE 3: Sensortime is enabled by default */
|
||||
printf("FIFO is configured in header mode\n");
|
||||
rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* FIFO water-mark interrupt is enabled. */
|
||||
fifoframe.data_int_map = BMI2_FWM_INT;
|
||||
|
||||
/* Map water-mark interrupt to the required interrupt pin. */
|
||||
rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Set water-mark level. */
|
||||
fifoframe.wm_lvl = BMI270_FIFO_WATERMARK_LEVEL;
|
||||
|
||||
/* Set the water-mark level if water-mark interrupt is mapped. */
|
||||
rslt = bmi2_set_fifo_wm(fifoframe.wm_lvl, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
rslt = bmi2_get_fifo_wm(&watermark, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
printf("\nFIFO watermark level is %d\n", watermark);
|
||||
|
||||
while (try <= 10)
|
||||
{
|
||||
/* Read FIFO data on interrupt. */
|
||||
rslt = bmi2_get_int_status(&int_status, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* To check the status of FIFO watermark interrupt. */
|
||||
if ((rslt == BMI2_OK) && (int_status & BMI2_FWM_INT_STATUS_MASK))
|
||||
{
|
||||
printf("\nIteration : %d\n", try);
|
||||
|
||||
printf("\nWatermark interrupt occurred\n");
|
||||
|
||||
rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT;
|
||||
gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT;
|
||||
|
||||
printf("\nFIFO data bytes available : %d \n", fifo_length);
|
||||
printf("\nFIFO data bytes requested : %d \n", fifoframe.length);
|
||||
|
||||
/* Read FIFO data. */
|
||||
rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
/* Read FIFO data on interrupt. */
|
||||
rslt = bmi2_get_int_status(&int_status, &bmi2_dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
printf("\nFIFO accel frames requested : %d \n", accel_frame_length);
|
||||
|
||||
/* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */
|
||||
rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev);
|
||||
printf("\nFIFO accel frames extracted : %d \n", accel_frame_length);
|
||||
|
||||
printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length);
|
||||
|
||||
/* Parse the FIFO data to extract gyro data from the FIFO buffer. */
|
||||
rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev);
|
||||
printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length);
|
||||
|
||||
/* Calculating the frame count from the available bytes in FIFO
|
||||
* frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len + header_byte)) */
|
||||
frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH + 1));
|
||||
printf("\nAvailable frame count from available bytes: %d\n", frame_count);
|
||||
|
||||
printf("\nExracted accel frames\n");
|
||||
|
||||
if (accel_frame_length > frame_count)
|
||||
{
|
||||
accel_frame_length = frame_count;
|
||||
}
|
||||
|
||||
/* Print the parsed accelerometer data from the FIFO buffer. */
|
||||
for (index = 0; index < accel_frame_length; index++)
|
||||
{
|
||||
printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x,
|
||||
fifo_accel_data[index].y, fifo_accel_data[index].z);
|
||||
}
|
||||
|
||||
printf("\nExtracted gyro frames\n");
|
||||
|
||||
if (gyro_frame_length > frame_count)
|
||||
{
|
||||
gyro_frame_length = frame_count;
|
||||
}
|
||||
|
||||
/* Print the parsed gyro data from the FIFO buffer. */
|
||||
for (index = 0; index < gyro_frame_length; index++)
|
||||
{
|
||||
printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n",
|
||||
index,
|
||||
fifo_gyro_data[index].x,
|
||||
fifo_gyro_data[index].y,
|
||||
fifo_gyro_data[index].z);
|
||||
}
|
||||
|
||||
/* Print control frames like sensor time and skipped frame count. */
|
||||
printf("Skipped frame count = %d\n", fifoframe.skipped_frame_count);
|
||||
printf("Sensor time = %lu\r\n", fifoframe.sensor_time);
|
||||
|
||||
try++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bmi2_coines_deinit();
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief This internal API is used to set configurations for accel and gyro.
|
||||
*/
|
||||
static int8_t set_accel_gyro_config(struct bmi2_dev *dev)
|
||||
{
|
||||
/* Status of api are returned to this variable. */
|
||||
int8_t rslt;
|
||||
|
||||
/* Structure to define accel and gyro configurations. */
|
||||
struct bmi2_sens_config config[2];
|
||||
|
||||
/* Configure the type of feature. */
|
||||
config[0].type = BMI2_ACCEL;
|
||||
config[1].type = BMI2_GYRO;
|
||||
|
||||
/* Get default configurations for the type of feature selected. */
|
||||
rslt = bmi270_hc_get_sensor_config(config, 2, dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
|
||||
if (rslt == BMI2_OK)
|
||||
{
|
||||
/* NOTE: The user can change the following configuration parameters according to their requirement. */
|
||||
/* Accel configuration settings. */
|
||||
/* Set Output Data Rate */
|
||||
config[0].cfg.acc.odr = BMI2_ACC_ODR_25HZ;
|
||||
|
||||
/* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */
|
||||
config[0].cfg.acc.range = BMI2_ACC_RANGE_2G;
|
||||
|
||||
/* The bandwidth parameter is used to configure the number of sensor samples that are averaged
|
||||
* if it is set to 2, then 2^(bandwidth parameter) samples
|
||||
* are averaged, resulting in 4 averaged samples
|
||||
* Note1 : For more information, refer the datasheet.
|
||||
* Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but
|
||||
* this has an adverse effect on the power consumed.
|
||||
*/
|
||||
config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4;
|
||||
|
||||
/* Enable the filter performance mode where averaging of samples
|
||||
* will be done based on above set bandwidth and ODR.
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode
|
||||
* 1 -> High performance mode(Default)
|
||||
* For more info refer datasheet.
|
||||
*/
|
||||
config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE;
|
||||
|
||||
/* Gyro configuration settings. */
|
||||
/* Set Output Data Rate */
|
||||
config[1].cfg.gyr.odr = BMI2_GYR_ODR_25HZ;
|
||||
|
||||
/* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */
|
||||
config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000;
|
||||
|
||||
/* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */
|
||||
config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE;
|
||||
|
||||
/* Enable/Disable the noise performance mode for precision yaw rate sensing
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode(Default)
|
||||
* 1 -> High performance mode
|
||||
*/
|
||||
config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE;
|
||||
|
||||
/* Enable/Disable the filter performance mode where averaging of samples
|
||||
* will be done based on above set bandwidth and ODR.
|
||||
* There are two modes
|
||||
* 0 -> Ultra low power mode
|
||||
* 1 -> High performance mode(Default)
|
||||
*/
|
||||
config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE;
|
||||
|
||||
/* Set new configurations. */
|
||||
rslt = bmi270_hc_set_sensor_config(config, 2, dev);
|
||||
bmi2_error_codes_print_result(rslt);
|
||||
}
|
||||
|
||||
return rslt;
|
||||
}
|
|
@ -0,0 +1,18 @@
|
|||
COINES_INSTALL_PATH ?= ../../../..
|
||||
|
||||
EXAMPLE_FILE ?= fifo_watermark_headerless_mode.c
|
||||
|
||||
API_LOCATION ?= ../..
|
||||
|
||||
COMMON_LOCATION ?= ..
|
||||
|
||||
C_SRCS += \
|
||||
$(API_LOCATION)/bmi2.c \
|
||||
$(API_LOCATION)/bmi270_hc.c \
|
||||
$(COMMON_LOCATION)/common/common.c
|
||||
|
||||
INCLUDEPATHS += \
|
||||
$(API_LOCATION) \
|
||||
$(COMMON_LOCATION)/common
|
||||
|
||||
include $(COINES_INSTALL_PATH)/coines.mk
|
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue