1 | #ifndef L3G_h
|
---|
2 | #define L3G_h
|
---|
3 |
|
---|
4 | #include <Arduino.h> // for byte data type
|
---|
5 |
|
---|
6 | class L3G
|
---|
7 | {
|
---|
8 | public:
|
---|
9 | template <typename T> struct vector
|
---|
10 | {
|
---|
11 | T x, y, z;
|
---|
12 | };
|
---|
13 |
|
---|
14 | enum deviceType { device_4200D, device_D20, device_D20H, device_auto };
|
---|
15 | enum sa0State { sa0_low, sa0_high, sa0_auto };
|
---|
16 |
|
---|
17 | // register addresses
|
---|
18 | enum regAddr
|
---|
19 | {
|
---|
20 | WHO_AM_I = 0x0F,
|
---|
21 |
|
---|
22 | CTRL1 = 0x20, // D20H
|
---|
23 | CTRL_REG1 = 0x20, // D20, 4200D
|
---|
24 | CTRL2 = 0x21, // D20H
|
---|
25 | CTRL_REG2 = 0x21, // D20, 4200D
|
---|
26 | CTRL3 = 0x22, // D20H
|
---|
27 | CTRL_REG3 = 0x22, // D20, 4200D
|
---|
28 | CTRL4 = 0x23, // D20H
|
---|
29 | CTRL_REG4 = 0x23, // D20, 4200D
|
---|
30 | CTRL5 = 0x24, // D20H
|
---|
31 | CTRL_REG5 = 0x24, // D20, 4200D
|
---|
32 | REFERENCE = 0x25,
|
---|
33 | OUT_TEMP = 0x26,
|
---|
34 | STATUS = 0x27, // D20H
|
---|
35 | STATUS_REG = 0x27, // D20, 4200D
|
---|
36 |
|
---|
37 | OUT_X_L = 0x28,
|
---|
38 | OUT_X_H = 0x29,
|
---|
39 | OUT_Y_L = 0x2A,
|
---|
40 | OUT_Y_H = 0x2B,
|
---|
41 | OUT_Z_L = 0x2C,
|
---|
42 | OUT_Z_H = 0x2D,
|
---|
43 |
|
---|
44 | FIFO_CTRL = 0x2E, // D20H
|
---|
45 | FIFO_CTRL_REG = 0x2E, // D20, 4200D
|
---|
46 | FIFO_SRC = 0x2F, // D20H
|
---|
47 | FIFO_SRC_REG = 0x2F, // D20, 4200D
|
---|
48 |
|
---|
49 | IG_CFG = 0x30, // D20H
|
---|
50 | INT1_CFG = 0x30, // D20, 4200D
|
---|
51 | IG_SRC = 0x31, // D20H
|
---|
52 | INT1_SRC = 0x31, // D20, 4200D
|
---|
53 | IG_THS_XH = 0x32, // D20H
|
---|
54 | INT1_THS_XH = 0x32, // D20, 4200D
|
---|
55 | IG_THS_XL = 0x33, // D20H
|
---|
56 | INT1_THS_XL = 0x33, // D20, 4200D
|
---|
57 | IG_THS_YH = 0x34, // D20H
|
---|
58 | INT1_THS_YH = 0x34, // D20, 4200D
|
---|
59 | IG_THS_YL = 0x35, // D20H
|
---|
60 | INT1_THS_YL = 0x35, // D20, 4200D
|
---|
61 | IG_THS_ZH = 0x36, // D20H
|
---|
62 | INT1_THS_ZH = 0x36, // D20, 4200D
|
---|
63 | IG_THS_ZL = 0x37, // D20H
|
---|
64 | INT1_THS_ZL = 0x37, // D20, 4200D
|
---|
65 | IG_DURATION = 0x38, // D20H
|
---|
66 | INT1_DURATION = 0x38, // D20, 4200D
|
---|
67 |
|
---|
68 | LOW_ODR = 0x39 // D20H
|
---|
69 | };
|
---|
70 |
|
---|
71 | vector<int16_t> g; // gyro angular velocity readings
|
---|
72 |
|
---|
73 | byte last_status; // status of last I2C transmission
|
---|
74 |
|
---|
75 | L3G(void);
|
---|
76 |
|
---|
77 | bool init(deviceType device = device_auto, sa0State sa0 = sa0_auto);
|
---|
78 | deviceType getDeviceType(void) { return _device; }
|
---|
79 |
|
---|
80 | void enableDefault(void);
|
---|
81 |
|
---|
82 | void writeReg(byte reg, byte value);
|
---|
83 | byte readReg(byte reg);
|
---|
84 |
|
---|
85 | void read(void);
|
---|
86 |
|
---|
87 | void setTimeout(unsigned int timeout);
|
---|
88 | unsigned int getTimeout(void);
|
---|
89 | bool timeoutOccurred(void);
|
---|
90 |
|
---|
91 | // vector functions
|
---|
92 | template <typename Ta, typename Tb, typename To> static void vector_cross(const vector<Ta> *a, const vector<Tb> *b, vector<To> *out);
|
---|
93 | template <typename Ta, typename Tb> static float vector_dot(const vector<Ta> *a, const vector<Tb> *b);
|
---|
94 | static void vector_normalize(vector<float> *a);
|
---|
95 |
|
---|
96 | private:
|
---|
97 | deviceType _device; // chip type (D20H, D20, or 4200D)
|
---|
98 | byte address;
|
---|
99 |
|
---|
100 | unsigned int io_timeout;
|
---|
101 | bool did_timeout;
|
---|
102 |
|
---|
103 | int testReg(byte address, regAddr reg);
|
---|
104 | };
|
---|
105 |
|
---|
106 | template <typename Ta, typename Tb, typename To> void L3G::vector_cross(const vector<Ta> *a, const vector<Tb> *b, vector<To> *out)
|
---|
107 | {
|
---|
108 | out->x = (a->y * b->z) - (a->z * b->y);
|
---|
109 | out->y = (a->z * b->x) - (a->x * b->z);
|
---|
110 | out->z = (a->x * b->y) - (a->y * b->x);
|
---|
111 | }
|
---|
112 |
|
---|
113 | template <typename Ta, typename Tb> float L3G::vector_dot(const vector<Ta> *a, const vector<Tb> *b)
|
---|
114 | {
|
---|
115 | return (a->x * b->x) + (a->y * b->y) + (a->z * b->z);
|
---|
116 | }
|
---|
117 |
|
---|
118 | #endif
|
---|
119 |
|
---|
120 |
|
---|
121 |
|
---|