add ability to build and upload sketches from the bash command line (via beta ino2cpp...
[zumo-cc3200/zumo-cc3200.git] / src / Energia / ZumoTest / LSM303.h
1 #ifndef LSM303_h
2 #define LSM303_h
4 #include <Arduino.h> // for byte data type
6 class LSM303
7 {
8   public:
9     template <typename T> struct vector {
10         T x, y, z;
11     };
13     enum deviceType { device_DLH, device_DLM, device_DLHC, device_D, device_auto };
14     enum sa0State { sa0_low, sa0_high, sa0_auto };
16     // register addresses
17     enum regAddr {
18         TEMP_OUT_L        = 0x05, // D
19         TEMP_OUT_H        = 0x06, // D
21         STATUS_M          = 0x07, // D
23         INT_CTRL_M        = 0x12, // D
24         INT_SRC_M         = 0x13, // D
25         INT_THS_L_M       = 0x14, // D
26         INT_THS_H_M       = 0x15, // D
28         OFFSET_X_L_M      = 0x16, // D
29         OFFSET_X_H_M      = 0x17, // D
30         OFFSET_Y_L_M      = 0x18, // D
31         OFFSET_Y_H_M      = 0x19, // D
32         OFFSET_Z_L_M      = 0x1A, // D
33         OFFSET_Z_H_M      = 0x1B, // D
34         REFERENCE_X       = 0x1C, // D
35         REFERENCE_Y       = 0x1D, // D
36         REFERENCE_Z       = 0x1E, // D
38         CTRL0             = 0x1F, // D
39         CTRL1             = 0x20, // D
40         CTRL_REG1_A       = 0x20, // DLH, DLM, DLHC
41         CTRL2             = 0x21, // D
42         CTRL_REG2_A       = 0x21, // DLH, DLM, DLHC
43         CTRL3             = 0x22, // D
44         CTRL_REG3_A       = 0x22, // DLH, DLM, DLHC
45         CTRL4             = 0x23, // D
46         CTRL_REG4_A       = 0x23, // DLH, DLM, DLHC
47         CTRL5             = 0x24, // D
48         CTRL_REG5_A       = 0x24, // DLH, DLM, DLHC
49         CTRL6             = 0x25, // D
50         CTRL_REG6_A       = 0x25, // DLHC
51         HP_FILTER_RESET_A = 0x25, // DLH, DLM
52         CTRL7             = 0x26, // D
53         REFERENCE_A       = 0x26, // DLH, DLM, DLHC
54         STATUS_A          = 0x27, // D
55         STATUS_REG_A      = 0x27, // DLH, DLM, DLHC
57         OUT_X_L_A         = 0x28,
58         OUT_X_H_A         = 0x29,
59         OUT_Y_L_A         = 0x2A,
60         OUT_Y_H_A         = 0x2B,
61         OUT_Z_L_A         = 0x2C,
62         OUT_Z_H_A         = 0x2D,
64         FIFO_CTRL         = 0x2E, // D
65         FIFO_CTRL_REG_A   = 0x2E, // DLHC
66         FIFO_SRC          = 0x2F, // D
67         FIFO_SRC_REG_A    = 0x2F, // DLHC
69         IG_CFG1           = 0x30, // D
70         INT1_CFG_A        = 0x30, // DLH, DLM, DLHC
71         IG_SRC1           = 0x31, // D
72         INT1_SRC_A        = 0x31, // DLH, DLM, DLHC
73         IG_THS1           = 0x32, // D
74         INT1_THS_A        = 0x32, // DLH, DLM, DLHC
75         IG_DUR1           = 0x33, // D
76         INT1_DURATION_A   = 0x33, // DLH, DLM, DLHC
77         IG_CFG2           = 0x34, // D
78         INT2_CFG_A        = 0x34, // DLH, DLM, DLHC
79         IG_SRC2           = 0x35, // D
80         INT2_SRC_A        = 0x35, // DLH, DLM, DLHC
81         IG_THS2           = 0x36, // D
82         INT2_THS_A        = 0x36, // DLH, DLM, DLHC
83         IG_DUR2           = 0x37, // D
84         INT2_DURATION_A   = 0x37, // DLH, DLM, DLHC
86         CLICK_CFG         = 0x38, // D
87         CLICK_CFG_A       = 0x38, // DLHC
88         CLICK_SRC         = 0x39, // D
89         CLICK_SRC_A       = 0x39, // DLHC
90         CLICK_THS         = 0x3A, // D
91         CLICK_THS_A       = 0x3A, // DLHC
92         TIME_LIMIT        = 0x3B, // D
93         TIME_LIMIT_A      = 0x3B, // DLHC
94         TIME_LATENCY      = 0x3C, // D
95         TIME_LATENCY_A    = 0x3C, // DLHC
96         TIME_WINDOW       = 0x3D, // D
97         TIME_WINDOW_A     = 0x3D, // DLHC
99         Act_THS           = 0x3E, // D
100         Act_DUR           = 0x3F, // D
102         CRA_REG_M         = 0x00, // DLH, DLM, DLHC
103         CRB_REG_M         = 0x01, // DLH, DLM, DLHC
104         MR_REG_M          = 0x02, // DLH, DLM, DLHC
106         SR_REG_M          = 0x09, // DLH, DLM, DLHC
107         IRA_REG_M         = 0x0A, // DLH, DLM, DLHC
108         IRB_REG_M         = 0x0B, // DLH, DLM, DLHC
109         IRC_REG_M         = 0x0C, // DLH, DLM, DLHC
111         WHO_AM_I          = 0x0F, // D
112         WHO_AM_I_M        = 0x0F, // DLM
114         TEMP_OUT_H_M      = 0x31, // DLHC
115         TEMP_OUT_L_M      = 0x32, // DLHC
117         // dummy addresses for registers in different locations on different
118         // devices; the library translates these based on device type value
119         // with sign flipped is used as index into translated_regs array
121         OUT_X_H_M         = -1,
122         OUT_X_L_M         = -2,
123         OUT_Y_H_M         = -3,
124         OUT_Y_L_M         = -4,
125         OUT_Z_H_M         = -5,
126         OUT_Z_L_M         = -6,
127         // update dummy_reg_count if registers are added here!
129         // device-specific register addresses
130         DLH_OUT_X_H_M     = 0x03,
131         DLH_OUT_X_L_M     = 0x04,
132         DLH_OUT_Y_H_M     = 0x05,
133         DLH_OUT_Y_L_M     = 0x06,
134         DLH_OUT_Z_H_M     = 0x07,
135         DLH_OUT_Z_L_M     = 0x08,
137         DLM_OUT_X_H_M     = 0x03,
138         DLM_OUT_X_L_M     = 0x04,
139         DLM_OUT_Z_H_M     = 0x05,
140         DLM_OUT_Z_L_M     = 0x06,
141         DLM_OUT_Y_H_M     = 0x07,
142         DLM_OUT_Y_L_M     = 0x08,
144         DLHC_OUT_X_H_M    = 0x03,
145         DLHC_OUT_X_L_M    = 0x04,
146         DLHC_OUT_Z_H_M    = 0x05,
147         DLHC_OUT_Z_L_M    = 0x06,
148         DLHC_OUT_Y_H_M    = 0x07,
149         DLHC_OUT_Y_L_M    = 0x08,
151         D_OUT_X_L_M       = 0x08,
152         D_OUT_X_H_M       = 0x09,
153         D_OUT_Y_L_M       = 0x0A,
154         D_OUT_Y_H_M       = 0x0B,
155         D_OUT_Z_L_M       = 0x0C,
156         D_OUT_Z_H_M       = 0x0D
157     };
159     vector<int16_t> a;     // accelerometer readings
160     vector<int16_t> m;     // magnetometer readings
161     vector<int16_t> m_max; // maximum magnetometer values, used for calibration
162     vector<int16_t> m_min; // minimum magnetometer values, used for calibration
164     /*
165      *  ======== last_status ========
166      */
167     byte last_status;      // status of last I2C transmission
169     /*
170      *  ======== LSM303 ========
171      */
172     LSM303(void);
174     /*
175      *  ======== init ========
176      */
177     bool init(deviceType device = device_auto, sa0State sa0 = sa0_auto);
179     /*
180      *  ======== getDeviceType ========
181      */
182     deviceType getDeviceType(void) { return _device; }
184     /*
185      *  ======== enableDefault ========
186      */
187     void enableDefault(void);
189     /*
190      *  ======== writeAccReg ========
191      */
192     void writeAccReg(byte reg, byte value);
194     /*
195      *  ======== readAccReg ========
196      */
197     byte readAccReg(byte reg);
199     /*
200      *  ======== writeMagReg ========
201      */
202     void writeMagReg(byte reg, byte value);
204     /*
205      *  ======== readMagReg ========
206      */
207     byte readMagReg(int reg);
209     /*
210      *  ======== writeReg ========
211      */
212     void writeReg(byte reg, byte value);
214     /*
215      *  ======== readReg ========
216      */
217     byte readReg(int reg);
219     /*
220      *  ======== readAcc ========
221      */
222     void readAcc(void);
224     /*
225      *  ======== readMag ========
226      */
227     void readMag(void);
229     /*
230      *  ======== read ========
231      */
232     void read(void);
234     /*
235      *  ======== setTimeout ========
236      */
237     void setTimeout(unsigned int timeout);
239     /*
240      *  ======== getTimeout ========
241      */
242     unsigned int getTimeout(void);
244     /*
245      *  ======== timeoutOccurred ========
246      */
247     bool timeoutOccurred(void);
249     /*
250      *  ======== heading ========
251      */
252     float heading(void);
254     template <typename T> float heading(vector<T> from);
256     /*
257      *  ======== vector_cross ========
258      */
259     template <typename Ta, typename Tb, typename To>
260         static void vector_cross(
261             const vector<Ta> *a, const vector<Tb> *b, vector<To> *out);
263     /*
264      *  ======== vector_dot ========
265      */
266     template <typename Ta, typename Tb>
267         static float vector_dot(const vector<Ta> *a, const vector<Tb> *b);
269     /*
270      *  ======== vector_normalize ========
271      */
272     static void vector_normalize(vector<float> *a);
274   private:
275     deviceType _device; // chip type (D, DLHC, DLM, or DLH)
276     byte acc_address;
277     byte mag_address;
279     static const int dummy_reg_count = 6;
280     regAddr translated_regs[dummy_reg_count + 1]; // index 0 not used
282     unsigned int io_timeout;
283     bool did_timeout;
284 };
286 /*
287  *  ======== heading ========
288  *  Returns the angular difference in the horizontal plane between the
289  *  "from" vector and north, in degrees.
290  *
291  *  Description of heading algorithm:
292  *    Shift and scale the magnetic reading based on calibration data to find
293  *    the North vector. Use the acceleration readings to determine the Up
294  *    vector (gravity is measured as an upward acceleration). The cross
295  *    product of North and Up vectors is East. The vectors East and North
296  *    form a basis for the horizontal plane. The From vector is projected
297  *    into the horizontal plane and the angle between the projected vector
298  *    and horizontal north is returned.
299  */
300 template <typename T> float LSM303::heading(vector<T> from)
302     vector<int32_t> temp_m = {m.x, m.y, m.z};
304     /* subtract offset (average of min and max) from magnetometer readings */
305     temp_m.x -= ((int32_t)m_min.x + m_max.x) / 2;
306     temp_m.y -= ((int32_t)m_min.y + m_max.y) / 2;
307     temp_m.z -= ((int32_t)m_min.z + m_max.z) / 2;
309     /* compute E and N */
310     vector<float> E;
311     vector<float> N;
313     vector_cross(&temp_m, &a, &E);
314     vector_normalize(&E);
315     vector_cross(&a, &E, &N);
316     vector_normalize(&N);
318     /* compute heading */
319     float heading = atan2(vector_dot(&E, &from), vector_dot(&N, &from)) * 180 / M_PI;
320     if (heading < 0) {
321         heading += 360;
322     }
323     return heading;
326 /*
327  *  ======== vector_cross ========
328  *  To = Ta x Tb
329  */
330 template <typename Ta, typename Tb, typename To>
331     void LSM303::vector_cross(
332         const vector<Ta> *a, const vector<Tb> *b, vector<To> *out)
334     out->x = (a->y * b->z) - (a->z * b->y);
335     out->y = (a->z * b->x) - (a->x * b->z);
336     out->z = (a->x * b->y) - (a->y * b->x);
339 /*
340  *  ======== vector_dot ========
341  *  Returns Ta * Tb
342  */
343 template <typename Ta, typename Tb>
344     float LSM303::vector_dot(const vector<Ta> *a, const vector<Tb> *b)
346     return (a->x * b->x) + (a->y * b->y) + (a->z * b->z);
349 #endif