Zumo32U4 library
Zumo32U4IMU.cpp
1#include <Wire.h>
2#include <Zumo32U4IMU.h>
3
4#define TEST_REG_ERROR -1
5
6#define LSM303D_WHO_ID 0x49
7#define L3GD20H_WHO_ID 0xD7
8#define LSM6DS33_WHO_ID 0x69
9#define LIS3MDL_WHO_ID 0x3D
10
11bool Zumo32U4IMU::init()
12{
13 if (testReg(LSM303D_ADDR, LSM303D_REG_WHO_AM_I) == LSM303D_WHO_ID &&
14 testReg(L3GD20H_ADDR, L3GD20H_REG_WHO_AM_I) == L3GD20H_WHO_ID)
15 {
16 type = Zumo32U4IMUType::LSM303D_L3GD20H;
17 return true;
18 }
19 else if (testReg(LSM6DS33_ADDR, LSM6DS33_REG_WHO_AM_I) == LSM6DS33_WHO_ID &&
20 testReg( LIS3MDL_ADDR, LIS3MDL_REG_WHO_AM_I) == LIS3MDL_WHO_ID)
21 {
22 type = Zumo32U4IMUType::LSM6DS33_LIS3MDL;
23 return true;
24 }
25 else
26 {
27 return false;
28 }
29}
30
31void Zumo32U4IMU::enableDefault()
32{
33 switch (type)
34 {
35 case Zumo32U4IMUType::LSM303D_L3GD20H:
36
37 // Accelerometer
38
39 // 0x57 = 0b01010111
40 // AODR = 0101 (50 Hz ODR); AZEN = AYEN = AXEN = 1 (all axes enabled)
41 writeReg(LSM303D_ADDR, LSM303D_REG_CTRL1, 0x57);
42 if (lastError) { return; }
43
44 // 0x00 = 0b00000000
45 // AFS = 0 (+/- 2 g full scale)
46 writeReg(LSM303D_ADDR, LSM303D_REG_CTRL2, 0x00);
47 if (lastError) { return; }
48
49 // Magnetometer
50
51 // 0x64 = 0b01100100
52 // M_RES = 11 (high resolution mode); M_ODR = 001 (6.25 Hz ODR)
53 writeReg(LSM303D_ADDR, LSM303D_REG_CTRL5, 0x64);
54 if (lastError) { return; }
55
56 // 0x20 = 0b00100000
57 // MFS = 01 (+/- 4 gauss full scale)
58 writeReg(LSM303D_ADDR, LSM303D_REG_CTRL6, 0x20);
59 if (lastError) { return; }
60
61 // 0x00 = 0b00000000
62 // MD = 00 (continuous-conversion mode)
63 writeReg(LSM303D_ADDR, LSM303D_REG_CTRL7, 0x00);
64 if (lastError) { return; }
65
66 // Gyro
67
68 // 0x7F = 0b01111111
69 // DR = 01 (189.4 Hz ODR); BW = 11 (70 Hz bandwidth); PD = 1 (normal mode); Zen = Yen = Xen = 1 (all axes enabled)
70 writeReg(L3GD20H_ADDR, L3GD20H_REG_CTRL1, 0x7F);
71 if (lastError) { return; }
72
73 // 0x00 = 0b00000000
74 // FS = 00 (+/- 245 dps full scale)
75 writeReg(L3GD20H_ADDR, L3GD20H_REG_CTRL4, 0x00);
76 return;
77
78 case Zumo32U4IMUType::LSM6DS33_LIS3MDL:
79
80 // Accelerometer
81
82 // 0x30 = 0b00110000
83 // ODR = 0011 (52 Hz (high performance)); FS_XL = 00 (+/- 2 g full scale)
84 writeReg(LSM6DS33_ADDR, LSM6DS33_REG_CTRL1_XL, 0x30);
85 if (lastError) { return; }
86
87 // Gyro
88
89 // 0x50 = 0b01010000
90 // ODR = 0101 (208 Hz (high performance)); FS_G = 00 (+/- 245 dps full scale)
91 writeReg(LSM6DS33_ADDR, LSM6DS33_REG_CTRL2_G, 0x50);
92 if (lastError) { return; }
93
94 // Accelerometer + Gyro
95
96 // 0x04 = 0b00000100
97 // IF_INC = 1 (automatically increment register address)
98 writeReg(LSM6DS33_ADDR, LSM6DS33_REG_CTRL3_C, 0x04);
99 if (lastError) { return; }
100
101 // Magnetometer
102
103 // 0x70 = 0b01110000
104 // OM = 11 (ultra-high-performance mode for X and Y); DO = 100 (10 Hz ODR)
105 writeReg(LIS3MDL_ADDR, LIS3MDL_REG_CTRL_REG1, 0x70);
106 if (lastError) { return; }
107
108 // 0x00 = 0b00000000
109 // FS = 00 (+/- 4 gauss full scale)
110 writeReg(LIS3MDL_ADDR, LIS3MDL_REG_CTRL_REG2, 0x00);
111 if (lastError) { return; }
112
113 // 0x00 = 0b00000000
114 // MD = 00 (continuous-conversion mode)
115 writeReg(LIS3MDL_ADDR, LIS3MDL_REG_CTRL_REG3, 0x00);
116 if (lastError) { return; }
117
118 // 0x0C = 0b00001100
119 // OMZ = 11 (ultra-high-performance mode for Z)
120 writeReg(LIS3MDL_ADDR, LIS3MDL_REG_CTRL_REG4, 0x0C);
121 return;
122 }
123}
124
125void Zumo32U4IMU::configureForBalancing()
126{
127 switch (type)
128 {
129 case Zumo32U4IMUType::LSM303D_L3GD20H:
130
131 // Accelerometer
132
133 // 0x18 = 0b00011000
134 // AFS = 0 (+/- 2 g full scale)
135 writeReg(LSM303D_ADDR, LSM303D_REG_CTRL2, 0x18);
136 if (lastError) { return; }
137
138 // Gyro
139
140 // 0xFF = 0b11111111
141 // DR = 11 (757.6 Hz ODR); BW = 11 (100 Hz bandwidth); PD = 1 (normal mode); Zen = Yen = Xen = 1 (all axes enabled)
142 writeReg(L3GD20H_ADDR, L3GD20H_REG_CTRL1, 0xFF);
143 if (lastError) { return; }
144
145 // 0x20 = 0b00100000
146 // FS = 10 (+/- 2000 dps full scale)
147 writeReg(L3GD20H_ADDR, L3GD20H_REG_CTRL4, 0x20);
148 return;
149
150 case Zumo32U4IMUType::LSM6DS33_LIS3MDL:
151
152 // Accelerometer
153
154 // 0x3C = 0b00111100
155 // ODR = 0011 (52 Hz (high performance)); FS_XL = 11 (+/- 8 g full scale)
156 writeReg(LSM6DS33_ADDR, LSM6DS33_REG_CTRL1_XL, 0x3C);
157 if (lastError) { return; }
158
159 // Gyro
160
161 // 0x7C = 0b01111100
162 // ODR = 0111 (833 Hz (high performance)); FS_G = 11 (+/- 2000 dps full scale)
163 writeReg(LSM6DS33_ADDR, LSM6DS33_REG_CTRL2_G, 0x7C);
164 return;
165 }
166}
167
168void Zumo32U4IMU::configureForTurnSensing()
169{
170 switch (type)
171 {
172 case Zumo32U4IMUType::LSM303D_L3GD20H:
173
174 // Gyro
175
176 // 0xFF = 0b11111111
177 // DR = 11 (757.6 Hz ODR); BW = 11 (100 Hz bandwidth); PD = 1 (normal mode); Zen = Yen = Xen = 1 (all axes enabled)
178 writeReg(L3GD20H_ADDR, L3GD20H_REG_CTRL1, 0xFF);
179 if (lastError) { return; }
180
181 // 0x20 = 0b00100000
182 // FS = 10 (+/- 2000 dps full scale)
183 writeReg(L3GD20H_ADDR, L3GD20H_REG_CTRL4, 0x20);
184 return;
185
186 case Zumo32U4IMUType::LSM6DS33_LIS3MDL:
187
188 // Gyro
189
190 // 0x7C = 0b01111100
191 // ODR = 0111 (833 Hz (high performance)); FS_G = 11 (+/- 2000 dps full scale)
192 writeReg(LSM6DS33_ADDR, LSM6DS33_REG_CTRL2_G, 0x7C);
193 return;
194 }
195}
196
197void Zumo32U4IMU::configureForFaceUphill()
198{
199 switch (type)
200 {
201 case Zumo32U4IMUType::LSM303D_L3GD20H:
202
203 // Accelerometer
204
205 // 0x37 = 0b00110111
206 // AODR = 0011 (12.5 Hz ODR); AZEN = AYEN = AXEN = 1 (all axes enabled)
207 writeReg(LSM303D_ADDR, LSM303D_REG_CTRL1, 0x37);
208 return;
209
210 case Zumo32U4IMUType::LSM6DS33_LIS3MDL:
211
212 // Accelerometer
213
214 // 0x10 = 0b00010000
215 // ODR = 0001 (13 Hz (high performance)); FS_XL = 00 (+/- 2 g full scale)
216 writeReg(LSM6DS33_ADDR, LSM6DS33_REG_CTRL1_XL, 0x10);
217 return;
218 }
219}
220
221void Zumo32U4IMU::writeReg(uint8_t addr, uint8_t reg, uint8_t value)
222{
223 Wire.beginTransmission(addr);
224 Wire.write(reg);
225 Wire.write(value);
226 lastError = Wire.endTransmission();
227}
228
229uint8_t Zumo32U4IMU::readReg(uint8_t addr, uint8_t reg)
230{
231 Wire.beginTransmission(addr);
232 Wire.write(reg);
233 lastError = Wire.endTransmission();
234 if (lastError) { return 0; }
235
236 uint8_t byteCount = Wire.requestFrom(addr, (uint8_t)1);
237 if (byteCount != 1)
238 {
239 lastError = 50;
240 return 0;
241 }
242 return Wire.read();
243}
244
245// Reads the 3 accelerometer channels and stores them in vector a
246void Zumo32U4IMU::readAcc(void)
247{
248 switch (type)
249 {
250 case Zumo32U4IMUType::LSM303D_L3GD20H:
251 // set MSB of register address for auto-increment
252 readAxes16Bit(LSM303D_ADDR, LSM303D_REG_OUT_X_L_A | (1 << 7), a);
253 return;
254
255 case Zumo32U4IMUType::LSM6DS33_LIS3MDL:
256 // assumes register address auto-increment is enabled (IF_INC in CTRL3_C)
257 readAxes16Bit(LSM6DS33_ADDR, LSM6DS33_REG_OUTX_L_XL, a);
258 return;
259 }
260}
261
262// Reads the 3 gyro channels and stores them in vector g
263void Zumo32U4IMU::readGyro()
264{
265 switch (type)
266 {
267 case Zumo32U4IMUType::LSM303D_L3GD20H:
268 // set MSB of register address for auto-increment
269 readAxes16Bit(L3GD20H_ADDR, L3GD20H_REG_OUT_X_L | (1 << 7), g);
270 return;
271
272 case Zumo32U4IMUType::LSM6DS33_LIS3MDL:
273 // assumes register address auto-increment is enabled (IF_INC in CTRL3_C)
274 readAxes16Bit(LSM6DS33_ADDR, LSM6DS33_REG_OUTX_L_G, g);
275 return;
276 }
277}
278
279// Reads the 3 magnetometer channels and stores them in vector m
280void Zumo32U4IMU::readMag()
281{
282 switch (type)
283 {
284 case Zumo32U4IMUType::LSM303D_L3GD20H:
285 // set MSB of register address for auto-increment
286 readAxes16Bit(LSM303D_ADDR, LSM303D_REG_OUT_X_L_M | (1 << 7), m);
287 return;
288
289 case Zumo32U4IMUType::LSM6DS33_LIS3MDL:
290 // set MSB of register address for auto-increment
291 readAxes16Bit(LIS3MDL_ADDR, LIS3MDL_REG_OUT_X_L | (1 << 7), m);
292 return;
293 }
294}
295
296// Reads all 9 accelerometer, gyro, and magnetometer channels and stores them
297// in the respective vectors
298void Zumo32U4IMU::read()
299{
300 readAcc();
301 if (lastError) { return; }
302 readGyro();
303 if (lastError) { return; }
304 readMag();
305}
306
307bool Zumo32U4IMU::accDataReady()
308{
309 switch (type)
310 {
311 case Zumo32U4IMUType::LSM303D_L3GD20H:
312 return readReg(LSM303D_ADDR, LSM303D_REG_STATUS_A) & 0x08;
313
314 case Zumo32U4IMUType::LSM6DS33_LIS3MDL:
315 return readReg(LSM6DS33_ADDR, LSM6DS33_REG_STATUS_REG) & 0x01;
316 }
317 return false;
318}
319
320bool Zumo32U4IMU::gyroDataReady()
321{
322 switch (type)
323 {
324 case Zumo32U4IMUType::LSM303D_L3GD20H:
325 return readReg(L3GD20H_ADDR, L3GD20H_REG_STATUS) & 0x08;
326
327 case Zumo32U4IMUType::LSM6DS33_LIS3MDL:
328 return readReg(LSM6DS33_ADDR, LSM6DS33_REG_STATUS_REG) & 0x02;
329 }
330 return false;
331}
332
333bool Zumo32U4IMU::magDataReady()
334{
335 switch (type)
336 {
337 case Zumo32U4IMUType::LSM303D_L3GD20H:
338 return readReg(LSM303D_ADDR, LSM303D_REG_STATUS_M) & 0x08;
339
340 case Zumo32U4IMUType::LSM6DS33_LIS3MDL:
341 return readReg(LIS3MDL_ADDR, LIS3MDL_REG_STATUS_REG) & 0x08;
342 }
343 return false;
344}
345
346int16_t Zumo32U4IMU::testReg(uint8_t addr, uint8_t reg)
347{
348 Wire.beginTransmission(addr);
349 Wire.write(reg);
350 if (Wire.endTransmission() != 0)
351 {
352 return TEST_REG_ERROR;
353 }
354
355 uint8_t byteCount = Wire.requestFrom(addr, (uint8_t)1);
356 if (byteCount != 1)
357 {
358 return TEST_REG_ERROR;
359 }
360 return Wire.read();
361}
362
363void Zumo32U4IMU::readAxes16Bit(uint8_t addr, uint8_t firstReg, vector<int16_t> & v)
364{
365 Wire.beginTransmission(addr);
366 Wire.write(firstReg);
367 lastError = Wire.endTransmission();
368 if (lastError) { return; }
369
370 uint8_t byteCount = (Wire.requestFrom(addr, (uint8_t)6));
371 if (byteCount != 6)
372 {
373 lastError = 50;
374 return;
375 }
376 uint8_t xl = Wire.read();
377 uint8_t xh = Wire.read();
378 uint8_t yl = Wire.read();
379 uint8_t yh = Wire.read();
380 uint8_t zl = Wire.read();
381 uint8_t zh = Wire.read();
382
383 // combine high and low bytes
384 v.x = (int16_t)(xh << 8 | xl);
385 v.y = (int16_t)(yh << 8 | yl);
386 v.z = (int16_t)(zh << 8 | zl);
387}
Zumo32U4IMU::readAcc
void readAcc()
Takes a reading from the accelerometer and makes the measurements available in a.
Definition: Zumo32U4IMU.cpp:246
Zumo32U4IMU::readGyro
void readGyro()
Takes a reading from the gyro and makes the measurements available in g.
Definition: Zumo32U4IMU.cpp:263
Zumo32U4IMU::a
vector< int16_t > a
Raw accelerometer readings.
Definition: Zumo32U4IMU.h:87
Zumo32U4IMU::m
vector< int16_t > m
Raw magnetometer readings.
Definition: Zumo32U4IMU.h:93
Zumo32U4IMU::g
vector< int16_t > g
Raw gyro readings.
Definition: Zumo32U4IMU.h:90
Zumo32U4IMU::accDataReady
bool accDataReady()
Indicates whether the accelerometer has new measurement data ready.
Definition: Zumo32U4IMU.cpp:307
Zumo32U4IMU::configureForBalancing
void configureForBalancing()
Configures the sensors with settings optimized for balancing.
Definition: Zumo32U4IMU.cpp:125
Zumo32U4IMU::gyroDataReady
bool gyroDataReady()
Indicates whether the gyro has new measurement data ready.
Definition: Zumo32U4IMU.cpp:320
Zumo32U4IMU::readMag
void readMag()
Takes a reading from the magnetometer and makes the measurements available in m.
Definition: Zumo32U4IMU.cpp:280
Zumo32U4IMU::readReg
uint8_t readReg(uint8_t addr, uint8_t reg)
Reads an 8-bit sensor register.
Definition: Zumo32U4IMU.cpp:229
Zumo32U4IMU::read
void read()
Takes a reading from all three sensors (accelerometer, gyro, and magnetometer) and makes their measur...
Definition: Zumo32U4IMU.cpp:298
Zumo32U4IMU::writeReg
void writeReg(uint8_t addr, uint8_t reg, uint8_t value)
Writes an 8-bit sensor register.
Definition: Zumo32U4IMU.cpp:221
Zumo32U4IMU::init
bool init()
Initializes the inertial sensors and detects their type.
Definition: Zumo32U4IMU.cpp:11
Zumo32U4IMU::enableDefault
void enableDefault()
Enables all of the inertial sensors with a default configuration.
Definition: Zumo32U4IMU.cpp:31
Zumo32U4IMU::configureForTurnSensing
void configureForTurnSensing()
Configures the sensors with settings optimized for turn sensing.
Definition: Zumo32U4IMU.cpp:168
Zumo32U4IMU::magDataReady
bool magDataReady()
Indicates whether the magnetometer has new measurement data ready.
Definition: Zumo32U4IMU.cpp:333
Zumo32U4IMU::configureForFaceUphill
void configureForFaceUphill()
Configures the sensors with settings optimized for the FaceUphill example program.
Definition: Zumo32U4IMU.cpp:197
Generated by doxygen 1.9.4