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