add ability to build and upload sketches from the bash command line (via beta ino2cpp...
[zumo-cc3200/zumo-cc3200.git] / src / Energia / ZumoTest / L3G.cpp
1 /*
2  *  ======== L3G.cpp ========
3  */
5 #include <Wire.h>
6 #include <math.h>
8 #include "L3G.h"
10 // Defines ////////////////////////////////////////////////////////////////
12 /* The Arduino two-wire interface uses a 7-bit number for the address,
13  * and sets the last bit correctly based on reads and writes
14  */
15 #define D20_SA0_HIGH_ADDRESS      0b1101011 // also applies to D20H
16 #define D20_SA0_LOW_ADDRESS       0b1101010 // also applies to D20H
17 #define L3G4200D_SA0_HIGH_ADDRESS 0b1101001
18 #define L3G4200D_SA0_LOW_ADDRESS  0b1101000
20 #define TEST_REG_ERROR -1
22 #define D20H_WHO_ID     0xD7
23 #define D20_WHO_ID      0xD4
24 #define L3G4200D_WHO_ID 0xD3
26 static int testReg(byte address, L3G::regAddr reg);
28 /*
29  *  ======== L3G ========
30  *  Constructors for L3G gyro
31  */
32 L3G::L3G(void)
33 {
34     _device = device_auto;
36     io_timeout = 0;  // 0 = no timeout
37     did_timeout = false;
38 }
40 /*
41  *  ======== timeoutOccurred ========
42  *  Did a timeout occur in read() since the last call to timeoutOccurred()?
43  */
44 bool L3G::timeoutOccurred()
45 {
46     bool tmp = did_timeout;
47     did_timeout = false;
48     return tmp;
49 }
51 /*
52  *  ======== setTimeout ========
53  */
54 void L3G::setTimeout(unsigned int timeout)
55 {
56     io_timeout = timeout;
57 }
59 /*
60  *  ======== getTimeout ========
61  */
62 unsigned int L3G::getTimeout()
63 {
64     return io_timeout;
65 }
67 /*
68  *  ======== init ========
69  */
70 bool L3G::init(deviceType device, sa0State sa0)
71 {
72     int id;
73   
74     /* perform auto-detection unless device type and SA0 state were both
75      * specified
76      */
77     if (device == device_auto || sa0 == sa0_auto) {
78         /* check for L3GD20H, D20 if device is unidentified or was specified
79          * to be one of these types
80          */
81         if (device == device_auto || device == device_D20H
82             || device == device_D20) {
83             // check SA0 high address unless SA0 was specified to be low
84             if (sa0 != sa0_low
85                 && (id = testReg(D20_SA0_HIGH_ADDRESS, WHO_AM_I)) != TEST_REG_ERROR) {
86                 /* device responds to address 1101011; it's a D20H or D20
87                  * with SA0 high
88                  */
89                 sa0 = sa0_high;
90                 if (device == device_auto) {
91                     // use ID from WHO_AM_I register to determine device type
92                     device = (id == D20H_WHO_ID) ? device_D20H : device_D20;
93                 }
94             }
95             // check SA0 low address unless SA0 was specified to be high
96             else if (sa0 != sa0_high
97                      && (id = testReg(D20_SA0_LOW_ADDRESS, WHO_AM_I)) != TEST_REG_ERROR) {
98                 /* device responds to address 1101010; it's a D20H or D20
99                  * with SA0 low
100                  */
101                 sa0 = sa0_low;
102                 if (device == device_auto) {
103                     // use ID from WHO_AM_I register to determine device type
104                     device = (id == D20H_WHO_ID) ? device_D20H : device_D20;
105                 }
106             }
107         }
108     
109         /* check for L3G4200D if device is still unidentified or was specified
110          * to be this type
111          */
112         if (device == device_auto || device == device_4200D) {
113             if (sa0 != sa0_low
114                 && testReg(L3G4200D_SA0_HIGH_ADDRESS, WHO_AM_I) == L3G4200D_WHO_ID) {
115                 /* device responds to address 1101001; it's a 4200D with SA0
116                  * high
117                  */
118                 device = device_4200D;
119                 sa0 = sa0_high;
120             }
121             else if (sa0 != sa0_high
122                      && testReg(L3G4200D_SA0_LOW_ADDRESS, WHO_AM_I) == L3G4200D_WHO_ID) {
123                 /* device responds to address 1101000; it's a 4200D with SA0
124                  * low
125                  */
126                 device = device_4200D;
127                 sa0 = sa0_low;
128             }
129         }
130     
131         /* make sure device and SA0 were successfully detected; otherwise,
132          * indicate failure
133          */
134         if (device == device_auto || sa0 == sa0_auto) {
135             return false;
136         }
137     }
138   
139     _device = device;
141     // set device address
142     switch (device) {
143         case device_D20H:
144         case device_D20:
145             address = (sa0 == sa0_high) ? D20_SA0_HIGH_ADDRESS : D20_SA0_LOW_ADDRESS;
146             break;
148         case device_4200D:
149             address = (sa0 == sa0_high) ? L3G4200D_SA0_HIGH_ADDRESS : L3G4200D_SA0_LOW_ADDRESS;
150             break;
151     }
152   
153     return true;
156 /*
157  *  ======== enableDefault ========
158  *  Enables the L3G's gyro
159  *
160  *  Also:
161  *      - Sets gyro full scale (gain) to default power-on value of +/- 250 dps
162  *       (specified as +/- 245 dps for L3GD20H).
163  *      - Selects 200 Hz ODR (output data rate). (Exact rate is specified as
164  *        189.4 Hz for L3GD20H and 190 Hz for L3GD20.)
165  *  
166  *  Note that this function will also reset other settings controlled by
167  *  the registers it writes to.
168 */
169 void L3G::enableDefault(void)
171     if (_device == device_D20H) {
172         // 0x00 = 0b00000000
173         //     Low_ODR = 0 (low speed ODR disabled)
174         writeReg(LOW_ODR, 0x00);
175     }
176   
177     // 0x00 = 0b00000000
178     //     FS = 00 (+/- 250 dps full scale)
179     writeReg(CTRL_REG4, 0x00);
180   
181     // 0x6F = 0b01101111
182     //     DR = 01 (200 Hz ODR);
183     //     BW = 10 (50 Hz bandwidth);
184     //     PD = 1 (normal mode);
185     //     Zen = Yen = Xen = 1 (all axes enabled)
186     writeReg(CTRL_REG1, 0x6F);
189 /*
190  *  ======== writeReg ========
191  *  Writes a gyro register
192  */
193 void L3G::writeReg(byte reg, byte value)
195     Wire.beginTransmission(address);
196     Wire.write(reg);
197     Wire.write(value);
198     last_status = Wire.endTransmission();
201 /*
202  *  ======== readReg ========
203  *  Reads a gyro register
204  */
205 byte L3G::readReg(byte reg)
207     byte value;
209     Wire.beginTransmission(address);
210     Wire.write(reg);
211     last_status = Wire.endTransmission();
212     Wire.requestFrom(address, (byte)1);
213     value = Wire.read();
214     Wire.endTransmission();
216     return value;
219 /*
220  *  ======== read ========
221  *  Reads the 3 gyro channels and stores them in vector g
222  */
223 void L3G::read()
225     Wire.beginTransmission(address);
226     // assert the MSB of the address to get the gyro
227     // to do slave-transmit subaddress updating.
228     Wire.write(OUT_X_L | (1 << 7));
229     Wire.endTransmission();
230     Wire.requestFrom(address, (byte)6);
231   
232     unsigned int millis_start = millis();
233     while (Wire.available() < 6) {
234         if (io_timeout > 0 && ((unsigned int)millis() - millis_start) > io_timeout) {
235             did_timeout = true;
236             return;
237         }
238     }
240     uint8_t xlg = Wire.read();
241     uint8_t xhg = Wire.read();
242     uint8_t ylg = Wire.read();
243     uint8_t yhg = Wire.read();
244     uint8_t zlg = Wire.read();
245     uint8_t zhg = Wire.read();
247     // combine high and low bytes
248     g.x = (int16_t)(xhg << 8 | xlg);
249     g.y = (int16_t)(yhg << 8 | ylg);
250     g.z = (int16_t)(zhg << 8 | zlg);
253 /*
254  *  ======== vector_normalize ========
255  */
256 void L3G::vector_normalize(vector<float> *a)
258     float mag = sqrt(vector_dot(a,a));
259     a->x /= mag;
260     a->y /= mag;
261     a->z /= mag;
264 /*
265  *  ======== testReg ========
266  */
267 static int testReg(byte address, L3G::regAddr reg)
269     Wire.beginTransmission(address);
270     Wire.write((byte)reg);
271     if (Wire.endTransmission() != 0) {
272         return TEST_REG_ERROR;
273     }
275     Wire.requestFrom(address, (byte)1);
276     if (Wire.available()) {
277         return Wire.read();
278     }
279     else {
280         return TEST_REG_ERROR;
281     }