Skip to content

Commit 6a96bba

Browse files
LiquidCGSLiquidCGS
committed
Add ODR stuff
1 parent 3a37da8 commit 6a96bba

45 files changed

Lines changed: 784 additions & 146 deletions

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

‎src/IMUBase.hpp‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -59,6 +59,14 @@ class IMUBase {
5959
virtual int setAccelRange(int range) = 0;
6060
virtual int setIMUGeometry(int index) = 0;
6161

62+
virtual int setAccelODR(int odr_hz) { return -1; }
63+
virtual int setGyroODR(int odr_hz) { return -1; }
64+
virtual int getAccelODR() { return -1; }
65+
virtual int getGyroODR() { return -1; }
66+
67+
virtual int setMagODR(int odr_hz) { return -1; }
68+
virtual int getMagODR() { return -1; }
69+
6270
virtual void calibrateAccelGyro(calData* cal) = 0;
6371
virtual void calibrateMag(calData* cal) = 0;
6472

@@ -81,6 +89,12 @@ class IMUBase {
8189
virtual String IMUManufacturer(){
8290
return "Unknown";
8391
}
92+
93+
static int nearestHigherODR(const int* table, int count, int requested) {
94+
for (int i = 0; i < count; i++)
95+
if (table[i] >= requested) return table[i];
96+
return table[count - 1];
97+
}
8498
};
8599

86100
#endif /* _F_IMUBase_H_ */

‎src/sensors/F_AK09918.cpp‎

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -180,3 +180,25 @@ void AK09918::calibrateMag(calData* cal)
180180
cal->magScale[1] = avg_rad / ((float)mag_scale[1]);
181181
cal->magScale[2] = avg_rad / ((float)mag_scale[2]);
182182
}
183+
184+
// AK09918 continuous mode ODR options: 10, 20, 50, 100 Hz
185+
static const int AK09918_ODR_TABLE[] = {10, 20, 50, 100};
186+
static const uint8_t AK09918_ODR_MODE[] = {
187+
AK09918_CONTINUOUS_10HZ,
188+
AK09918_CONTINUOUS_20HZ,
189+
AK09918_CONTINUOUS_50HZ,
190+
AK09918_CONTINUOUS_100HZ
191+
};
192+
193+
int AK09918::setMagODR(int odr_hz) {
194+
if (odr_hz <= 0) return -1;
195+
int actual = nearestHigherODR(AK09918_ODR_TABLE, 4, odr_hz);
196+
int idx = 0;
197+
while (AK09918_ODR_TABLE[idx] != actual) idx++;
198+
writeByteI2C(wire, AK09918_ADDRESS, AK09918_CNTL2, AK09918_POWER_DOWN);
199+
delay(10);
200+
writeByteI2C(wire, AK09918_ADDRESS, AK09918_CNTL2, AK09918_ODR_MODE[idx]);
201+
delay(10);
202+
currentMagODR = actual;
203+
return actual;
204+
}

‎src/sensors/F_AK09918.hpp‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -76,6 +76,9 @@ class AK09918 : public IMUBase {
7676
int setAccelRange(int range) override;
7777
int setIMUGeometry(int index) override { geometryIndex = index; return 0; };
7878

79+
int setMagODR(int odr_hz) override;
80+
int getMagODR() override { return currentMagODR; }
81+
7982
void calibrateAccelGyro(calData* cal) override;
8083
void calibrateMag(calData* cal) override;
8184

@@ -103,6 +106,7 @@ class AK09918 : public IMUBase {
103106
}
104107
private:
105108
int geometryIndex = 0;
109+
int currentMagODR = 0;
106110
float temperature = 0.f;
107111
MagData mag = { 0 };
108112

‎src/sensors/F_AK8963.cpp‎

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -184,4 +184,21 @@ void AK8963::calibrateMag(calData* cal)
184184
cal->magScale[0] = avg_rad / ((float)mag_scale[0]);
185185
cal->magScale[1] = avg_rad / ((float)mag_scale[1]);
186186
cal->magScale[2] = avg_rad / ((float)mag_scale[2]);
187+
}
188+
189+
// AK8963 supports 8 Hz (mode 0x02) and 100 Hz (mode 0x06) continuous modes.
190+
static const int AK8963_ODR_TABLE[] = {8, 100};
191+
static const uint8_t AK8963_ODR_MODE[] = {0x02, 0x06};
192+
193+
int AK8963::setMagODR(int odr_hz) {
194+
if (odr_hz <= 0) return -1;
195+
int actual = nearestHigherODR(AK8963_ODR_TABLE, 2, odr_hz);
196+
int idx = 0;
197+
while (AK8963_ODR_TABLE[idx] != actual) idx++;
198+
writeByteI2C(wire, AK8963_ADDRESS, AK8963_CNTL, 0x00); // power down before mode change
199+
delay(10);
200+
writeByteI2C(wire, AK8963_ADDRESS, AK8963_CNTL, (uint8_t)(1 << 4) | AK8963_ODR_MODE[idx]);
201+
delay(10);
202+
currentMagODR = actual;
203+
return actual;
187204
}

‎src/sensors/F_AK8963.hpp‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -51,6 +51,9 @@ class AK8963 : public IMUBase {
5151
int setAccelRange(int range) override;
5252
int setIMUGeometry(int index) override { geometryIndex = index; return 0; };
5353

54+
int setMagODR(int odr_hz) override;
55+
int getMagODR() override { return currentMagODR; }
56+
5457
void calibrateAccelGyro(calData* cal) override;
5558
void calibrateMag(calData* cal) override;
5659

@@ -77,6 +80,7 @@ class AK8963 : public IMUBase {
7780
float mRes = 10. * 4912. / 32760.0; //mres value for full range (4912uT) readings
7881

7982
int geometryIndex = 0;
83+
int currentMagODR = 100;
8084
float temperature = 0.f;
8185
MagData mag = { 0 };
8286

‎src/sensors/F_AK8975.hpp‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -50,6 +50,10 @@ class AK8975 : public IMUBase {
5050
int setAccelRange(int range) override;
5151
int setIMUGeometry(int index) override { geometryIndex = index; return 0; };
5252

53+
// AK8975 is single-shot only
54+
int setMagODR(int odr_hz) override { return -1; }
55+
int getMagODR() override { return -1; }
56+
5357
void calibrateAccelGyro(calData* cal) override;
5458
void calibrateMag(calData* cal) override;
5559

‎src/sensors/F_BMI055.cpp‎

Lines changed: 56 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -61,38 +61,39 @@ int BMI055::init(calData cal, uint8_t address)
6161

6262
void BMI055::update()
6363
{
64-
int16_t AccelCount[3]; // used to read all 6 bytes at once from the BMI055 accel
65-
int16_t GyroCount[3]; // used to read all 6 bytes at once from the BMI055 gyro
66-
uint8_t rawDataAccel[7]; // x/y/z accel register data stored here
67-
uint8_t rawDataGyro[6];
68-
69-
readBytesI2C(wire, AccelAddress, BMI055_ACCD_X_LSB, 7, &rawDataAccel[0]); // Read the 7 raw accelerometer data registers into data array
70-
accel.timestamp = micros();
71-
72-
readBytesI2C(wire, GyroAddress, BMI055_GYR_RATE_X_LSB , 6, &rawDataGyro[0]); // Read the 6 raw gyroscope data registers into data array
73-
gyro.timestamp = micros();
64+
bool accelReady = readByteI2C(wire, AccelAddress, BMI055_ACCD_X_LSB) & 0x01;
65+
bool gyroReady = readByteI2C(wire, GyroAddress, BMI055_GYR_INT_STATUS_0) & 0x80;
66+
if (!accelReady && !gyroReady) return;
7467

75-
//accel registers
76-
AccelCount[0] = ((rawDataAccel[1] << 8) | (rawDataAccel[0] & 0xF0)) >> 4; // Turn the MSB and LSB into a signed 12-bit value
77-
AccelCount[1] = ((rawDataAccel[3] << 8) | (rawDataAccel[2] & 0xF0)) >> 4; // praise sign extension, making this code clean and simple.
78-
AccelCount[2] = ((rawDataAccel[5] << 8) | (rawDataAccel[4] & 0xF0)) >> 4;
68+
int16_t AccelCount[3] = {0};
69+
int16_t GyroCount[3] = {0};
70+
uint8_t rawDataAccel[7];
71+
uint8_t rawDataGyro[6];
7972

80-
//gyro registers
81-
GyroCount[0] = (rawDataGyro[1] << 8) | (rawDataGyro[0]);
82-
GyroCount[1] = (rawDataGyro[3] << 8) | (rawDataGyro[2]);
83-
GyroCount[2] = (rawDataGyro[5] << 8) | (rawDataGyro[4]);
73+
if (accelReady) {
74+
readBytesI2C(wire, AccelAddress, BMI055_ACCD_X_LSB, 7, &rawDataAccel[0]);
75+
accel.timestamp = micros();
76+
AccelCount[0] = ((rawDataAccel[1] << 8) | (rawDataAccel[0] & 0xF0)) >> 4;
77+
AccelCount[1] = ((rawDataAccel[3] << 8) | (rawDataAccel[2] & 0xF0)) >> 4;
78+
AccelCount[2] = ((rawDataAccel[5] << 8) | (rawDataAccel[4] & 0xF0)) >> 4;
79+
temperature = -((rawDataAccel[6] * -0.5f) * (86.5f - -40.5f) / (float)(128.f) - 40.5f) - 20.f;
80+
}
8481

85-
float ax, ay, az, gx, gy, gz;
82+
if (gyroReady) {
83+
readBytesI2C(wire, GyroAddress, BMI055_GYR_RATE_X_LSB, 6, &rawDataGyro[0]);
84+
gyro.timestamp = micros();
85+
GyroCount[0] = (rawDataGyro[1] << 8) | rawDataGyro[0];
86+
GyroCount[1] = (rawDataGyro[3] << 8) | rawDataGyro[2];
87+
GyroCount[2] = (rawDataGyro[5] << 8) | rawDataGyro[4];
88+
}
8689

87-
// Calculate the accel value into actual g's per second
88-
ax = AccelCount[0] * (float)aRes - calibration.accelBias[0];
89-
ay = AccelCount[1] * (float)aRes - calibration.accelBias[1];
90-
az = AccelCount[2] * (float)aRes - calibration.accelBias[2];
90+
float ax = AccelCount[0] * (float)aRes - calibration.accelBias[0];
91+
float ay = AccelCount[1] * (float)aRes - calibration.accelBias[1];
92+
float az = AccelCount[2] * (float)aRes - calibration.accelBias[2];
9193

92-
// Calculate the gyro value into actual degrees per second
93-
gx = GyroCount[0] * (float)gRes - calibration.gyroBias[0];
94-
gy = GyroCount[1] * (float)gRes - calibration.gyroBias[1];
95-
gz = GyroCount[2] * (float)gRes - calibration.gyroBias[2];
94+
float gx = GyroCount[0] * (float)gRes - calibration.gyroBias[0];
95+
float gy = GyroCount[1] * (float)gRes - calibration.gyroBias[1];
96+
float gz = GyroCount[2] * (float)gRes - calibration.gyroBias[2];
9697

9798
switch (geometryIndex) {
9899
case 0:
@@ -136,8 +137,6 @@ void BMI055::update()
136137
accel.accelZ = ay; gyro.gyroZ = gy;
137138
break;
138139
}
139-
// Calculate the temperature value into actual deg c
140-
temperature = -((rawDataAccel[6] * -0.5f) * (86.5f - -40.5f) / (float)(128.f) - 40.5f) - 20.f;
141140
}
142141

143142
void BMI055::getAccel(AccelData* out)
@@ -306,4 +305,32 @@ void BMI055::calibrateAccelGyro(calData* cal)
306305
cal->gyroBias[1] = (float)gyro_bias[1];
307306
cal->gyroBias[2] = (float)gyro_bias[2];
308307
cal->valid = true;
308+
}
309+
310+
// Accel ODR = 2 × bandwidth. PMU_BW values 0x08-0x0F give BW 7.81-1000 Hz.
311+
static const int BMI055_ACCEL_ODR_TABLE[] = {15, 31, 62, 125, 250, 500, 1000, 2000};
312+
static const uint8_t BMI055_ACCEL_BW_REG[] = {0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F};
313+
314+
// Gyro: distinct ODR values with highest available bandwidth for each.
315+
static const int BMI055_GYRO_ODR_TABLE[] = {100, 200, 400, 1000};
316+
static const uint8_t BMI055_GYRO_BW_REG[] = {0x07, 0x06, 0x00, 0x01};
317+
318+
int BMI055::setAccelODR(int odr_hz) {
319+
if (odr_hz <= 0) return -1;
320+
int actual = nearestHigherODR(BMI055_ACCEL_ODR_TABLE, 8, odr_hz);
321+
int idx = 0;
322+
while (BMI055_ACCEL_ODR_TABLE[idx] != actual) idx++;
323+
writeByteI2C(wire, AccelAddress, BMI055_PMU_BW, BMI055_ACCEL_BW_REG[idx]);
324+
currentAccelODR = actual;
325+
return actual;
326+
}
327+
328+
int BMI055::setGyroODR(int odr_hz) {
329+
if (odr_hz <= 0) return -1;
330+
int actual = nearestHigherODR(BMI055_GYRO_ODR_TABLE, 4, odr_hz);
331+
int idx = 0;
332+
while (BMI055_GYRO_ODR_TABLE[idx] != actual) idx++;
333+
writeByteI2C(wire, GyroAddress, BMI055_GYR_BW, BMI055_GYRO_BW_REG[idx]);
334+
currentGyroODR = actual;
335+
return actual;
309336
}

‎src/sensors/F_BMI055.hpp‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -34,6 +34,7 @@
3434
#define BMI055_GYR_RATE_Y_MSB 0x05
3535
#define BMI055_GYR_RATE_Z_LSB 0x06
3636
#define BMI055_GYR_RATE_Z_MSB 0x07
37+
#define BMI055_GYR_INT_STATUS_0 0x09
3738
#define BMI055_GYR_RANGE 0x0F
3839
#define BMI055_GYR_BW 0x10
3940
#define BMI055_GYR_LPM1 0x11
@@ -58,6 +59,11 @@ class BMI055 : public IMUBase {
5859
int setAccelRange(int range) override;
5960
int setIMUGeometry(int index) override { geometryIndex = index; return 0; };
6061

62+
int setAccelODR(int odr_hz) override;
63+
int setGyroODR(int odr_hz) override;
64+
int getAccelODR() override { return currentAccelODR; }
65+
int getGyroODR() override { return currentGyroODR; }
66+
6167
void calibrateAccelGyro(calData* cal) override;
6268
void calibrateMag(calData* cal) override {};
6369

@@ -92,6 +98,9 @@ class BMI055 : public IMUBase {
9298

9399
calData calibration;
94100

101+
int currentAccelODR = 125;
102+
int currentGyroODR = 400;
103+
95104
uint8_t AccelAddress;
96105
uint8_t GyroAddress;
97106

‎src/sensors/F_BMI160.cpp‎

Lines changed: 35 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -42,13 +42,19 @@ int BMI160::init(calData cal, uint8_t address)
4242
}
4343

4444
void BMI160::update() {
45+
uint8_t status = readByteI2C(wire, IMUAddress, BMI160_STATUS);
46+
bool accelReady = status & 0x80;
47+
bool gyroReady = status & 0x40;
48+
if (!accelReady && !gyroReady) return;
49+
4550
int16_t IMUCount[6]; // used to read all 16 bytes at once from the accel/gyro
4651
uint8_t rawData[12]; // x/y/z accel register data stored here
4752

4853
readBytesI2C(wire, IMUAddress, BMI160_GYR_X_L, 12, &rawData[0]); // Read the 12 raw data registers into data array
4954

50-
accel.timestamp = micros();
51-
gyro.timestamp = accel.timestamp;
55+
uint32_t now = micros();
56+
if (accelReady) accel.timestamp = now;
57+
if (gyroReady) gyro.timestamp = now;
5258

5359
IMUCount[0] = ((int16_t)rawData[1] << 8) | rawData[0]; // Turn the MSB and LSB into a signed 16-bit value
5460
IMUCount[1] = ((int16_t)rawData[3] << 8) | rawData[2];
@@ -280,3 +286,30 @@ void BMI160::calibrateAccelGyro(calData* cal)
280286
cal->gyroBias[2] = (float)gyro_bias[2];
281287
cal->valid = true;
282288
}
289+
290+
static const int BMI160_ODR_TABLE[] = {12, 25, 50, 100, 200, 400, 800, 1600};
291+
static const uint8_t BMI160_ODR_REG[] = {0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, 0x0C};
292+
293+
int BMI160::setAccelODR(int odr_hz) {
294+
if (odr_hz <= 0) return -1;
295+
int actual = nearestHigherODR(BMI160_ODR_TABLE, 8, odr_hz);
296+
int idx = 0;
297+
while (BMI160_ODR_TABLE[idx] != actual) idx++;
298+
uint8_t conf = readByteI2C(wire, IMUAddress, BMI160_ACC_CONF);
299+
conf = (conf & 0xF0) | BMI160_ODR_REG[idx];
300+
writeByteI2C(wire, IMUAddress, BMI160_ACC_CONF, conf);
301+
currentAccelODR = actual;
302+
return actual;
303+
}
304+
305+
int BMI160::setGyroODR(int odr_hz) {
306+
if (odr_hz <= 0) return -1;
307+
int actual = nearestHigherODR(BMI160_ODR_TABLE, 8, odr_hz);
308+
int idx = 0;
309+
while (BMI160_ODR_TABLE[idx] != actual) idx++;
310+
uint8_t conf = readByteI2C(wire, IMUAddress, BMI160_GYR_CONF);
311+
conf = (conf & 0xF0) | BMI160_ODR_REG[idx];
312+
writeByteI2C(wire, IMUAddress, BMI160_GYR_CONF, conf);
313+
currentGyroODR = actual;
314+
return actual;
315+
}

‎src/sensors/F_BMI160.hpp‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -121,6 +121,11 @@ class BMI160 : public IMUBase {
121121
int setAccelRange(int range) override;
122122
int setIMUGeometry(int index) override { geometryIndex = index; return 0; };
123123

124+
int setAccelODR(int odr_hz) override;
125+
int setGyroODR(int odr_hz) override;
126+
int getAccelODR() override { return currentAccelODR; }
127+
int getGyroODR() override { return currentGyroODR; }
128+
124129
void calibrateAccelGyro(calData* cal) override;
125130
virtual void calibrateMag(calData* cal) override {};
126131

@@ -149,6 +154,9 @@ class BMI160 : public IMUBase {
149154
float gRes = 2000.0 / 32768.0; //gres value for full range (2000dps) readings
150155
int geometryIndex = 0;
151156

157+
int currentAccelODR = 400;
158+
int currentGyroODR = 400;
159+
152160
float temperature = 0.f;
153161
AccelData accel = { 0 };
154162
GyroData gyro = { 0 };

0 commit comments

Comments
 (0)