Skip to content

Commit 3e7e139

Browse files
facchinmfacchinm
committed
wip: make FIFO actually usable
1 parent d3a735b commit 3e7e139

File tree

2 files changed

+80
-11
lines changed

2 files changed

+80
-11
lines changed

‎src/BMI270.cpp‎

Lines changed: 72 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -87,7 +87,12 @@ int BoschSensorClass::begin(CfgBoshSensor_t cfg) {
8787

8888

8989
void BoschSensorClass::setContinuousMode() {
90+
continuousModeFifoFrame.data = continuousModeFifoData;
91+
continuousModeFifoFrame.length = sizeof(continuousModeFifoData);
9092
bmi2_set_fifo_config(BMI2_FIFO_GYR_EN | BMI2_FIFO_ACC_EN, 1, &bmi2);
93+
bmi2_set_fifo_config(BMI2_FIFO_HEADER_EN, BMI2_DISABLE, &bmi2);
94+
bmi2_map_data_int(BMI2_FWM_INT, BMI2_INT1, &bmi2);
95+
bmi2_set_fifo_wm((sizeof(continuousModeFifoData)), &bmi2); // 32 samples of accel+gyro, match LSM9DS1
9196
continuousMode = true;
9297
}
9398

@@ -102,7 +107,17 @@ void BoschSensorClass::oneShotMode() {
102107
// Accelerometer
103108
int BoschSensorClass::readAcceleration(float& x, float& y, float& z) {
104109
struct bmi2_sens_data sensor_data;
105-
auto ret = bmi2_get_sensor_data(&sensor_data, &bmi2);
110+
int ret = 0;
111+
112+
if (continuousModeAvailableA > 0) {
113+
sensor_data.acc.x = fifo_accel_data[8 - continuousModeAvailableA].x;
114+
sensor_data.acc.y = fifo_accel_data[8 - continuousModeAvailableA].y;
115+
sensor_data.acc.z = fifo_accel_data[8 - continuousModeAvailableA].z;
116+
continuousModeAvailableA--;
117+
} else {
118+
ret = bmi2_get_sensor_data(&sensor_data, &bmi2);
119+
}
120+
106121
#ifdef TARGET_ARDUINO_NANO33BLE
107122
x = -sensor_data.acc.y / INT16_to_G;
108123
y = -sensor_data.acc.x / INT16_to_G;
@@ -116,11 +131,31 @@ int BoschSensorClass::readAcceleration(float& x, float& y, float& z) {
116131

117132
int BoschSensorClass::accelerationAvailable() {
118133
uint16_t status;
134+
if (continuousModeAvailableA > 0) {
135+
return continuousModeAvailableA;
136+
}
137+
if (continuousMode) {
138+
bmi2_get_int_status(&status, &bmi2);
139+
if ((status & BMI2_FWM_INT_STATUS_MASK) == 0) {
140+
return 0;
141+
}
142+
bmi2_get_fifo_length(&status, &bmi2);
143+
auto ret = bmi2_read_fifo_data(&continuousModeFifoFrame, &bmi2);
144+
if (ret != 0) {
145+
return 0;
146+
}
147+
continuousModeAvailable = min(status, sizeof(continuousModeFifoData)) / (6 + 6); // 6 bytes per accel sample
148+
continuousModeAvailableG = continuousModeAvailable;
149+
continuousModeAvailableA = continuousModeAvailable;
150+
ret = bmi2_extract_accel(fifo_accel_data, &continuousModeAvailable, &continuousModeFifoFrame, &bmi2);
151+
ret = bmi2_extract_gyro(fifo_gyro_data, &continuousModeAvailable, &continuousModeFifoFrame, &bmi2);
152+
return continuousModeAvailable;
153+
}
119154
bmi2_get_int_status(&status, &bmi2);
120155
int ret = ((status | _int_status) & BMI2_ACC_DRDY_INT_MASK);
121156
_int_status = status;
122157
_int_status &= ~BMI2_ACC_DRDY_INT_MASK;
123-
return ret;
158+
return ret > 0;
124159
}
125160

126161
float BoschSensorClass::accelerationSampleRate() {
@@ -136,7 +171,16 @@ float BoschSensorClass::accelerationSampleRate() {
136171
// Gyroscope
137172
int BoschSensorClass::readGyroscope(float& x, float& y, float& z) {
138173
struct bmi2_sens_data sensor_data;
139-
auto ret = bmi2_get_sensor_data(&sensor_data, &bmi2);
174+
int ret = 0;
175+
176+
if (continuousModeAvailableG > 0) {
177+
sensor_data.gyr.x = fifo_gyro_data[8 - continuousModeAvailableG].x;
178+
sensor_data.gyr.y = fifo_gyro_data[8 - continuousModeAvailableG].y;
179+
sensor_data.gyr.z = fifo_gyro_data[8 - continuousModeAvailableG].z;
180+
continuousModeAvailableG--;
181+
} else {
182+
ret = bmi2_get_sensor_data(&sensor_data, &bmi2);
183+
}
140184
#ifdef TARGET_ARDUINO_NANO33BLE
141185
x = -sensor_data.gyr.y / INT16_to_DPS;
142186
y = -sensor_data.gyr.x / INT16_to_DPS;
@@ -150,11 +194,32 @@ int BoschSensorClass::readGyroscope(float& x, float& y, float& z) {
150194

151195
int BoschSensorClass::gyroscopeAvailable() {
152196
uint16_t status;
197+
if (continuousModeAvailableG > 0) {
198+
return continuousModeAvailableG;
199+
}
200+
if (continuousMode) {
201+
bmi2_get_int_status(&status, &bmi2);
202+
if ((status & BMI2_FWM_INT_STATUS_MASK) == 0) {
203+
return 0;
204+
}
205+
bmi2_get_fifo_length(&status, &bmi2);
206+
auto ret = bmi2_read_fifo_data(&continuousModeFifoFrame, &bmi2);
207+
if (ret != 0) {
208+
return 0;
209+
}
210+
continuousModeAvailable = min(status, sizeof(continuousModeFifoData)) / (6 + 6); // 6 bytes per accel sample
211+
continuousModeAvailableG = continuousModeAvailable;
212+
continuousModeAvailableA = continuousModeAvailable;
213+
ret = bmi2_extract_accel(fifo_accel_data, &continuousModeAvailable, &continuousModeFifoFrame, &bmi2);
214+
ret = bmi2_extract_gyro(fifo_gyro_data, &continuousModeAvailable, &continuousModeFifoFrame, &bmi2);
215+
216+
return continuousModeAvailable;
217+
}
153218
bmi2_get_int_status(&status, &bmi2);
154219
int ret = ((status | _int_status) & BMI2_GYR_DRDY_INT_MASK);
155220
_int_status = status;
156221
_int_status &= ~BMI2_GYR_DRDY_INT_MASK;
157-
return ret;
222+
return ret > 0;
158223
}
159224

160225
float BoschSensorClass::gyroscopeSampleRate() {
@@ -282,7 +347,7 @@ int8_t BoschSensorClass::configure_sensor(struct bmm150_dev *dev)
282347

283348
int8_t BoschSensorClass::bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr)
284349
{
285-
if ((reg_data == NULL) || (len == 0) || (len > 32)) {
350+
if ((reg_data == NULL) || (len == 0) || (len > 128)) {
286351
return -1;
287352
}
288353
uint8_t bytes_received;
@@ -308,18 +373,15 @@ int8_t BoschSensorClass::bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint
308373

309374
int8_t BoschSensorClass::bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr)
310375
{
311-
if ((reg_data == NULL) || (len == 0) || (len > 32)) {
376+
if ((reg_data == NULL) || (len == 0) || (len > 128)) {
312377
return -1;
313378
}
314379

315380
struct dev_info* dev_info = (struct dev_info*)intf_ptr;
316381
uint8_t dev_id = dev_info->dev_addr;
317382
dev_info->_wire->beginTransmission(dev_id);
318383
dev_info->_wire->write(reg_addr);
319-
for (uint16_t i = 0; i < len; i++)
320-
{
321-
dev_info->_wire->write(reg_data[i]);
322-
}
384+
dev_info->_wire->write(reg_data, (size_t)len);
323385
if (dev_info->_wire->endTransmission() != 0) {
324386
return -1;
325387
}

‎src/BoschSensorClass.h‎

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -96,7 +96,14 @@ class BoschSensorClass {
9696
struct bmm150_dev bmm1;
9797
uint16_t _int_status;
9898
private:
99-
bool continuousMode;
99+
bool continuousMode = false;
100+
bmi2_fifo_frame continuousModeFifoFrame;
101+
uint8_t continuousModeFifoData[8 * (6+6)]; // 8 samples of accel+gyro, match LSM9DS1
102+
uint16_t continuousModeAvailable = 0;
103+
uint16_t continuousModeAvailableG = 0;
104+
uint16_t continuousModeAvailableA = 0;
105+
struct bmi2_sens_axes_data fifo_accel_data[8];
106+
struct bmi2_sens_axes_data fifo_gyro_data[8];
100107
};
101108

102109
extern BoschSensorClass IMU_BMI270_BMM150;

0 commit comments

Comments
 (0)