From 565ee430c03b9a8940873b1171f21f0263dd9b15 Mon Sep 17 00:00:00 2001 From: Martino Facchin Date: Mon, 20 Oct 2025 16:07:31 +0200 Subject: [PATCH 1/2] feat: increase i2c max burst --- src/BMI270.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/BMI270.cpp b/src/BMI270.cpp index ae13785..dfcc0bf 100644 --- a/src/BMI270.cpp +++ b/src/BMI270.cpp @@ -282,7 +282,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; @@ -308,7 +308,7 @@ 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; } @@ -316,10 +316,7 @@ int8_t BoschSensorClass::bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_dat 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; } From ddcb8a3449e1529f4971aa85338f7d5d860a394b Mon Sep 17 00:00:00 2001 From: Martino Facchin Date: Mon, 20 Oct 2025 16:08:28 +0200 Subject: [PATCH 2/2] feat: properly implement continuous mode (fifo) the previous code was matching Arduino_LSM9DS1 a bit too closely, not taking into account thet the FIFO must be read from different registers in BMI270. --- src/BMI270.cpp | 33 ++++++++++---- src/BoschSensorClass.h | 97 +++++++++++++++++++++++++++++++++++++++++- 2 files changed, 121 insertions(+), 9 deletions(-) diff --git a/src/BMI270.cpp b/src/BMI270.cpp index dfcc0bf..59256f8 100644 --- a/src/BMI270.cpp +++ b/src/BMI270.cpp @@ -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)) @@ -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; @@ -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() { @@ -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; @@ -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() { diff --git a/src/BoschSensorClass.h b/src/BoschSensorClass.h index 5192f10..0af91de 100644 --- a/src/BoschSensorClass.h +++ b/src/BoschSensorClass.h @@ -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); @@ -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;