Skip to content

feat: properly implement continuous mode (fifo) #56

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open

feat: properly implement continuous mode (fifo) #56

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
565ee43
feat: increase i2c max burst
facchinm Oct 20, 2025
ddcb8a3
feat: properly implement continuous mode (fifo)
facchinm Oct 20, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
42 changes: 28 additions & 14 deletions src/BMI270.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -87,13 +87,11 @@ int BoschSensorClass::begin(CfgBoshSensor_t cfg) {


void BoschSensorClass::setContinuousMode() {
bmi2_set_fifo_config(BMI2_FIFO_GYR_EN | BMI2_FIFO_ACC_EN, 1, &bmi2);
continuousMode = true;
continuousMode.begin();
}

void BoschSensorClass::oneShotMode() {
bmi2_set_fifo_config(BMI2_FIFO_GYR_EN | BMI2_FIFO_ACC_EN, 0, &bmi2);
continuousMode = false;
continuousMode.end();
}

// default range is +-4G, so conversion factor is (((1 << 15)/4.0f))
Expand All @@ -102,7 +100,14 @@ void BoschSensorClass::oneShotMode() {
// Accelerometer
int BoschSensorClass::readAcceleration(float& x, float& y, float& z) {
struct bmi2_sens_data sensor_data;
auto ret = bmi2_get_sensor_data(&sensor_data, &bmi2);
int ret = 0;

if (continuousMode.hasData(BMI270_ACCELEROMETER)) {
continuousMode.getAccelerometerData(&sensor_data.acc);
} else {
ret = bmi2_get_sensor_data(&sensor_data, &bmi2);
}

#ifdef TARGET_ARDUINO_NANO33BLE
x = -sensor_data.acc.y / INT16_to_G;
y = -sensor_data.acc.x / INT16_to_G;
Expand All @@ -116,11 +121,14 @@ int BoschSensorClass::readAcceleration(float& x, float& y, float& z) {

int BoschSensorClass::accelerationAvailable() {
uint16_t status;
if (continuousMode) {
return continuousMode.available(BMI270_ACCELEROMETER);
}
bmi2_get_int_status(&status, &bmi2);
int ret = ((status | _int_status) & BMI2_ACC_DRDY_INT_MASK);
_int_status = status;
_int_status &= ~BMI2_ACC_DRDY_INT_MASK;
return ret;
return ret > 0;
}

float BoschSensorClass::accelerationSampleRate() {
Expand All @@ -136,7 +144,13 @@ float BoschSensorClass::accelerationSampleRate() {
// Gyroscope
int BoschSensorClass::readGyroscope(float& x, float& y, float& z) {
struct bmi2_sens_data sensor_data;
auto ret = bmi2_get_sensor_data(&sensor_data, &bmi2);
int ret = 0;

if (continuousMode.hasData(BMI270_GYROSCOPE)) {
continuousMode.getGyroscopeData(&sensor_data.gyr);
} else {
ret = bmi2_get_sensor_data(&sensor_data, &bmi2);
}
#ifdef TARGET_ARDUINO_NANO33BLE
x = -sensor_data.gyr.y / INT16_to_DPS;
y = -sensor_data.gyr.x / INT16_to_DPS;
Expand All @@ -150,11 +164,14 @@ int BoschSensorClass::readGyroscope(float& x, float& y, float& z) {

int BoschSensorClass::gyroscopeAvailable() {
uint16_t status;
if (continuousMode) {
return continuousMode.available(BMI270_GYROSCOPE);
}
bmi2_get_int_status(&status, &bmi2);
int ret = ((status | _int_status) & BMI2_GYR_DRDY_INT_MASK);
_int_status = status;
_int_status &= ~BMI2_GYR_DRDY_INT_MASK;
return ret;
return ret > 0;
}

float BoschSensorClass::gyroscopeSampleRate() {
Expand Down Expand Up @@ -282,7 +299,7 @@ int8_t BoschSensorClass::configure_sensor(struct bmm150_dev *dev)

int8_t BoschSensorClass::bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr)
{
if ((reg_data == NULL) || (len == 0) || (len > 32)) {
if ((reg_data == NULL) || (len == 0) || (len > 250)) {
return -1;
}
uint8_t bytes_received;
Expand All @@ -308,18 +325,15 @@ int8_t BoschSensorClass::bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint

int8_t BoschSensorClass::bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr)
{
if ((reg_data == NULL) || (len == 0) || (len > 32)) {
if ((reg_data == NULL) || (len == 0) || (len > 250)) {
return -1;
}

struct dev_info* dev_info = (struct dev_info*)intf_ptr;
uint8_t dev_id = dev_info->dev_addr;
dev_info->_wire->beginTransmission(dev_id);
dev_info->_wire->write(reg_addr);
for (uint16_t i = 0; i < len; i++)
{
dev_info->_wire->write(reg_data[i]);
}
dev_info->_wire->write(reg_data, (size_t)len);
if (dev_info->_wire->endTransmission() != 0) {
return -1;
}
Expand Down
97 changes: 96 additions & 1 deletion src/BoschSensorClass.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,11 +28,106 @@ typedef enum {
BOSCH_ACCEL_AND_MAGN
} CfgBoshSensor_t;

typedef enum {
BMI270_ACCELEROMETER,
BMI270_GYROSCOPE,
} BoschSensorType_t;

struct dev_info {
TwoWire* _wire;
uint8_t dev_addr;
};

class ContinuousMode {
public:
ContinuousMode(struct bmi2_dev * dev) : bmi2(dev) {}
void begin() {
fifoFrame.data = fifoData;
fifoFrame.length = sizeof(fifoData);
bmi2_set_fifo_config(BMI2_FIFO_GYR_EN | BMI2_FIFO_ACC_EN, 1, bmi2);
bmi2_set_fifo_config(BMI2_FIFO_HEADER_EN, BMI2_DISABLE, bmi2);
bmi2_map_data_int(BMI2_FWM_INT, BMI2_INT1, bmi2);
bmi2_set_fifo_wm((sizeof(fifoData)), bmi2);
continuousMode = true;
}

void end() {
bmi2_set_fifo_config(BMI2_FIFO_GYR_EN | BMI2_FIFO_ACC_EN, 0, bmi2);
continuousMode = false;
}

int available(BoschSensorType_t sensor) {
uint16_t status;
bmi2_get_int_status(&status, bmi2);
if ((status & BMI2_FWM_INT_STATUS_MASK) == 0) {
return 0;
}
switch (sensor) {
case BMI270_ACCELEROMETER:
if (_availableA != 0) {
return 1;
}
break;
case BMI270_GYROSCOPE:
if (_availableG != 0) {
return 1;
}
break;
}
bmi2_get_fifo_length(&status, bmi2);
auto ret = bmi2_read_fifo_data(&fifoFrame, bmi2);
if (ret != 0) {
return 0;
}
_available = min(status, sizeof(fifoData)) / (6 + 6); // 6 bytes per accel sample
_availableG = _available;
_availableA = _available;
ret = bmi2_extract_accel(accel_data, &_available, &fifoFrame, bmi2);
ret = bmi2_extract_gyro(gyro_data, &_available, &fifoFrame, bmi2);
return _available;
}

int hasData(BoschSensorType_t sensor) {
switch (sensor) {
case BMI270_ACCELEROMETER:
return _availableA > 0;
case BMI270_GYROSCOPE:
return _availableG > 0;
}
return 0;
}

void getGyroscopeData(struct bmi2_sens_axes_data* gyr) {
gyr->x = gyro_data[samples_count - 1 - _availableG].x;
gyr->y = gyro_data[samples_count - 1 - _availableG].y;
gyr->z = gyro_data[samples_count - 1 - _availableG].z;
_availableG--;
}

void getAccelerometerData(struct bmi2_sens_axes_data* acc) {
acc->x = accel_data[samples_count - 1 - _availableA].x;
acc->y = accel_data[samples_count - 1 - _availableA].y;
acc->z = accel_data[samples_count - 1 - _availableA].z;
_availableA--;
}

operator bool() const {
return continuousMode == true;
}

private:
bool continuousMode = false;
struct bmi2_dev * bmi2;
static const size_t samples_count = 8;
bmi2_fifo_frame fifoFrame;
uint8_t fifoData[samples_count * (6+6)];
uint16_t _available = 0;
uint16_t _availableG = 0;
uint16_t _availableA = 0;
struct bmi2_sens_axes_data accel_data[samples_count];
struct bmi2_sens_axes_data gyro_data[samples_count];
};

class BoschSensorClass {
public:
BoschSensorClass(TwoWire& wire = Wire);
Expand Down Expand Up @@ -96,7 +191,7 @@ class BoschSensorClass {
struct bmm150_dev bmm1;
uint16_t _int_status;
private:
bool continuousMode;
ContinuousMode continuousMode{&bmi2};
};

extern BoschSensorClass IMU_BMI270_BMM150;
Expand Down
Loading