Skip to content

Commit 3374bb3

Browse files
facchinmfacchinm
committed
properly implement continuous mode
1 parent 3e7e139 commit 3374bb3

File tree

2 files changed

+106
-66
lines changed

2 files changed

+106
-66
lines changed

‎src/BMI270.cpp‎

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

8888

8989
void BoschSensorClass::setContinuousMode() {
90-
continuousModeFifoFrame.data = continuousModeFifoData;
91-
continuousModeFifoFrame.length = sizeof(continuousModeFifoData);
92-
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
96-
continuousMode = true;
90+
continuousMode.begin();
9791
}
9892

9993
void BoschSensorClass::oneShotMode() {
100-
bmi2_set_fifo_config(BMI2_FIFO_GYR_EN | BMI2_FIFO_ACC_EN, 0, &bmi2);
101-
continuousMode = false;
94+
continuousMode.end();
10295
}
10396

10497
// default range is +-4G, so conversion factor is (((1 << 15)/4.0f))
@@ -109,11 +102,8 @@ int BoschSensorClass::readAcceleration(float& x, float& y, float& z) {
109102
struct bmi2_sens_data sensor_data;
110103
int ret = 0;
111104

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--;
105+
if (continuousMode.hasData(BMI270_ACCELEROMETER)) {
106+
continuousMode.getAccelerometerData(&sensor_data.acc);
117107
} else {
118108
ret = bmi2_get_sensor_data(&sensor_data, &bmi2);
119109
}
@@ -131,25 +121,8 @@ int BoschSensorClass::readAcceleration(float& x, float& y, float& z) {
131121

132122
int BoschSensorClass::accelerationAvailable() {
133123
uint16_t status;
134-
if (continuousModeAvailableA > 0) {
135-
return continuousModeAvailableA;
136-
}
137124
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;
125+
return continuousMode.available(BMI270_ACCELEROMETER);
153126
}
154127
bmi2_get_int_status(&status, &bmi2);
155128
int ret = ((status | _int_status) & BMI2_ACC_DRDY_INT_MASK);
@@ -173,11 +146,8 @@ int BoschSensorClass::readGyroscope(float& x, float& y, float& z) {
173146
struct bmi2_sens_data sensor_data;
174147
int ret = 0;
175148

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--;
149+
if (continuousMode.hasData(BMI270_GYROSCOPE)) {
150+
continuousMode.getGyroscopeData(&sensor_data.gyr);
181151
} else {
182152
ret = bmi2_get_sensor_data(&sensor_data, &bmi2);
183153
}
@@ -194,26 +164,8 @@ int BoschSensorClass::readGyroscope(float& x, float& y, float& z) {
194164

195165
int BoschSensorClass::gyroscopeAvailable() {
196166
uint16_t status;
197-
if (continuousModeAvailableG > 0) {
198-
return continuousModeAvailableG;
199-
}
200167
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;
168+
return continuousMode.available(BMI270_GYROSCOPE);
217169
}
218170
bmi2_get_int_status(&status, &bmi2);
219171
int ret = ((status | _int_status) & BMI2_GYR_DRDY_INT_MASK);
@@ -347,7 +299,7 @@ int8_t BoschSensorClass::configure_sensor(struct bmm150_dev *dev)
347299

348300
int8_t BoschSensorClass::bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr)
349301
{
350-
if ((reg_data == NULL) || (len == 0) || (len > 128)) {
302+
if ((reg_data == NULL) || (len == 0) || (len > 250)) {
351303
return -1;
352304
}
353305
uint8_t bytes_received;
@@ -373,7 +325,7 @@ int8_t BoschSensorClass::bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint
373325

374326
int8_t BoschSensorClass::bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr)
375327
{
376-
if ((reg_data == NULL) || (len == 0) || (len > 128)) {
328+
if ((reg_data == NULL) || (len == 0) || (len > 250)) {
377329
return -1;
378330
}
379331

‎src/BoschSensorClass.h‎

Lines changed: 96 additions & 8 deletions
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,14 +191,7 @@ class BoschSensorClass {
96191
struct bmm150_dev bmm1;
97192
uint16_t _int_status;
98193
private:
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];
194+
ContinuousMode continuousMode{&bmi2};
107195
};
108196

109197
extern BoschSensorClass IMU_BMI270_BMM150;

0 commit comments

Comments
 (0)