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 
11 bool 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 
31 void 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  default:
124  return;
125  }
126 }
127 
128 void Zumo32U4IMU::configureForBalancing()
129 {
130  switch (type)
131  {
132  case Zumo32U4IMUType::LSM303D_L3GD20H:
133 
134  // Accelerometer
135 
136  // 0x18 = 0b00011000
137  // AFS = 0 (+/- 2 g full scale)
138  writeReg(LSM303D_ADDR, LSM303D_REG_CTRL2, 0x18);
139  if (lastError) { return; }
140 
141  // Gyro
142 
143  // 0xFF = 0b11111111
144  // DR = 11 (757.6 Hz ODR); BW = 11 (100 Hz bandwidth); PD = 1 (normal mode); Zen = Yen = Xen = 1 (all axes enabled)
145  writeReg(L3GD20H_ADDR, L3GD20H_REG_CTRL1, 0xFF);
146  if (lastError) { return; }
147 
148  // 0x20 = 0b00100000
149  // FS = 10 (+/- 2000 dps full scale)
150  writeReg(L3GD20H_ADDR, L3GD20H_REG_CTRL4, 0x20);
151  return;
152 
153  case Zumo32U4IMUType::LSM6DS33_LIS3MDL:
154 
155  // Accelerometer
156 
157  // 0x3C = 0b00111100
158  // ODR = 0011 (52 Hz (high performance)); FS_XL = 11 (+/- 8 g full scale)
159  writeReg(LSM6DS33_ADDR, LSM6DS33_REG_CTRL1_XL, 0x3C);
160  if (lastError) { return; }
161 
162  // Gyro
163 
164  // 0x7C = 0b01111100
165  // ODR = 0111 (833 Hz (high performance)); FS_G = 11 (+/- 2000 dps full scale)
166  writeReg(LSM6DS33_ADDR, LSM6DS33_REG_CTRL2_G, 0x7C);
167  return;
168 
169  default:
170  return;
171  }
172 }
173 
174 void Zumo32U4IMU::configureForTurnSensing()
175 {
176  switch (type)
177  {
178  case Zumo32U4IMUType::LSM303D_L3GD20H:
179 
180  // Gyro
181 
182  // 0xFF = 0b11111111
183  // DR = 11 (757.6 Hz ODR); BW = 11 (100 Hz bandwidth); PD = 1 (normal mode); Zen = Yen = Xen = 1 (all axes enabled)
184  writeReg(L3GD20H_ADDR, L3GD20H_REG_CTRL1, 0xFF);
185  if (lastError) { return; }
186 
187  // 0x20 = 0b00100000
188  // FS = 10 (+/- 2000 dps full scale)
189  writeReg(L3GD20H_ADDR, L3GD20H_REG_CTRL4, 0x20);
190  return;
191 
192  case Zumo32U4IMUType::LSM6DS33_LIS3MDL:
193 
194  // Gyro
195 
196  // 0x7C = 0b01111100
197  // ODR = 0111 (833 Hz (high performance)); FS_G = 11 (+/- 2000 dps full scale)
198  writeReg(LSM6DS33_ADDR, LSM6DS33_REG_CTRL2_G, 0x7C);
199  return;
200 
201  default:
202  return;
203  }
204 }
205 
206 void Zumo32U4IMU::configureForFaceUphill()
207 {
208  switch (type)
209  {
210  case Zumo32U4IMUType::LSM303D_L3GD20H:
211 
212  // Accelerometer
213 
214  // 0x37 = 0b00110111
215  // AODR = 0011 (12.5 Hz ODR); AZEN = AYEN = AXEN = 1 (all axes enabled)
216  writeReg(LSM303D_ADDR, LSM303D_REG_CTRL1, 0x37);
217  return;
218 
219  case Zumo32U4IMUType::LSM6DS33_LIS3MDL:
220 
221  // Accelerometer
222 
223  // 0x10 = 0b00010000
224  // ODR = 0001 (13 Hz (high performance)); FS_XL = 00 (+/- 2 g full scale)
225  writeReg(LSM6DS33_ADDR, LSM6DS33_REG_CTRL1_XL, 0x10);
226  return;
227 
228  default:
229  return;
230  }
231 }
232 
233 void Zumo32U4IMU::writeReg(uint8_t addr, uint8_t reg, uint8_t value)
234 {
235  Wire.beginTransmission(addr);
236  Wire.write(reg);
237  Wire.write(value);
238  lastError = Wire.endTransmission();
239 }
240 
241 uint8_t Zumo32U4IMU::readReg(uint8_t addr, uint8_t reg)
242 {
243  Wire.beginTransmission(addr);
244  Wire.write(reg);
245  lastError = Wire.endTransmission();
246  if (lastError) { return 0; }
247 
248  uint8_t byteCount = Wire.requestFrom(addr, (uint8_t)1);
249  if (byteCount != 1)
250  {
251  lastError = 50;
252  return 0;
253  }
254  return Wire.read();
255 }
256 
257 // Reads the 3 accelerometer channels and stores them in vector a
258 void Zumo32U4IMU::readAcc(void)
259 {
260  switch (type)
261  {
262  case Zumo32U4IMUType::LSM303D_L3GD20H:
263  // set MSB of register address for auto-increment
264  readAxes16Bit(LSM303D_ADDR, LSM303D_REG_OUT_X_L_A | (1 << 7), a);
265  return;
266 
267  case Zumo32U4IMUType::LSM6DS33_LIS3MDL:
268  // assumes register address auto-increment is enabled (IF_INC in CTRL3_C)
269  readAxes16Bit(LSM6DS33_ADDR, LSM6DS33_REG_OUTX_L_XL, a);
270  return;
271 
272  default:
273  return;
274  }
275 }
276 
277 // Reads the 3 gyro channels and stores them in vector g
278 void Zumo32U4IMU::readGyro()
279 {
280  switch (type)
281  {
282  case Zumo32U4IMUType::LSM303D_L3GD20H:
283  // set MSB of register address for auto-increment
284  readAxes16Bit(L3GD20H_ADDR, L3GD20H_REG_OUT_X_L | (1 << 7), g);
285  return;
286 
287  case Zumo32U4IMUType::LSM6DS33_LIS3MDL:
288  // assumes register address auto-increment is enabled (IF_INC in CTRL3_C)
289  readAxes16Bit(LSM6DS33_ADDR, LSM6DS33_REG_OUTX_L_G, g);
290  return;
291 
292  default:
293  return;
294  }
295 }
296 
297 // Reads the 3 magnetometer channels and stores them in vector m
298 void Zumo32U4IMU::readMag()
299 {
300  switch (type)
301  {
302  case Zumo32U4IMUType::LSM303D_L3GD20H:
303  // set MSB of register address for auto-increment
304  readAxes16Bit(LSM303D_ADDR, LSM303D_REG_OUT_X_L_M | (1 << 7), m);
305  return;
306 
307  case Zumo32U4IMUType::LSM6DS33_LIS3MDL:
308  // set MSB of register address for auto-increment
309  readAxes16Bit(LIS3MDL_ADDR, LIS3MDL_REG_OUT_X_L | (1 << 7), m);
310  return;
311 
312  default:
313  return;
314  }
315 }
316 
317 // Reads all 9 accelerometer, gyro, and magnetometer channels and stores them
318 // in the respective vectors
319 void Zumo32U4IMU::read()
320 {
321  readAcc();
322  if (lastError) { return; }
323  readGyro();
324  if (lastError) { return; }
325  readMag();
326 }
327 
328 bool Zumo32U4IMU::accDataReady()
329 {
330  switch (type)
331  {
332  case Zumo32U4IMUType::LSM303D_L3GD20H:
333  return readReg(LSM303D_ADDR, LSM303D_REG_STATUS_A) & 0x08;
334 
335  case Zumo32U4IMUType::LSM6DS33_LIS3MDL:
336  return readReg(LSM6DS33_ADDR, LSM6DS33_REG_STATUS_REG) & 0x01;
337 
338  default:
339  return false;
340  }
341 }
342 
343 bool Zumo32U4IMU::gyroDataReady()
344 {
345  switch (type)
346  {
347  case Zumo32U4IMUType::LSM303D_L3GD20H:
348  return readReg(L3GD20H_ADDR, L3GD20H_REG_STATUS) & 0x08;
349 
350  case Zumo32U4IMUType::LSM6DS33_LIS3MDL:
351  return readReg(LSM6DS33_ADDR, LSM6DS33_REG_STATUS_REG) & 0x02;
352 
353  default:
354  return false;
355  }
356 }
357 
358 bool Zumo32U4IMU::magDataReady()
359 {
360  switch (type)
361  {
362  case Zumo32U4IMUType::LSM303D_L3GD20H:
363  return readReg(LSM303D_ADDR, LSM303D_REG_STATUS_M) & 0x08;
364 
365  case Zumo32U4IMUType::LSM6DS33_LIS3MDL:
366  return readReg(LIS3MDL_ADDR, LIS3MDL_REG_STATUS_REG) & 0x08;
367 
368  default:
369  return false;
370  }
371 }
372 
373 int16_t Zumo32U4IMU::testReg(uint8_t addr, uint8_t reg)
374 {
375  Wire.beginTransmission(addr);
376  Wire.write(reg);
377  if (Wire.endTransmission() != 0)
378  {
379  return TEST_REG_ERROR;
380  }
381 
382  uint8_t byteCount = Wire.requestFrom(addr, (uint8_t)1);
383  if (byteCount != 1)
384  {
385  return TEST_REG_ERROR;
386  }
387  return Wire.read();
388 }
389 
390 void Zumo32U4IMU::readAxes16Bit(uint8_t addr, uint8_t firstReg, vector<int16_t> & v)
391 {
392  Wire.beginTransmission(addr);
393  Wire.write(firstReg);
394  lastError = Wire.endTransmission();
395  if (lastError) { return; }
396 
397  uint8_t byteCount = (Wire.requestFrom(addr, (uint8_t)6));
398  if (byteCount != 6)
399  {
400  lastError = 50;
401  return;
402  }
403  uint8_t xl = Wire.read();
404  uint8_t xh = Wire.read();
405  uint8_t yl = Wire.read();
406  uint8_t yh = Wire.read();
407  uint8_t zl = Wire.read();
408  uint8_t zh = Wire.read();
409 
410  // combine high and low bytes
411  v.x = (int16_t)(xh << 8 | xl);
412  v.y = (int16_t)(yh << 8 | yl);
413  v.z = (int16_t)(zh << 8 | zl);
414 }
Zumo32U4IMU::readAcc
void readAcc()
Takes a reading from the accelerometer and makes the measurements available in a.
Definition: Zumo32U4IMU.cpp:258
Zumo32U4IMU::readGyro
void readGyro()
Takes a reading from the gyro and makes the measurements available in g.
Definition: Zumo32U4IMU.cpp:278
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:328
Zumo32U4IMU::configureForBalancing
void configureForBalancing()
Configures the sensors with settings optimized for balancing.
Definition: Zumo32U4IMU.cpp:128
Zumo32U4IMU::gyroDataReady
bool gyroDataReady()
Indicates whether the gyro has new measurement data ready.
Definition: Zumo32U4IMU.cpp:343
Zumo32U4IMU::readMag
void readMag()
Takes a reading from the magnetometer and makes the measurements available in m.
Definition: Zumo32U4IMU.cpp:298
Zumo32U4IMU::readReg
uint8_t readReg(uint8_t addr, uint8_t reg)
Reads an 8-bit sensor register.
Definition: Zumo32U4IMU.cpp:241
Zumo32U4IMU::read
void read()
Takes a reading from all three sensors (accelerometer, gyro, and magnetometer) and makes their measur...
Definition: Zumo32U4IMU.cpp:319
Zumo32U4IMU::writeReg
void writeReg(uint8_t addr, uint8_t reg, uint8_t value)
Writes an 8-bit sensor register.
Definition: Zumo32U4IMU.cpp:233
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:174
Zumo32U4IMU::magDataReady
bool magDataReady()
Indicates whether the magnetometer has new measurement data ready.
Definition: Zumo32U4IMU.cpp:358
Zumo32U4IMU::configureForFaceUphill
void configureForFaceUphill()
Configures the sensors with settings optimized for the FaceUphill example program.
Definition: Zumo32U4IMU.cpp:206
Generated by doxygen 1.9.1