source: rtos_arduino/trunk/arduino_lib/libraries/ZumoShield/LSM303.h@ 232

Last change on this file since 232 was 232, checked in by ertl-honda, 8 years ago

Zumo用ライブラリの追加
File size: 8.5 KB
Line 
1#ifndef LSM303_h
2#define LSM303_h
3
4#include <Arduino.h> // for byte data type
5
6class 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/*
216Returns the angular difference in the horizontal plane between the
217"from" vector and north, in degrees.
218
219Description of heading algorithm:
220Shift and scale the magnetic reading based on calibration data to find
221the North vector. Use the acceleration readings to determine the Up
222vector (gravity is measured as an upward acceleration). The cross
223product of North and Up vectors is East. The vectors East and North
224form a basis for the horizontal plane. The From vector is projected
225into the horizontal plane and the angle between the projected vector
226and horizontal north is returned.
227*/
228template <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
251template <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
258template <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
Note: See TracBrowser for help on using the repository browser.