1 /* Copyright (c) 2015, Ian Sutton <ian@kremlin.cc>
3 * Permission to use, copy, modify, and/or distribute this software for any
4 * purpose with or without fee is hereby granted, provided that the above
5 * copyright notice and this permission notice appear in all copies.
7 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
8 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
9 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
10 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
11 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
12 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
13 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. */
15 /* this is a library for the arduino due that controls the LSM303D accelerometer
16 * & magnometer. it will not work on other arduino models, it uses API calls
17 * exclusive to newer ARM-based arduinos.
21 * !! be especially careful of the 5V output in on the due's SPI header. it is a high !!
22 * !! current output & will cook anything nearby. refer to this diagram before making !!
23 * !! any connections: http://uglyman.kremlin.cc/quick/due-pinout-web.png !!
25 * - wire SPI ports on the LSM to their respective pins on the due's SPI header (not
28 * - wire INT2 to digital pin 53 and the LSM's chip select/slave select port to
29 * pin 10 above the pwm module.
31 * - ground the SPI module, arduino, and LSM together, preferably alone. i had to use
32 * an audio isolation amplifier to provide a quiet enough ground to support the
33 * unusually high SPI baud rate i use here, discussed later.
35 * - wire LSM's Vin to the arduino's 3.3V supply. leave Vdd floating */
39 /* slave select pin */
42 /* wire to INT2 which latches to a magnometer-read-ready signal */
43 #define MAGNO_RDY_PIN 53
45 /* SPI clock divider (84MHz divided by this equals SPI frequency) */
48 /* frequency of temperature sensor in Hz, set in lsm_config() */
51 /* struct representing magnometer values at shared instant of time */
60 const byte BAD_REGS
[8] = { 0x00, 0x01, 0x02, 0x03, 0x04, 0x0E, 0x10, 0x11 };
62 /* these are set on successful completion of their respective funcs */
63 bool SERIAL_CONFIGURED
= 0;
64 bool SPI_CONFIGURED
= 0;
66 bool LSM_CONFIGURED
= 0;
68 /* don't brick the LSM */
69 bool matches_badreg(byte addr
) {
75 if (addr
== BAD_REGS
[i
])
81 /* fired when critical exception encountered, doesn't return */
82 void killspin(String msg
) {
85 Serial
.println("exception occurred or assertion failed. no-operation until reset");
89 /* reads n sequential bytes starting @ addr returns byte array of size n
90 * returned array must be free()'d manually! */
91 byte
*spi_multiread(byte addr
, short n
) {
96 if (matches_badreg(addr
))
97 killspin("tried to read from a reserved, internal register!");
99 if (!SPI_CONFIGURED
|| !SPI_OK
)
100 killspin("tried to read over SPI before it was configured or tested!");
102 /* addresses are 6 bits wide */
104 killspin("invalid lsm register address");
106 /* set msb to read, 2nd msb for multibyte op, and append 6 bit address field */
107 first
= 128 + 64 + addr
;
111 ret
= (byte
*) calloc(n
, 1);
113 /* kickoff read operation */
114 SPI
.transfer(LSM_CS
, first
, SPI_CONTINUE
);
116 for (; cnt
< n
- 1; cnt
++)
117 ret
[cnt
] = SPI
.transfer(LSM_CS
, first
, SPI_CONTINUE
);
119 ret
[n
- 1] = SPI
.transfer(LSM_CS
, first
, SPI_LAST
);
124 /* reads & returns 1 byte @ addr */
125 byte
spi_read(byte addr
) {
129 if (matches_badreg(addr
))
130 killspin("tried to read from a reserved, internal register!");
132 if (!SPI_CONFIGURED
|| !SPI_OK
)
133 killspin("tried to read over SPI before it was configured or tested!");
135 /* addresses are 6 bits wide */
137 killspin("invalid lsm register address");
139 /* set msb to read and append 6 bit address field */
142 /* perform 16 bit SPI exchange as per LSM datasheet's specification */
143 SPI
.transfer(LSM_CS
, first
, SPI_CONTINUE
);
144 ret
= SPI
.transfer(LSM_CS
, 0x00, SPI_LAST
);
149 /* write single byte @ addr */
150 void spi_write(byte addr
, byte data
) {
154 if (matches_badreg(addr
))
155 killspin("tried to write to a reserved, internal register!");
157 if (!SPI_CONFIGURED
|| !SPI_OK
)
158 killspin("tried to write over SPI before it was configured or tested!");
160 /* addresses are 6 bits wide */
162 killspin("INVALID ADDRESS");
164 /* set 6 bit address field */
167 /* perform 16 bit SPI exchange as per LSM datasheet's specification */
168 SPI
.transfer(LSM_CS
, first
, SPI_CONTINUE
);
169 SPI
.transfer(LSM_CS
, data
, SPI_LAST
);
176 /* my arduino due has a 84 MHz cpu which is divided here to provide the
177 * SPI baud rate. the LSM's data sheet purports the maximum SPI frequency
178 * is 10MHz, however i've found that it works fine up to 28MHz, which is
179 * the frequency set below (84 MHz / 3 = 28 MHz) */
180 SPI
.setClockDivider(LSM_CS
, SPI_CLK_DIV
);
182 /* LSM is big endian */
183 SPI
.setBitOrder(LSM_CS
, MSBFIRST
);
185 /* clock is active-low, exchange occurs on clock's first falling edge
186 * CPOL = 1, CKE = 0 */
187 SPI
.setDataMode(LSM_CS
, SPI_MODE3
);
192 /* read & check immutable device ID reg a number of times to guarantee LSM
193 * slave is responding and capable of handling master's SPI clock freq.
194 * then, write reg & read back to test writing */
197 bool read_ok
, write_ok
;
205 killspin("tried to test SPI before it was configured!");
207 /* cheat a little here */
211 Serial
.print(" [READ: ");
214 if (spi_read(0x0F) != 0x49)
218 Serial
.print("OK, WRITE: ");
220 Serial
.print("FAIL, WRITE: ");
222 /* test writing with */
224 for (; i
< 100; i
++) {
228 /* uncomment for write debugging
229 Serial.print("WROTE ");
231 Serial.print(" GOT ");
232 Serial.println(spi_read(0x17));*/
234 if (spi_read(0x17) != i
)
238 /* write back datasheet-defined default value to test
239 * register we used (OFFSET_X_L_M) */
241 spi_write(0x17, 0x00);
245 Serial
.print("OK] ");
247 Serial
.print("FAIL] ");
249 if (read_ok
&& write_ok
) {
257 Serial
.println(":: failed!");
264 if (!SPI_CONFIGURED
|| !SPI_OK
)
265 killspin("tried to configure lsm before spi was configured & tested");
267 /* set 16 bit 2's comp. magnetic field offset values for x, y, z.
268 * these default to zero as correct offset values depend on your geographical
269 * location. you can find the current magnetic field strength at your coords
270 * using NOAA's database: http://www.ngdc.noaa.gov/geomag-web/#igrfwmm
272 * in the WMM model, the significant values are north comp (z offset), east comp
273 * (x offset), and vertical comp (y offset). you can ignore the 'change/year' and
274 * 'uncertainty' values
276 * you must translate the given tesla values into corresponding gauss equivalents
277 * and scale them according to the range specified later in this function. you will
278 * usually use the +2:-2 gauss scale. here is an example conversion for
279 * latitude 43° 2' 14" N, longitude 76° 7' 36" W (near syracuse university in
280 * syracuse, NY 13210), 0 meters above sea level, taken on february 16th, 2015 at 07:32 UTC:
282 * [x] :: -4,129.9 nanoteslas :: -0.041299 gauss
283 * [y] :: 49,817.8 nanoteslas :: 0.498178 gauss
284 * [z] :: 18,755.9 nanoteslas :: 0.187559 gauss
286 * next, we need to fit these values into our 4-gauss scale (spanning from +2 gauss
287 * to -2 gauss) in the context of a 16-bit signed number. to do this, take the gauss value
288 * and divide it by 2. take this number, and multiply it by 2^15 - 1. round to closest integer.
289 * finally, multiply this value by -1 as the offset value combined with the sensed value should
290 * result in zero. negatives should be expressed as two's complement. here are the offsets derived
291 * from the previous values:
293 * [x] :: 677 :: 0x02A5
294 * [y] :: -16,324 :: 0xC03C
295 * [z] :: -6,146 :: 0xE7FE
297 const byte x_lo_offset
= 0xA5;
298 const byte x_hi_offset
= 0x02;
300 const byte y_lo_offset
= 0x3C;
301 const byte y_hi_offset
= 0xC0;
303 const byte z_lo_offset
= 0xFE;
304 const byte z_hi_offset
= 0xE7;
306 spi_write(0x16, x_lo_offset
);
307 spi_write(0x17, x_hi_offset
);
309 spi_write(0x18, y_lo_offset
);
310 spi_write(0x19, y_hi_offset
);
312 spi_write(0x1A, z_lo_offset
);
313 spi_write(0x1B, z_hi_offset
);
315 /* latch magnometer-ready to INT2 output pin */
316 spi_write(0x23, 0x04);
318 /* enable temperature sensor,
319 * select high magnetic resolution,
320 * select 100Hz sensor rate */
321 spi_write(0x24, 0xF4);
323 /* set full scale of magnetometer to +2:-2 gauss as
324 * earth's field is usually between +0.65:-0.65 G */
325 spi_write(0x25, 0x00);
327 /* switch on magnometer, to continuous conversion mode */
328 spi_write(0x26, 0x00);
333 /* returns a size-n array of readings from temperature sensor. function waits
334 * between reads for a time equal to the period length of the sensor as to avoid
335 * multiple reads of an un-updated value. this is a hack to get around the fact
336 * this chip does not have a TEMP_READY bit in a status register like the magnometer
337 * or accelerometer do.
338 * caller must free() returned pointer */
339 signed short *pull_temp_values(int n
) {
342 int sensor_period
, i
;
343 byte
*temp_pair
, *sync_pair_i
, *sync_pair_f
;
345 /* period length in milliseconds of temperature sensor refresh */
346 sensor_period
= 1000 / TEMP_FREQ
;
349 ret
= (signed short *) calloc(n
, 2);
351 sync_pair_i
= sync_pair_f
= spi_multiread(0x05, 2);
353 /* spin until sensor cranks */
354 while (sync_pair_i
== sync_pair_f
)
355 sync_pair_f
= spi_multiread(0x05, 2);
357 /* wait until mid-period to read as to avoid edge-case duplicates */
358 delay(sensor_period
/ 2);
361 temp_pair
= spi_multiread(0x05, 2);
362 ret
[i
] = word(temp_pair
[1], temp_pair
[0]);
364 delay(sensor_period
);
373 /* returns a magno_point struct from passed 48 bit input taken from magno sensors */
374 struct magno_point
parse_raw_magno_data(byte
*in
) {
376 struct magno_point ret
;
378 ret
.x
= word(in
[1], in
[0]);
379 ret
.y
= word(in
[3], in
[2]);
380 ret
.z
= word(in
[5], in
[4]);
385 /* returns a size-n array of readings from magnometer. result contains 3 words
386 * describing felt magnetic field strengths x, y, z directions. function polls INT2
387 * (latched to magnometer-ready signal) until it goes high before reading from sensor.
388 * this guarantees each member in returned array is a genuine, non-repeat value from a
389 * single magnometer sensor cycle
390 * caller must free() returned pointer */
391 struct magno_point
*pull_magno_values(int n
) {
393 struct magno_point
*ret
;
397 ret
= (struct magno_point
*) calloc(n
, sizeof(struct magno_point
));
401 /* spin until sensors are fresh */
402 while(digitalRead(MAGNO_RDY_PIN
) != 1);
404 ret
[i
] = parse_raw_magno_data(spi_multiread(0x08, 6));
412 /* set up magno. read ready signal */
413 pinMode(MAGNO_RDY_PIN
, INPUT
);
415 /* this is about as fast as you can get on the serial console */
416 Serial
.begin(115200);
417 SERIAL_CONFIGURED
= 1;
420 Serial
.print("configuring spi..");
422 Serial
.println("done");
425 Serial
.print("testing spi..");
427 Serial
.println("done");
429 killspin("SPI test failed. perhaps the device is not wired correctly or your frequency is too high");
432 Serial
.print("configuring lsm..");
434 Serial
.println("done");
437 /* demonstration of realtime temperature stream. does not return */
441 signed short *temp_vals
;
442 temp_vals
= pull_temp_values(100);
444 for (int i
= 0; i
< 100; i
++) {
450 Serial
.print(temp_vals
[i
], DEC
);
453 Serial
.println("\n--------------------------------------------------------------------------------");
459 /* demonstration of realtime magnometer stream. does not return */
460 void stream_magno() {
462 struct magno_point
*read_buf
;
464 read_buf
= pull_magno_values(100);
466 for(int i
= 0; i
< 100; i
++) {
469 Serial
.println(read_buf
[i
].x
, DEC
);
471 Serial
.println(read_buf
[i
].y
, DEC
);
473 Serial
.println(read_buf
[i
].z
, DEC
);
474 Serial
.println("---------");