[232] | 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 |
|
---|