add ability to build and upload sketches from the bash command line (via beta ino2cpp...
[zumo-cc3200/zumo-cc3200.git] / src / Energia / ZumoTest / LSM303.cpp
1 #include <Wire.h>
2 #include <math.h>
4 #include "LSM303.h"
6 #include <xdc/runtime/System.h>
8 // Defines ////////////////////////////////////////////////////////////////
10 // The Arduino two-wire interface uses a 7-bit number for the address,
11 // and sets the last bit correctly based on reads and writes
12 #define D_SA0_HIGH_ADDRESS                0b0011101
13 #define D_SA0_LOW_ADDRESS                 0b0011110
14 #define DLHC_DLM_DLH_MAG_ADDRESS          0b0011110
15 #define DLHC_DLM_DLH_ACC_SA0_HIGH_ADDRESS 0b0011001
16 #define DLM_DLH_ACC_SA0_LOW_ADDRESS       0b0011000
18 #define TEST_REG_ERROR -1
20 #define D_WHO_ID    0x49
21 #define DLM_WHO_ID  0x3C
23 static int testReg(byte address, LSM303::regAddr reg);
25 // Constructors ///////////////////////////////////////////////////////////////
27 LSM303::LSM303(void)
28 {
29     /*
30      * These values lead to an assumed magnetometer bias of 0.
31      * Use the Calibrate example program to determine appropriate values
32      * for your particular unit. The Heading example demonstrates how to
33      * adjust these values in your own sketch.
34      */
35     m_min = (LSM303::vector<int16_t>){-32767, -32767, -32767};
36     m_max = (LSM303::vector<int16_t>){+32767, +32767, +32767};
38     _device = device_auto;
40     io_timeout = 0;  // 0 = no timeout
41     did_timeout = false;
42 }
44 // Public Methods /////////////////////////////////////////////////////////////
46 // Did a timeout occur in readAcc(), readMag(), or read() since the last call to timeoutOccurred()?
47 bool LSM303::timeoutOccurred()
48 {
49     bool tmp = did_timeout;
50     did_timeout = false;
51     return tmp;
52 }
54 void LSM303::setTimeout(unsigned int timeout)
55 {
56     io_timeout = timeout;
57 }
59 unsigned int LSM303::getTimeout()
60 {
61     return io_timeout;
62 }
64 bool LSM303::init(deviceType device, sa0State sa0)
65 {
66     // perform auto-detection unless device type and SA0 state were both specified
67     if (device == device_auto || sa0 == sa0_auto)
68     {
69         // check for LSM303D if device is unidentified or was specified to be this type
70         if (device == device_auto || device == device_D)
71         {
72             // check SA0 high address unless SA0 was specified to be low
73             if (sa0 != sa0_low && testReg(D_SA0_HIGH_ADDRESS, WHO_AM_I) == D_WHO_ID)
74             {
75                 // device responds to address 0011101 with D ID; it's a D with SA0 high
76                 device = device_D;
77                 sa0 = sa0_high;
78             }
79             // check SA0 low address unless SA0 was specified to be high
80             else if (sa0 != sa0_high && testReg(D_SA0_LOW_ADDRESS, WHO_AM_I) == D_WHO_ID)
81             {
82                 // device responds to address 0011110 with D ID; it's a D with SA0 low
83                 device = device_D;
84                 sa0 = sa0_low;
85             }
86         }
87     
88         // check for LSM303DLHC, DLM, DLH if device is still unidentified or was specified to be one of these types
89         if (device == device_auto || device == device_DLHC || device == device_DLM || device == device_DLH)
90         {
91             // check SA0 high address unless SA0 was specified to be low
92             if (sa0 != sa0_low && testReg(DLHC_DLM_DLH_ACC_SA0_HIGH_ADDRESS, CTRL_REG1_A) != TEST_REG_ERROR)
93             {
94                 // device responds to address 0011001; it's a DLHC, DLM with SA0 high, or DLH with SA0 high
95                 sa0 = sa0_high;
96                 if (device == device_auto)
97                 { 
98                     // use magnetometer WHO_AM_I register to determine device type
99                     //
100                     // DLHC seems to respond to WHO_AM_I request the same way as DLM, even though this
101                     // register isn't documented in its datasheet. Since the DLHC accelerometer address is the
102                     // same as the DLM with SA0 high, but Pololu DLM boards pull SA0 low by default, we'll
103                     // guess that a device whose accelerometer responds to the SA0 high address and whose
104                     // magnetometer gives the DLM ID is actually a DLHC.
105                     device = (testReg(DLHC_DLM_DLH_MAG_ADDRESS, WHO_AM_I_M) == DLM_WHO_ID) ? device_DLHC : device_DLH;
106                 }
107             }
108             // check SA0 low address unless SA0 was specified to be high
109             else if (sa0 != sa0_high && testReg(DLM_DLH_ACC_SA0_LOW_ADDRESS, CTRL_REG1_A) != TEST_REG_ERROR)
110             {
111                 // device responds to address 0011000; it's a DLM with SA0 low or DLH with SA0 low
112                 sa0 = sa0_low;
113                 if (device == device_auto)
114                 {
115                     // use magnetometer WHO_AM_I register to determine device type
116                     device = (testReg(DLHC_DLM_DLH_MAG_ADDRESS, WHO_AM_I_M) == DLM_WHO_ID) ? device_DLM : device_DLH;
117                 }
118             }
119         }
120     
121         // make sure device and SA0 were successfully detected; otherwise, indicate failure
122         if (device == device_auto || sa0 == sa0_auto)
123         {
124             return false;
125         }
126     }
127   
128     _device = device;
129   
130     // set device addresses and translated register addresses
131     switch (device) {
132         case device_D:
133             acc_address = mag_address = (sa0 == sa0_high) ? D_SA0_HIGH_ADDRESS : D_SA0_LOW_ADDRESS;
134             translated_regs[-OUT_X_L_M] = D_OUT_X_L_M;
135             translated_regs[-OUT_X_H_M] = D_OUT_X_H_M;
136             translated_regs[-OUT_Y_L_M] = D_OUT_Y_L_M;
137             translated_regs[-OUT_Y_H_M] = D_OUT_Y_H_M;
138             translated_regs[-OUT_Z_L_M] = D_OUT_Z_L_M;
139             translated_regs[-OUT_Z_H_M] = D_OUT_Z_H_M;
140             break;
142         case device_DLHC:
143             acc_address = DLHC_DLM_DLH_ACC_SA0_HIGH_ADDRESS; // DLHC doesn't have configurable SA0 but uses same acc address as DLM/DLH with SA0 high
144             mag_address = DLHC_DLM_DLH_MAG_ADDRESS;
145             translated_regs[-OUT_X_H_M] = DLHC_OUT_X_H_M;
146             translated_regs[-OUT_X_L_M] = DLHC_OUT_X_L_M;
147             translated_regs[-OUT_Y_H_M] = DLHC_OUT_Y_H_M;
148             translated_regs[-OUT_Y_L_M] = DLHC_OUT_Y_L_M;
149             translated_regs[-OUT_Z_H_M] = DLHC_OUT_Z_H_M;
150             translated_regs[-OUT_Z_L_M] = DLHC_OUT_Z_L_M;
151             break;
153         case device_DLM:
154             acc_address = (sa0 == sa0_high) ? DLHC_DLM_DLH_ACC_SA0_HIGH_ADDRESS : DLM_DLH_ACC_SA0_LOW_ADDRESS;
155             mag_address = DLHC_DLM_DLH_MAG_ADDRESS;
156             translated_regs[-OUT_X_H_M] = DLM_OUT_X_H_M;
157             translated_regs[-OUT_X_L_M] = DLM_OUT_X_L_M;
158             translated_regs[-OUT_Y_H_M] = DLM_OUT_Y_H_M;
159             translated_regs[-OUT_Y_L_M] = DLM_OUT_Y_L_M;
160             translated_regs[-OUT_Z_H_M] = DLM_OUT_Z_H_M;
161             translated_regs[-OUT_Z_L_M] = DLM_OUT_Z_L_M;
162             break;
164         case device_DLH:
165             acc_address = (sa0 == sa0_high) ? DLHC_DLM_DLH_ACC_SA0_HIGH_ADDRESS : DLM_DLH_ACC_SA0_LOW_ADDRESS;
166             mag_address = DLHC_DLM_DLH_MAG_ADDRESS;
167             translated_regs[-OUT_X_H_M] = DLH_OUT_X_H_M;
168             translated_regs[-OUT_X_L_M] = DLH_OUT_X_L_M;
169             translated_regs[-OUT_Y_H_M] = DLH_OUT_Y_H_M;
170             translated_regs[-OUT_Y_L_M] = DLH_OUT_Y_L_M;
171             translated_regs[-OUT_Z_H_M] = DLH_OUT_Z_H_M;
172             translated_regs[-OUT_Z_L_M] = DLH_OUT_Z_L_M;
173             break;
174     }
175   
176     return true;
179 /*
180 Enables the LSM303's accelerometer and magnetometer. Also:
181 - Sets sensor full scales (gain) to default power-on values, which are
182   +/- 2 g for accelerometer and +/- 1.3 gauss for magnetometer
183   (+/- 4 gauss on LSM303D).
184 - Selects 50 Hz ODR (output data rate) for accelerometer and 7.5 Hz
185   ODR for magnetometer (6.25 Hz on LSM303D). (These are the ODR
186   settings for which the electrical characteristics are specified in
187   the datasheets.)
188 - Enables high resolution modes (if available).
189 Note that this function will also reset other settings controlled by
190 the registers it writes to.
191 */
192 void LSM303::enableDefault(void)
194     if (_device == device_D)
195     {
196         // Accelerometer
198         // 0x00 = 0b00000000
199         // AFS = 0 (+/- 2 g full scale)
200         writeReg(CTRL2, 0x00);
202         // 0x57 = 0b01010111
203         // AODR = 0101 (50 Hz ODR); AZEN = AYEN = AXEN = 1 (all axes enabled)
204         writeReg(CTRL1, 0x57);
206         // Magnetometer
208         // 0x64 = 0b01100100
209         // M_RES = 11 (high resolution mode); M_ODR = 001 (6.25 Hz ODR)
210         writeReg(CTRL5, 0x64);
212         // 0x20 = 0b00100000
213         // MFS = 01 (+/- 4 gauss full scale)
214         writeReg(CTRL6, 0x20);
216         // 0x00 = 0b00000000
217         // MLP = 0 (low power mode off); MD = 00 (continuous-conversion mode)
218         writeReg(CTRL7, 0x00);
219     }
220     else
221     {
222         // Accelerometer
223     
224         if (_device == device_DLHC)
225         {
226             // 0x08 = 0b00001000
227             // FS = 00 (+/- 2 g full scale); HR = 1 (high resolution enable)
228             writeAccReg(CTRL_REG4_A, 0x08);
230             // 0x47 = 0b01000111
231             // ODR = 0100 (50 Hz ODR); LPen = 0 (normal mode); Zen = Yen = Xen = 1 (all axes enabled)
232             writeAccReg(CTRL_REG1_A, 0x47);
233         }
234         else // DLM, DLH
235         {
236             // 0x00 = 0b00000000
237             // FS = 00 (+/- 2 g full scale)
238             writeAccReg(CTRL_REG4_A, 0x00);
240             // 0x27 = 0b00100111
241             // PM = 001 (normal mode); DR = 00 (50 Hz ODR); Zen = Yen = Xen = 1 (all axes enabled)
242             writeAccReg(CTRL_REG1_A, 0x27);
243         }
245         // Magnetometer
247         // 0x0C = 0b00001100
248         // DO = 011 (7.5 Hz ODR)
249         writeMagReg(CRA_REG_M, 0x0C);
251         // 0x20 = 0b00100000
252         // GN = 001 (+/- 1.3 gauss full scale)
253         writeMagReg(CRB_REG_M, 0x20);
255         // 0x00 = 0b00000000
256         // MD = 00 (continuous-conversion mode)
257         writeMagReg(MR_REG_M, 0x00);
258     }
261 // Writes an accelerometer register
262 void LSM303::writeAccReg(byte reg, byte value)
264     Wire.beginTransmission(acc_address);
265     Wire.write(reg);
266     Wire.write(value);
267     last_status = Wire.endTransmission();
270 // Reads an accelerometer register
271 byte LSM303::readAccReg(byte reg)
273     byte value;
275     Wire.beginTransmission(acc_address);
276     Wire.write(reg);
277     last_status = Wire.endTransmission();
278     Wire.requestFrom(acc_address, (byte)1);
279     value = Wire.read();
280     Wire.endTransmission();
282     return value;
285 // Writes a magnetometer register
286 void LSM303::writeMagReg(byte reg, byte value)
288     Wire.beginTransmission(mag_address);
289     Wire.write(reg);
290     Wire.write(value);
291     last_status = Wire.endTransmission();
294 // Reads a magnetometer register
295 byte LSM303::readMagReg(int reg)
297     byte value;
299     // if dummy register address (magnetometer Y/Z), look up actual translated address (based on device type)
300     if (reg < 0)
301     {
302         reg = translated_regs[-reg];
303     }
305     Wire.beginTransmission(mag_address);
306     Wire.write(reg);
307     last_status = Wire.endTransmission();
308     Wire.requestFrom(mag_address, (byte)1);
309     value = Wire.read();
310     Wire.endTransmission();
312     return value;
315 void LSM303::writeReg(byte reg, byte value)
317     // mag address == acc_address for LSM303D, so it doesn't really matter which one we use.
318     if (_device == device_D || reg < CTRL_REG1_A)
319     {
320         writeMagReg(reg, value);
321     }
322     else
323     {
324         writeAccReg(reg, value);
325     }
328 // Note that this function will not work for reading TEMP_OUT_H_M and TEMP_OUT_L_M on the DLHC.
329 // To read those two registers, use readMagReg() instead.
330 byte LSM303::readReg(int reg)
332     // mag address == acc_address for LSM303D, so it doesn't really matter which one we use.
333     // Use readMagReg so it can translate OUT_[XYZ]_[HL]_M
334     if (_device == device_D || reg < CTRL_REG1_A)
335     {
336         return readMagReg(reg);
337     }
338     else
339     {
340         return readAccReg(reg);
341     }
344 // Reads the 3 accelerometer channels and stores them in vector a
345 void LSM303::readAcc(void)
347     Wire.beginTransmission(acc_address);
348     // assert the MSB of the address to get the accelerometer
349     // to do slave-transmit subaddress updating.
350     Wire.write(OUT_X_L_A | (1 << 7));
351     last_status = Wire.endTransmission();
352     Wire.requestFrom(acc_address, (byte)6);
354     unsigned int millis_start = millis();
355     while (Wire.available() < 6) {
356         if (io_timeout > 0 && ((unsigned int)millis() - millis_start) > io_timeout)
357         {
358             did_timeout = true;
359             return;
360         }
361     }
363     byte xla = Wire.read();
364     byte xha = Wire.read();
365     byte yla = Wire.read();
366     byte yha = Wire.read();
367     byte zla = Wire.read();
368     byte zha = Wire.read();
370     // combine high and low bytes
371     // This no longer drops the lowest 4 bits of the readings from the DLH/DLM/DLHC, which are always 0
372     // (12-bit resolution, left-aligned). The D has 16-bit resolution
373     a.x = (int16_t)(xha << 8 | xla);
374     a.y = (int16_t)(yha << 8 | yla);
375     a.z = (int16_t)(zha << 8 | zla);
378 // Reads the 3 magnetometer channels and stores them in vector m
379 void LSM303::readMag(void)
381     Wire.beginTransmission(mag_address);
382     // If LSM303D, assert MSB to enable subaddress updating
383     // OUT_X_L_M comes first on D, OUT_X_H_M on others
384     Wire.write((_device == device_D) ? translated_regs[-OUT_X_L_M] | (1 << 7) : translated_regs[-OUT_X_H_M]);
385     last_status = Wire.endTransmission();
386     Wire.requestFrom(mag_address, (byte)6);
388     unsigned int millis_start = millis();
389     while (Wire.available() < 6) {
390         if (io_timeout > 0 && ((unsigned int)millis() - millis_start) > io_timeout)
391         {
392             did_timeout = true;
393             return;
394         }
395     }
397     byte xlm, xhm, ylm, yhm, zlm, zhm;
399     if (_device == device_D)
400     {
401         // D: X_L, X_H, Y_L, Y_H, Z_L, Z_H
402         xlm = Wire.read();
403         xhm = Wire.read();
404         ylm = Wire.read();
405         yhm = Wire.read();
406         zlm = Wire.read();
407         zhm = Wire.read();
408     }
409     else
410     {
411         // DLHC, DLM, DLH: X_H, X_L...
412         xhm = Wire.read();
413         xlm = Wire.read();
415         if (_device == device_DLH)
416         {
417             // DLH: ...Y_H, Y_L, Z_H, Z_L
418             yhm = Wire.read();
419             ylm = Wire.read();
420             zhm = Wire.read();
421             zlm = Wire.read();
422         }
423         else
424         {
425             // DLM, DLHC: ...Z_H, Z_L, Y_H, Y_L
426             zhm = Wire.read();
427             zlm = Wire.read();
428             yhm = Wire.read();
429             ylm = Wire.read();
430         }
431     }
433     // combine high and low bytes
434     m.x = (int16_t)(xhm << 8 | xlm);
435     m.y = (int16_t)(yhm << 8 | ylm);
436     m.z = (int16_t)(zhm << 8 | zlm);
439 // Reads all 6 channels of the LSM303 and stores them in the object variables
440 void LSM303::read(void)
442     readAcc();
443     readMag();
446 /*
447   Returns the angular difference in the horizontal plane between a
448   default vector and north, in degrees.
450   The default vector here is chosen to point along the surface of the
451   PCB, in the direction of the top of the text on the silkscreen.
452   This is the +X axis on the Pololu LSM303D carrier and the -Y axis on
453   the Pololu LSM303DLHC, LSM303DLM, and LSM303DLH carriers.
454 */
455 float LSM303::heading(void)
457     if (_device == device_D)
458     {
459         return heading((vector<int>){1, 0, 0});
460     }
461     else
462     {
463         return heading((vector<int>){0, -1, 0});
464     }
467 void LSM303::vector_normalize(vector<float> *a)
469     float mag = sqrt(vector_dot(a, a));
470     a->x /= mag;
471     a->y /= mag;
472     a->z /= mag;
475 // Private Methods ////////////////////////////////////////////////////////////
477 static int testReg(byte address, LSM303::regAddr reg)
479     System_printf("testReg: 0x%x : 0x%x ...", address, reg);
480     Wire.beginTransmission(address);
481     System_printf(" begin ...");
482     Wire.write((byte)reg);
483     if (Wire.endTransmission() != 0) {
484         System_printf("testReg: error\n");
485         return TEST_REG_ERROR;
486     }
488     System_printf(" request from 0x%x...", address);
489     Wire.requestFrom(address, (byte)1);
490     System_printf(" read ...");
491     if (Wire.available()) {
492         return Wire.read();
493     }
494     else {
495         System_printf("testReg: error\n");
496         return TEST_REG_ERROR;
497     }