Skip to content

Commit d94a976

Browse files
facchinmfacchinm
authored
Merge pull request #56 from arduino-libraries/fix_fifo
feat: properly implement continuous mode (fifo)
2 parents 9200ead + ddcb8a3 commit d94a976

File tree

2 files changed

+124
-15
lines changed

2 files changed

+124
-15
lines changed

‎src/BMI270.cpp‎

Lines changed: 28 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -87,13 +87,11 @@ int BoschSensorClass::begin(CfgBoshSensor_t cfg) {
8787

8888

8989
void BoschSensorClass::setContinuousMode() {
90-
bmi2_set_fifo_config(BMI2_FIFO_GYR_EN | BMI2_FIFO_ACC_EN, 1, &bmi2);
91-
continuousMode = true;
90+
continuousMode.begin();
9291
}
9392

9493
void BoschSensorClass::oneShotMode() {
95-
bmi2_set_fifo_config(BMI2_FIFO_GYR_EN | BMI2_FIFO_ACC_EN, 0, &bmi2);
96-
continuousMode = false;
94+
continuousMode.end();
9795
}
9896

9997
// default range is +-4G, so conversion factor is (((1 << 15)/4.0f))
@@ -102,7 +100,14 @@ void BoschSensorClass::oneShotMode() {
102100
// Accelerometer
103101
int BoschSensorClass::readAcceleration(float& x, float& y, float& z) {
104102
struct bmi2_sens_data sensor_data;
105-
auto ret = bmi2_get_sensor_data(&sensor_data, &bmi2);
103+
int ret = 0;
104+
105+
if (continuousMode.hasData(BMI270_ACCELEROMETER)) {
106+
continuousMode.getAccelerometerData(&sensor_data.acc);
107+
} else {
108+
ret = bmi2_get_sensor_data(&sensor_data, &bmi2);
109+
}
110+
106111
#ifdef TARGET_ARDUINO_NANO33BLE
107112
x = -sensor_data.acc.y / INT16_to_G;
108113
y = -sensor_data.acc.x / INT16_to_G;
@@ -116,11 +121,14 @@ int BoschSensorClass::readAcceleration(float& x, float& y, float& z) {
116121

117122
int BoschSensorClass::accelerationAvailable() {
118123
uint16_t status;
124+
if (continuousMode) {
125+
return continuousMode.available(BMI270_ACCELEROMETER);
126+
}
119127
bmi2_get_int_status(&status, &bmi2);
120128
int ret = ((status | _int_status) & BMI2_ACC_DRDY_INT_MASK);
121129
_int_status = status;
122130
_int_status &= ~BMI2_ACC_DRDY_INT_MASK;
123-
return ret;
131+
return ret > 0;
124132
}
125133

126134
float BoschSensorClass::accelerationSampleRate() {
@@ -136,7 +144,13 @@ float BoschSensorClass::accelerationSampleRate() {
136144
// Gyroscope
137145
int BoschSensorClass::readGyroscope(float& x, float& y, float& z) {
138146
struct bmi2_sens_data sensor_data;
139-
auto ret = bmi2_get_sensor_data(&sensor_data, &bmi2);
147+
int ret = 0;
148+
149+
if (continuousMode.hasData(BMI270_GYROSCOPE)) {
150+
continuousMode.getGyroscopeData(&sensor_data.gyr);
151+
} else {
152+
ret = bmi2_get_sensor_data(&sensor_data, &bmi2);
153+
}
140154
#ifdef TARGET_ARDUINO_NANO33BLE
141155
x = -sensor_data.gyr.y / INT16_to_DPS;
142156
y = -sensor_data.gyr.x / INT16_to_DPS;
@@ -150,11 +164,14 @@ int BoschSensorClass::readGyroscope(float& x, float& y, float& z) {
150164

151165
int BoschSensorClass::gyroscopeAvailable() {
152166
uint16_t status;
167+
if (continuousMode) {
168+
return continuousMode.available(BMI270_GYROSCOPE);
169+
}
153170
bmi2_get_int_status(&status, &bmi2);
154171
int ret = ((status | _int_status) & BMI2_GYR_DRDY_INT_MASK);
155172
_int_status = status;
156173
_int_status &= ~BMI2_GYR_DRDY_INT_MASK;
157-
return ret;
174+
return ret > 0;
158175
}
159176

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

283300
int8_t BoschSensorClass::bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr)
284301
{
285-
if ((reg_data == NULL) || (len == 0) || (len > 32)) {
302+
if ((reg_data == NULL) || (len == 0) || (len > 250)) {
286303
return -1;
287304
}
288305
uint8_t bytes_received;
@@ -308,18 +325,15 @@ int8_t BoschSensorClass::bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint
308325

309326
int8_t BoschSensorClass::bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr)
310327
{
311-
if ((reg_data == NULL) || (len == 0) || (len > 32)) {
328+
if ((reg_data == NULL) || (len == 0) || (len > 250)) {
312329
return -1;
313330
}
314331

315332
struct dev_info* dev_info = (struct dev_info*)intf_ptr;
316333
uint8_t dev_id = dev_info->dev_addr;
317334
dev_info->_wire->beginTransmission(dev_id);
318335
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-
}
336+
dev_info->_wire->write(reg_data, (size_t)len);
323337
if (dev_info->_wire->endTransmission() != 0) {
324338
return -1;
325339
}

‎src/BoschSensorClass.h‎

Lines changed: 96 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -28,11 +28,106 @@ typedef enum {
2828
BOSCH_ACCEL_AND_MAGN
2929
} CfgBoshSensor_t;
3030

31+
typedef enum {
32+
BMI270_ACCELEROMETER,
33+
BMI270_GYROSCOPE,
34+
} BoschSensorType_t;
35+
3136
struct dev_info {
3237
TwoWire* _wire;
3338
uint8_t dev_addr;
3439
};
3540

41+
class ContinuousMode {
42+
public:
43+
ContinuousMode(struct bmi2_dev * dev) : bmi2(dev) {}
44+
void begin() {
45+
fifoFrame.data = fifoData;
46+
fifoFrame.length = sizeof(fifoData);
47+
bmi2_set_fifo_config(BMI2_FIFO_GYR_EN | BMI2_FIFO_ACC_EN, 1, bmi2);
48+
bmi2_set_fifo_config(BMI2_FIFO_HEADER_EN, BMI2_DISABLE, bmi2);
49+
bmi2_map_data_int(BMI2_FWM_INT, BMI2_INT1, bmi2);
50+
bmi2_set_fifo_wm((sizeof(fifoData)), bmi2);
51+
continuousMode = true;
52+
}
53+
54+
void end() {
55+
bmi2_set_fifo_config(BMI2_FIFO_GYR_EN | BMI2_FIFO_ACC_EN, 0, bmi2);
56+
continuousMode = false;
57+
}
58+
59+
int available(BoschSensorType_t sensor) {
60+
uint16_t status;
61+
bmi2_get_int_status(&status, bmi2);
62+
if ((status & BMI2_FWM_INT_STATUS_MASK) == 0) {
63+
return 0;
64+
}
65+
switch (sensor) {
66+
case BMI270_ACCELEROMETER:
67+
if (_availableA != 0) {
68+
return 1;
69+
}
70+
break;
71+
case BMI270_GYROSCOPE:
72+
if (_availableG != 0) {
73+
return 1;
74+
}
75+
break;
76+
}
77+
bmi2_get_fifo_length(&status, bmi2);
78+
auto ret = bmi2_read_fifo_data(&fifoFrame, bmi2);
79+
if (ret != 0) {
80+
return 0;
81+
}
82+
_available = min(status, sizeof(fifoData)) / (6 + 6); // 6 bytes per accel sample
83+
_availableG = _available;
84+
_availableA = _available;
85+
ret = bmi2_extract_accel(accel_data, &_available, &fifoFrame, bmi2);
86+
ret = bmi2_extract_gyro(gyro_data, &_available, &fifoFrame, bmi2);
87+
return _available;
88+
}
89+
90+
int hasData(BoschSensorType_t sensor) {
91+
switch (sensor) {
92+
case BMI270_ACCELEROMETER:
93+
return _availableA > 0;
94+
case BMI270_GYROSCOPE:
95+
return _availableG > 0;
96+
}
97+
return 0;
98+
}
99+
100+
void getGyroscopeData(struct bmi2_sens_axes_data* gyr) {
101+
gyr->x = gyro_data[samples_count - 1 - _availableG].x;
102+
gyr->y = gyro_data[samples_count - 1 - _availableG].y;
103+
gyr->z = gyro_data[samples_count - 1 - _availableG].z;
104+
_availableG--;
105+
}
106+
107+
void getAccelerometerData(struct bmi2_sens_axes_data* acc) {
108+
acc->x = accel_data[samples_count - 1 - _availableA].x;
109+
acc->y = accel_data[samples_count - 1 - _availableA].y;
110+
acc->z = accel_data[samples_count - 1 - _availableA].z;
111+
_availableA--;
112+
}
113+
114+
operator bool() const {
115+
return continuousMode == true;
116+
}
117+
118+
private:
119+
bool continuousMode = false;
120+
struct bmi2_dev * bmi2;
121+
static const size_t samples_count = 8;
122+
bmi2_fifo_frame fifoFrame;
123+
uint8_t fifoData[samples_count * (6+6)];
124+
uint16_t _available = 0;
125+
uint16_t _availableG = 0;
126+
uint16_t _availableA = 0;
127+
struct bmi2_sens_axes_data accel_data[samples_count];
128+
struct bmi2_sens_axes_data gyro_data[samples_count];
129+
};
130+
36131
class BoschSensorClass {
37132
public:
38133
BoschSensorClass(TwoWire& wire = Wire);
@@ -96,7 +191,7 @@ class BoschSensorClass {
96191
struct bmm150_dev bmm1;
97192
uint16_t _int_status;
98193
private:
99-
bool continuousMode;
194+
ContinuousMode continuousMode{&bmi2};
100195
};
101196

102197
extern BoschSensorClass IMU_BMI270_BMM150;

0 commit comments

Comments
 (0)