e4a4f539 |
1 | /* Copyright (c) 2015, Ian Sutton <ian@kremlin.cc> |
2 | |
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. |
6 | * |
7 | * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES |
e4a4f539 |
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. */ |
14 | |
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. |
18 | * |
19 | * steps to recreate: |
20 | * |
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 !! |
24 | * |
25 | * - wire SPI ports on the LSM to their respective pins on the due's SPI header (not |
26 | * ICSP!) |
27 | * |
28 | * - wire INT2 to digital pin 53 and the LSM's chip select/slave select port to |
29 | * pin 10 above the pwm module. |
30 | * |
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. |
34 | * |
35 | * - wire LSM's Vin to the arduino's 3.3V supply. leave Vdd floating */ |
36 | |
37 | #include <SPI.h> |
38 | |
39 | /* slave select pin */ |
40 | #define LSM_CS 10 |
41 | |
42 | /* wire to INT2 which latches to a magnometer-read-ready signal */ |
43 | #define MAGNO_RDY_PIN 53 |
44 | |
45 | /* SPI clock divider (84MHz divided by this equals SPI frequency) */ |
46 | #define SPI_CLK_DIV 3 |
47 | |
48 | /* frequency of temperature sensor in Hz, set in lsm_config() */ |
49 | #define TEMP_FREQ 100 |
50 | |
51 | /* struct representing magnometer values at shared instant of time */ |
52 | struct magno_point { |
53 | |
54 | signed short x; |
55 | signed short y; |
56 | signed short z; |
57 | }; |
58 | |
59 | /* hands off */ |
60 | const byte BAD_REGS[8] = { 0x00, 0x01, 0x02, 0x03, 0x04, 0x0E, 0x10, 0x11 }; |
61 | |
62 | /* these are set on successful completion of their respective funcs */ |
63 | bool SERIAL_CONFIGURED = 0; |
64 | bool SPI_CONFIGURED = 0; |
65 | bool SPI_OK = 0; |
66 | bool LSM_CONFIGURED = 0; |
67 | |
68 | /* don't brick the LSM */ |
69 | bool matches_badreg(byte addr) { |
70 | |
71 | short i; |
72 | i = 0; |
73 | |
74 | for (; i < 8; i++) |
75 | if (addr == BAD_REGS[i]) |
76 | return true; |
77 | |
78 | return false; |
79 | } |
80 | |
81 | /* fired when critical exception encountered, doesn't return */ |
82 | void killspin(String msg) { |
83 | |
84 | Serial.println(msg); |
85 | Serial.println("exception occurred or assertion failed. no-operation until reset"); |
86 | while (1); |
87 | } |
88 | |
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) { |
92 | |
93 | byte *ret, first; |
94 | short cnt; |
95 | |
96 | if (matches_badreg(addr)) |
97 | killspin("tried to read from a reserved, internal register!"); |
98 | |
99 | if (!SPI_CONFIGURED || !SPI_OK) |
100 | killspin("tried to read over SPI before it was configured or tested!"); |
101 | |
102 | /* addresses are 6 bits wide */ |
103 | if (addr > 63) |
104 | killspin("invalid lsm register address"); |
105 | |
106 | /* set msb to read, 2nd msb for multibyte op, and append 6 bit address field */ |
107 | first = 128 + 64 + addr; |
108 | cnt = 0; |
109 | |
110 | /* free this! */ |
111 | ret = (byte *) calloc(n, 1); |
112 | |
113 | /* kickoff read operation */ |
114 | SPI.transfer(LSM_CS, first, SPI_CONTINUE); |
115 | |
116 | for (; cnt < n - 1; cnt++) |
117 | ret[cnt] = SPI.transfer(LSM_CS, first, SPI_CONTINUE); |
118 | |
119 | ret[n - 1] = SPI.transfer(LSM_CS, first, SPI_LAST); |
120 | |
121 | return ret; |
122 | } |
123 | |
124 | /* reads & returns 1 byte @ addr */ |
125 | byte spi_read(byte addr) { |
126 | |
127 | byte first, ret; |
128 | |
129 | if (matches_badreg(addr)) |
130 | killspin("tried to read from a reserved, internal register!"); |
131 | |
132 | if (!SPI_CONFIGURED || !SPI_OK) |
133 | killspin("tried to read over SPI before it was configured or tested!"); |
134 | |
135 | /* addresses are 6 bits wide */ |
136 | if (addr > 63) |
137 | killspin("invalid lsm register address"); |
138 | |
139 | /* set msb to read and append 6 bit address field */ |
140 | first = 128 + addr; |
141 | |
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); |
145 | |
146 | return ret; |
147 | } |
148 | |
149 | /* write single byte @ addr */ |
150 | void spi_write(byte addr, byte data) { |
151 | |
152 | byte first; |
153 | |
154 | if (matches_badreg(addr)) |
155 | killspin("tried to write to a reserved, internal register!"); |
156 | |
157 | if (!SPI_CONFIGURED || !SPI_OK) |
158 | killspin("tried to write over SPI before it was configured or tested!"); |
159 | |
160 | /* addresses are 6 bits wide */ |
161 | if (addr > 63) |
162 | killspin("INVALID ADDRESS"); |
163 | |
164 | /* set 6 bit address field */ |
165 | first = addr; |
166 | |
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); |
170 | } |
171 | |
172 | void spi_config() { |
173 | |
174 | SPI.begin(LSM_CS); |
175 | |
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); |
181 | |
182 | /* LSM is big endian */ |
183 | SPI.setBitOrder(LSM_CS, MSBFIRST); |
184 | |
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); |
188 | |
189 | SPI_CONFIGURED = 1; |
190 | } |
191 | |
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 */ |
195 | boolean spi_test() { |
196 | |
197 | bool read_ok, write_ok; |
198 | byte i; |
199 | |
200 | i = 0; |
201 | read_ok = true; |
202 | write_ok = true; |
203 | |
204 | if (!SPI_CONFIGURED) |
205 | killspin("tried to test SPI before it was configured!"); |
206 | |
207 | /* cheat a little here */ |
208 | SPI_OK = 1; |
209 | |
210 | /* test reading */ |
211 | Serial.print(" [READ: "); |
212 | |
213 | for (; i < 100; i++) |
214 | if (spi_read(0x0F) != 0x49) |
215 | read_ok = false; |
216 | |
217 | if (read_ok) |
218 | Serial.print("OK, WRITE: "); |
219 | else |
220 | Serial.print("FAIL, WRITE: "); |
221 | |
222 | /* test writing with */ |
223 | i = 0; |
224 | for (; i < 100; i++) { |
225 | |
226 | spi_write(0x17, i); |
227 | |
228 | /* uncomment for write debugging |
229 | Serial.print("WROTE "); |
230 | Serial.print(i); |
231 | Serial.print(" GOT "); |
232 | Serial.println(spi_read(0x17));*/ |
233 | |
234 | if (spi_read(0x17) != i) |
235 | write_ok = false; |
236 | } |
237 | |
238 | /* write back datasheet-defined default value to test |
239 | * register we used (OFFSET_X_L_M) */ |
240 | if (write_ok) |
241 | spi_write(0x17, 0x00); |
242 | |
243 | /* finish up */ |
244 | if (write_ok) |
245 | Serial.print("OK] "); |
246 | else |
247 | Serial.print("FAIL] "); |
248 | |
249 | if (read_ok && write_ok) { |
250 | |
251 | SPI_OK = 1; |
252 | return true; |
253 | |
254 | } else { |
255 | |
256 | SPI_OK = 0; |
257 | Serial.println(":: failed!"); |
258 | return false; |
259 | } |
260 | } |
261 | |
262 | void lsm_config() { |
263 | |
264 | if (!SPI_CONFIGURED || !SPI_OK) |
265 | killspin("tried to configure lsm before spi was configured & tested"); |
266 | |
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 |
271 | * |
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 |
275 | * |
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: |
281 | * |
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 |
285 | * |
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: |
292 | * |
293 | * [x] :: 677 :: 0x02A5 |
294 | * [y] :: -16,324 :: 0xC03C |
295 | * [z] :: -6,146 :: 0xE7FE |
296 | */ |
297 | const byte x_lo_offset = 0xA5; |
298 | const byte x_hi_offset = 0x02; |
299 | |
300 | const byte y_lo_offset = 0x3C; |
301 | const byte y_hi_offset = 0xC0; |
302 | |
303 | const byte z_lo_offset = 0xFE; |
304 | const byte z_hi_offset = 0xE7; |
305 | |
306 | spi_write(0x16, x_lo_offset); |
307 | spi_write(0x17, x_hi_offset); |
308 | |
309 | spi_write(0x18, y_lo_offset); |
310 | spi_write(0x19, y_hi_offset); |
311 | |
312 | spi_write(0x1A, z_lo_offset); |
313 | spi_write(0x1B, z_hi_offset); |
314 | |
315 | /* latch magnometer-ready to INT2 output pin */ |
316 | spi_write(0x23, 0x04); |
317 | |
318 | /* enable temperature sensor, |
319 | * select high magnetic resolution, |
320 | * select 100Hz sensor rate */ |
321 | spi_write(0x24, 0xF4); |
322 | |
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); |
326 | |
327 | /* switch on magnometer, to continuous conversion mode */ |
328 | spi_write(0x26, 0x00); |
329 | |
330 | LSM_CONFIGURED = 1; |
331 | } |
332 | |
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) { |
340 | |
341 | signed short *ret; |
342 | int sensor_period, i; |
343 | byte *temp_pair, *sync_pair_i, *sync_pair_f; |
344 | |
345 | /* period length in milliseconds of temperature sensor refresh */ |
346 | sensor_period = 1000 / TEMP_FREQ; |
347 | |
348 | i = 0; |
349 | ret = (signed short *) calloc(n, 2); |
350 | |
351 | sync_pair_i = sync_pair_f = spi_multiread(0x05, 2); |
352 | |
353 | /* spin until sensor cranks */ |
354 | while (sync_pair_i == sync_pair_f) |
355 | sync_pair_f = spi_multiread(0x05, 2); |
356 | |
357 | /* wait until mid-period to read as to avoid edge-case duplicates */ |
358 | delay(sensor_period / 2); |
359 | |
360 | for (; i < n; i++) { |
361 | temp_pair = spi_multiread(0x05, 2); |
362 | ret[i] = word(temp_pair[1], temp_pair[0]); |
363 | free(temp_pair); |
364 | delay(sensor_period); |
365 | } |
366 | |
367 | free(sync_pair_i); |
368 | free(sync_pair_f); |
369 | |
370 | return ret; |
371 | } |
372 | |
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) { |
375 | |
376 | struct magno_point ret; |
377 | |
378 | ret.x = word(in[1], in[0]); |
379 | ret.y = word(in[3], in[2]); |
380 | ret.z = word(in[5], in[4]); |
381 | |
382 | return ret; |
383 | } |
384 | |
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) { |
392 | |
393 | struct magno_point *ret; |
394 | int i; |
395 | |
396 | i = 0; |
397 | ret = (struct magno_point *) calloc(n, sizeof(struct magno_point)); |
398 | |
399 | for(; i < n; i++) { |
400 | |
401 | /* spin until sensors are fresh */ |
402 | while(digitalRead(MAGNO_RDY_PIN) != 1); |
403 | |
404 | ret[i] = parse_raw_magno_data(spi_multiread(0x08, 6)); |
405 | } |
406 | |
407 | return ret; |
408 | } |
409 | |
410 | void setup() { |
411 | |
412 | /* set up magno. read ready signal */ |
413 | pinMode(MAGNO_RDY_PIN, INPUT); |
414 | |
415 | /* this is about as fast as you can get on the serial console */ |
416 | Serial.begin(115200); |
417 | SERIAL_CONFIGURED = 1; |
418 | |
419 | /* spi init */ |
420 | Serial.print("configuring spi.."); |
421 | spi_config(); |
422 | Serial.println("done"); |
423 | |
424 | /* spi check */ |
425 | Serial.print("testing spi.."); |
426 | if (spi_test()) |
427 | Serial.println("done"); |
428 | else |
429 | killspin("SPI test failed. perhaps the device is not wired correctly or your frequency is too high"); |
430 | |
431 | /* lsm init */ |
432 | Serial.print("configuring lsm.."); |
433 | lsm_config(); |
434 | Serial.println("done"); |
435 | } |
436 | |
437 | /* demonstration of realtime temperature stream. does not return */ |
438 | void stream_temp() { |
439 | |
440 | for(;;) { |
441 | signed short *temp_vals; |
442 | temp_vals = pull_temp_values(100); |
443 | |
444 | for (int i = 0; i < 100; i++) { |
445 | if(!(i % 20) && i) |
446 | Serial.print('\n'); |
447 | else if(i) |
448 | Serial.print(" "); |
449 | |
450 | Serial.print(temp_vals[i], DEC); |
451 | } |
452 | |
453 | Serial.println("\n--------------------------------------------------------------------------------"); |
454 | |
455 | free(temp_vals); |
456 | } |
457 | } |
458 | |
459 | /* demonstration of realtime magnometer stream. does not return */ |
460 | void stream_magno() { |
461 | |
462 | struct magno_point *read_buf; |
463 | |
464 | read_buf = pull_magno_values(100); |
465 | |
466 | for(int i = 0; i < 100; i++) { |
467 | |
468 | Serial.print("X: "); |
469 | Serial.println(read_buf[i].x, DEC); |
470 | Serial.print("Y: "); |
471 | Serial.println(read_buf[i].y, DEC); |
472 | Serial.print("Z: "); |
473 | Serial.println(read_buf[i].z, DEC); |
474 | Serial.println("---------"); |
475 | } |
476 | |
477 | free(read_buf); |
478 | } |
479 | |
480 | void loop() { |
481 | |
482 | stream_temp(); |
483 | //stream_magno(); |
484 | } |
485 | |