| 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 |
| 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 | |