Skip to content

Commit f9eb939

Browse files
LiquidCGSLiquidCGS
committed
Add support for Bosch's BMM150
1 parent 23e35ed commit f9eb939

5 files changed

Lines changed: 318 additions & 2 deletions

File tree

‎datasheets/BMM-150-Datasheet.pdf‎

1.06 MB
Binary file not shown.

‎examples/IMUIdentifier/IMUIdentifier.ino‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -44,8 +44,8 @@ const IMU IMUList[NUM_IMUS] =
4444
{0x0E, 0x0F, 0x01, 0x09, "AK8975", "3M", true},
4545
{0x0C, 0x0D, 0x01, 0x9A, "AK8963", "3M", true},
4646
{0x0E, 0x0F, 0x01, 0x9A, "AK8963", "3M", true},
47-
{0x13, 0x10, 0x40, 0x32, "BMM150", "3M", false},
48-
{0x12, 0x11, 0x40, 0x32, "BMM150", "3M", false},
47+
{0x13, 0x10, 0x40, 0x32, "BMM150", "3M", true},
48+
{0x12, 0x11, 0x40, 0x32, "BMM150", "3M", true},
4949
{0x6B, 0x6A, 0x0F, 0x6B, "LSM6DSR", "3A,3G", false},
5050
{0x6B, 0x6A, 0x0F, 0x6C, "LSM6DSO", "3A,3G", false},
5151
{0x6B, 0x6A, 0x00, 0xFC, "QMI8610", "3A,3G", false},

‎src/F_BMM150.cpp‎

Lines changed: 217 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,217 @@
1+
#include "F_BMM150.hpp"
2+
3+
int BMM150::init(calData cal, uint8_t address)
4+
{
5+
IMUAddress = address;
6+
if (cal.valid == false) {
7+
calibration = { 0 };
8+
calibration.magScale[0] = 1.f;
9+
calibration.magScale[1] = 1.f;
10+
calibration.magScale[2] = 1.f;
11+
} else {
12+
calibration = cal;
13+
}
14+
15+
// Enter suspend mode, then power on
16+
writeByteI2C(wire, IMUAddress, BMM150_PWR, 0x00);
17+
delay(5);
18+
writeByteI2C(wire, IMUAddress, BMM150_PWR, 0x01);
19+
delay(3);
20+
if (readByteI2C(wire, IMUAddress, BMM150_WHOAMI) != BMM150_WHOAMI_VALUE) {
21+
return -1;
22+
}
23+
24+
// Soft reset (bit7=1), then re-enable power
25+
writeByteI2C(wire, IMUAddress, BMM150_PWR, 0x82);
26+
delay(10);
27+
writeByteI2C(wire, IMUAddress, BMM150_PWR, 0x01);
28+
delay(3);
29+
30+
loadTrim();
31+
32+
// Normal mode, 30 Hz ODR
33+
writeByteI2C(wire, IMUAddress, BMM150_CONFIG, 0x38);
34+
35+
// Regular preset repetitions
36+
writeByteI2C(wire, IMUAddress, BMM150_REPXY, 0x04);
37+
writeByteI2C(wire, IMUAddress, BMM150_REPZ, 0x07);
38+
39+
// Enable all axes (DIS_X/Y/Z bits 3:5 = 0)
40+
writeByteI2C(wire, IMUAddress, BMM150_INT1, 0x00);
41+
return 0;
42+
}
43+
44+
void BMM150::loadTrim()
45+
{
46+
uint8_t buf2[2], buf4[4], buf10[10];
47+
48+
readBytesI2C(wire, IMUAddress, BMM150_TRIM_X1Y1, 2, buf2);
49+
readBytesI2C(wire, IMUAddress, BMM150_TRIM_Z4, 4, buf4);
50+
readBytesI2C(wire, IMUAddress, BMM150_TRIM_Z2, 10, buf10);
51+
52+
trim.digX1 = (int8_t)buf2[0];
53+
trim.digY1 = (int8_t)buf2[1];
54+
trim.digZ4 = (int16_t)(((uint16_t)buf4[1] << 8) | buf4[0]);
55+
trim.digX2 = (int8_t)buf4[2];
56+
trim.digY2 = (int8_t)buf4[3];
57+
trim.digZ2 = (int16_t)(((uint16_t)buf10[1] << 8) | buf10[0]);
58+
trim.digZ1 = (uint16_t)(((uint16_t)buf10[3] << 8) | buf10[2]);
59+
trim.digXYZ1 = (uint16_t)((((uint16_t)buf10[5] & 0x7F) << 8) | buf10[4]);
60+
trim.digZ3 = (int16_t)(((uint16_t)buf10[7] << 8) | buf10[6]);
61+
trim.digXY2 = (int8_t)buf10[8];
62+
trim.digXY1 = buf10[9];
63+
}
64+
65+
float BMM150::compensateX(int16_t raw, uint16_t rhall)
66+
{
67+
if (raw == BMM150_OVF_XY || rhall == 0 || trim.digXYZ1 == 0)
68+
return 0.f;
69+
float c0 = ((float)trim.digXYZ1) * 16384.f / rhall;
70+
float r = c0 - 16384.f;
71+
float c1 = (float)trim.digXY2 * (r * r / 268435456.f);
72+
float c2 = c1 + r * (float)trim.digXY1 / 16384.f;
73+
float c3 = (float)trim.digX2 + 160.f;
74+
float c4 = raw * ((c2 + 256.f) * c3);
75+
return (c4 / 8192.f + (float)trim.digX1 * 8.f) / 16.f;
76+
}
77+
78+
float BMM150::compensateY(int16_t raw, uint16_t rhall)
79+
{
80+
if (raw == BMM150_OVF_XY || rhall == 0 || trim.digXYZ1 == 0)
81+
return 0.f;
82+
float c0 = ((float)trim.digXYZ1) * 16384.f / rhall;
83+
float r = c0 - 16384.f;
84+
float c1 = (float)trim.digXY2 * (r * r / 268435456.f);
85+
float c2 = c1 + r * (float)trim.digXY1 / 16384.f;
86+
float c3 = (float)trim.digY2 + 160.f;
87+
float c4 = raw * ((c2 + 256.f) * c3);
88+
return (c4 / 8192.f + (float)trim.digY1 * 8.f) / 16.f;
89+
}
90+
91+
float BMM150::compensateZ(int16_t raw, uint16_t rhall)
92+
{
93+
if (raw == BMM150_OVF_Z || trim.digZ2 == 0 || trim.digZ1 == 0 || rhall == 0 || trim.digXYZ1 == 0)
94+
return 0.f;
95+
float z0 = (float)raw - (float)trim.digZ4;
96+
float z1 = (float)rhall - (float)trim.digXYZ1;
97+
float z2 = (float)trim.digZ3 * z1;
98+
float z3 = (float)trim.digZ1 * (float)rhall / 32768.f;
99+
float z4 = (float)trim.digZ2 + z3;
100+
return ((z0 * 131072.f - z2) / (z4 * 4.f)) / 16.f;
101+
}
102+
103+
void BMM150::update()
104+
{
105+
uint8_t rawData[8] = { 0 };
106+
readBytesI2C(wire, IMUAddress, BMM150_DATAX, 8, &rawData[0]);
107+
108+
if (!(rawData[6] & 0x01)) return; // data ready bit
109+
110+
// X and Y: 13-bit signed (sign-extend from bit 12)
111+
int16_t rawX = (int16_t)(((int16_t)(int8_t)rawData[1]) * 32) | (rawData[0] >> 3);
112+
int16_t rawY = (int16_t)(((int16_t)(int8_t)rawData[3]) * 32) | (rawData[2] >> 3);
113+
// Z: 15-bit signed (sign-extend from bit 14)
114+
int16_t rawZ = (int16_t)(((int16_t)(int8_t)rawData[5]) * 128) | (rawData[4] >> 1);
115+
// RHALL: 14-bit unsigned
116+
uint16_t rhall = (uint16_t)(((uint16_t)rawData[7] << 6) | (rawData[6] >> 2));
117+
118+
if (rhall == 0) return;
119+
120+
float mx = (compensateX(rawX, rhall) - calibration.magBias[0]) * calibration.magScale[0];
121+
float my = (compensateY(rawY, rhall) - calibration.magBias[1]) * calibration.magScale[1];
122+
float mz = (compensateZ(rawZ, rhall) - calibration.magBias[2]) * calibration.magScale[2];
123+
124+
switch (geometryIndex) {
125+
case 0:
126+
mag.magX = mx;
127+
mag.magY = my;
128+
mag.magZ = mz;
129+
break;
130+
case 1:
131+
mag.magX = -my;
132+
mag.magY = mx;
133+
mag.magZ = mz;
134+
break;
135+
case 2:
136+
mag.magX = mx;
137+
mag.magY = my;
138+
mag.magZ = mz;
139+
break;
140+
case 3:
141+
mag.magX = my;
142+
mag.magY = -mx;
143+
mag.magZ = mz;
144+
break;
145+
case 4:
146+
mag.magX = -mz;
147+
mag.magY = -my;
148+
mag.magZ = -mx;
149+
break;
150+
case 5:
151+
mag.magX = -mz;
152+
mag.magY = mx;
153+
mag.magZ = -my;
154+
break;
155+
case 6:
156+
mag.magX = -mz;
157+
mag.magY = my;
158+
mag.magZ = mx;
159+
break;
160+
case 7:
161+
mag.magX = -mz;
162+
mag.magY = -mx;
163+
mag.magZ = my;
164+
break;
165+
}
166+
}
167+
168+
void BMM150::getMag(MagData* out)
169+
{
170+
memcpy(out, &mag, sizeof(mag));
171+
}
172+
173+
void BMM150::calibrateMag(calData* cal)
174+
{
175+
float mag_max[3] = { -1e9f, -1e9f, -1e9f };
176+
float mag_min[3] = { 1e9f, 1e9f, 1e9f };
177+
178+
// ~15 seconds at 30 Hz
179+
for (uint16_t ii = 0; ii < 450; ii++) {
180+
uint8_t rawData[8] = { 0 };
181+
readBytesI2C(wire, IMUAddress, BMM150_DATAX, 8, &rawData[0]);
182+
if (rawData[6] & 0x01) {
183+
int16_t rawX = (int16_t)(((int16_t)(int8_t)rawData[1]) * 32) | (rawData[0] >> 3);
184+
int16_t rawY = (int16_t)(((int16_t)(int8_t)rawData[3]) * 32) | (rawData[2] >> 3);
185+
int16_t rawZ = (int16_t)(((int16_t)(int8_t)rawData[5]) * 128) | (rawData[4] >> 1);
186+
uint16_t rhall = (uint16_t)(((uint16_t)rawData[7] << 6) | (rawData[6] >> 2));
187+
if (rhall == 0) { delay(35); continue; }
188+
189+
float v[3];
190+
v[0] = compensateX(rawX, rhall);
191+
v[1] = compensateY(rawY, rhall);
192+
v[2] = compensateZ(rawZ, rhall);
193+
for (int j = 0; j < 3; j++) {
194+
if (v[j] > mag_max[j]) mag_max[j] = v[j];
195+
if (v[j] < mag_min[j]) mag_min[j] = v[j];
196+
}
197+
}
198+
delay(35);
199+
}
200+
201+
cal->magBias[0] = (mag_max[0] + mag_min[0]) / 2.f;
202+
cal->magBias[1] = (mag_max[1] + mag_min[1]) / 2.f;
203+
cal->magBias[2] = (mag_max[2] + mag_min[2]) / 2.f;
204+
205+
float half[3] = {
206+
(mag_max[0] - mag_min[0]) / 2.f,
207+
(mag_max[1] - mag_min[1]) / 2.f,
208+
(mag_max[2] - mag_min[2]) / 2.f
209+
};
210+
float avg_rad = (half[0] + half[1] + half[2]) / 3.f;
211+
212+
cal->magScale[0] = avg_rad / half[0];
213+
cal->magScale[1] = avg_rad / half[1];
214+
cal->magScale[2] = avg_rad / half[2];
215+
216+
cal->valid = true;
217+
}

‎src/F_BMM150.hpp‎

Lines changed: 98 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,98 @@
1+
#pragma once
2+
3+
#ifndef _F_BMM150_H_
4+
#define _F_BMM150_H_
5+
6+
#include "IMUBase.hpp"
7+
#include "IMUUtils.hpp"
8+
9+
/*
10+
BMM150 REGISTERS
11+
*/
12+
#define BMM150_WHOAMI 0x40
13+
#define BMM150_DATAX 0x42
14+
#define BMM150_CTRL 0x4A
15+
#define BMM150_PWR 0x4B
16+
#define BMM150_CONFIG 0x4C
17+
#define BMM150_INT2 0x4D
18+
#define BMM150_INT1 0x4E
19+
#define BMM150_LOWT 0x4F
20+
#define BMM150_HIGHT 0x50
21+
#define BMM150_REPXY 0x51
22+
#define BMM150_REPZ 0x52
23+
24+
/* Trim register base addresses */
25+
#define BMM150_TRIM_X1Y1 0x5D
26+
#define BMM150_TRIM_Z4 0x62
27+
#define BMM150_TRIM_Z2 0x68
28+
29+
#define BMM150_WHOAMI_VALUE 0x32
30+
31+
/* Overflow sentinels in raw ADC counts */
32+
#define BMM150_OVF_XY ((int16_t)-4096)
33+
#define BMM150_OVF_Z ((int16_t)-16384)
34+
35+
struct BMM150TrimData {
36+
int8_t digX1, digY1;
37+
int8_t digX2, digY2;
38+
int8_t digXY2;
39+
uint8_t digXY1;
40+
uint16_t digZ1, digXYZ1;
41+
int16_t digZ2, digZ3, digZ4;
42+
};
43+
44+
class BMM150 : public IMUBase {
45+
public:
46+
explicit BMM150(TwoWire& wire = Wire) : wire(wire) {};
47+
48+
// Inherited via IMUBase
49+
int init(calData cal, uint8_t address) override;
50+
51+
void update() override;
52+
void getAccel(AccelData* out) override {};
53+
void getGyro(GyroData* out) override {};
54+
void getMag(MagData* out) override;
55+
void getQuat(Quaternion* out) override {};
56+
float getTemp() override { return 0.f; };
57+
58+
void calibrateAccelGyro(calData* cal) override {};
59+
void calibrateMag(calData* cal) override;
60+
int setGyroRange(int range) override { return -1; };
61+
int setAccelRange(int range) override { return -1; };
62+
int setIMUGeometry(int index) override { geometryIndex = index; return 0; };
63+
64+
bool hasMagnetometer() override {
65+
return true;
66+
}
67+
bool hasTemperature() override {
68+
return false;
69+
}
70+
bool hasQuatOutput() override {
71+
return false;
72+
}
73+
74+
String IMUName() override {
75+
return "BMM-150";
76+
}
77+
String IMUType() override {
78+
return "BMM150";
79+
}
80+
String IMUManufacturer() override {
81+
return "Bosch";
82+
}
83+
84+
private:
85+
int geometryIndex = 0;
86+
MagData mag = { 0 };
87+
BMM150TrimData trim = {};
88+
89+
calData calibration;
90+
uint8_t IMUAddress;
91+
TwoWire& wire;
92+
93+
void loadTrim();
94+
float compensateX(int16_t raw, uint16_t rhall);
95+
float compensateY(int16_t raw, uint16_t rhall);
96+
float compensateZ(int16_t raw, uint16_t rhall);
97+
};
98+
#endif /* _F_BMM150_H_ */

‎src/FastIMU.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@
1111
#include "F_ICM20689.hpp"
1212
#include "F_ICM20690.hpp"
1313
#include "F_BMI055.hpp"
14+
#include "F_BMM150.hpp"
1415
#include "F_BMX055.hpp"
1516
#include "F_BMI160.hpp"
1617
#include "F_LSM6DS3.hpp"

0 commit comments

Comments
 (0)