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