-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathATT_LoRaWAN_LSM303.h
294 lines (241 loc) · 9.78 KB
/
ATT_LoRaWAN_LSM303.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
//Copyright(c) 2013 Pololu Corporation.For more information, see
//
//http ://www.pololu.com/
//http ://forum.pololu.com/
//
// Permission is hereby granted, free of charge, to any person
// obtaining a copy of this software and associated documentation
// files(the "Software"), to deal in the Software without
// restriction, including without limitation the rights to use,
// copy, modify, merge, publish, distribute, sublicense, and / or sell
// copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following
// conditions :
//
// The above copyright notice and this permission notice shall be
// included in all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
// OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
// NONINFRINGEMENT.IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
// WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
// OTHER DEALINGS IN THE SOFTWARE.
// for more info on sensor: http://www.st.com/content/ccc/resource/technical/document/datasheet/1c/9e/71/05/4e/b7/4d/d1/DM00057547.pdf/files/DM00057547.pdf/jcr:content/translations/en.DM00057547.pdf
#ifndef LSM303_h
#define LSM303_h
#include <Arduino.h> // for byte data type
class LSM303
{
public:
template <typename T> struct vector
{
T x, y, z;
};
enum deviceType { device_DLH, device_DLM, device_DLHC, device_D, device_auto };
enum sa0State { sa0_low, sa0_high, sa0_auto };
// register addresses
enum regAddr
{
TEMP_OUT_L = 0x05, // D
TEMP_OUT_H = 0x06, // D
STATUS_M = 0x07, // D
INT_CTRL_M = 0x12, // D
INT_SRC_M = 0x13, // D
INT_THS_L_M = 0x14, // D
INT_THS_H_M = 0x15, // D
OFFSET_X_L_M = 0x16, // D
OFFSET_X_H_M = 0x17, // D
OFFSET_Y_L_M = 0x18, // D
OFFSET_Y_H_M = 0x19, // D
OFFSET_Z_L_M = 0x1A, // D
OFFSET_Z_H_M = 0x1B, // D
REFERENCE_X = 0x1C, // D
REFERENCE_Y = 0x1D, // D
REFERENCE_Z = 0x1E, // D
CTRL0 = 0x1F, // D
CTRL1 = 0x20, // D
CTRL_REG1_A = 0x20, // DLH, DLM, DLHC
CTRL2 = 0x21, // D
CTRL_REG2_A = 0x21, // DLH, DLM, DLHC
CTRL3 = 0x22, // D
CTRL_REG3_A = 0x22, // DLH, DLM, DLHC
CTRL4 = 0x23, // D
CTRL_REG4_A = 0x23, // DLH, DLM, DLHC
CTRL5 = 0x24, // D
CTRL_REG5_A = 0x24, // DLH, DLM, DLHC
CTRL6 = 0x25, // D
CTRL_REG6_A = 0x25, // DLHC
HP_FILTER_RESET_A = 0x25, // DLH, DLM
CTRL7 = 0x26, // D
REFERENCE_A = 0x26, // DLH, DLM, DLHC
STATUS_A = 0x27, // D
STATUS_REG_A = 0x27, // DLH, DLM, DLHC
OUT_X_L_A = 0x28,
OUT_X_H_A = 0x29,
OUT_Y_L_A = 0x2A,
OUT_Y_H_A = 0x2B,
OUT_Z_L_A = 0x2C,
OUT_Z_H_A = 0x2D,
FIFO_CTRL = 0x2E, // D
FIFO_CTRL_REG_A = 0x2E, // DLHC
FIFO_SRC = 0x2F, // D
FIFO_SRC_REG_A = 0x2F, // DLHC
IG_CFG1 = 0x30, // D
INT1_CFG_A = 0x30, // DLH, DLM, DLHC
IG_SRC1 = 0x31, // D
INT1_SRC_A = 0x31, // DLH, DLM, DLHC
IG_THS1 = 0x32, // D
INT1_THS_A = 0x32, // DLH, DLM, DLHC
IG_DUR1 = 0x33, // D
INT1_DURATION_A = 0x33, // DLH, DLM, DLHC
IG_CFG2 = 0x34, // D
INT2_CFG_A = 0x34, // DLH, DLM, DLHC
IG_SRC2 = 0x35, // D
INT2_SRC_A = 0x35, // DLH, DLM, DLHC
IG_THS2 = 0x36, // D
INT2_THS_A = 0x36, // DLH, DLM, DLHC
IG_DUR2 = 0x37, // D
INT2_DURATION_A = 0x37, // DLH, DLM, DLHC
CLICK_CFG = 0x38, // D
CLICK_CFG_A = 0x38, // DLHC
CLICK_SRC = 0x39, // D
CLICK_SRC_A = 0x39, // DLHC
CLICK_THS = 0x3A, // D
CLICK_THS_A = 0x3A, // DLHC
TIME_LIMIT = 0x3B, // D
TIME_LIMIT_A = 0x3B, // DLHC
TIME_LATENCY = 0x3C, // D
TIME_LATENCY_A = 0x3C, // DLHC
TIME_WINDOW = 0x3D, // D
TIME_WINDOW_A = 0x3D, // DLHC
Act_THS = 0x3E, // D
Act_DUR = 0x3F, // D
CRA_REG_M = 0x00, // DLH, DLM, DLHC
CRB_REG_M = 0x01, // DLH, DLM, DLHC
MR_REG_M = 0x02, // DLH, DLM, DLHC
SR_REG_M = 0x09, // DLH, DLM, DLHC
IRA_REG_M = 0x0A, // DLH, DLM, DLHC
IRB_REG_M = 0x0B, // DLH, DLM, DLHC
IRC_REG_M = 0x0C, // DLH, DLM, DLHC
WHO_AM_I = 0x0F, // D
WHO_AM_I_M = 0x0F, // DLM
TEMP_OUT_H_M = 0x31, // DLHC
TEMP_OUT_L_M = 0x32, // DLHC
// dummy addresses for registers in different locations on different devices;
// the library translates these based on device type
// value with sign flipped is used as index into translated_regs array
OUT_X_H_M = -1,
OUT_X_L_M = -2,
OUT_Y_H_M = -3,
OUT_Y_L_M = -4,
OUT_Z_H_M = -5,
OUT_Z_L_M = -6,
// update dummy_reg_count if registers are added here!
// device-specific register addresses
DLH_OUT_X_H_M = 0x03,
DLH_OUT_X_L_M = 0x04,
DLH_OUT_Y_H_M = 0x05,
DLH_OUT_Y_L_M = 0x06,
DLH_OUT_Z_H_M = 0x07,
DLH_OUT_Z_L_M = 0x08,
DLM_OUT_X_H_M = 0x03,
DLM_OUT_X_L_M = 0x04,
DLM_OUT_Z_H_M = 0x05,
DLM_OUT_Z_L_M = 0x06,
DLM_OUT_Y_H_M = 0x07,
DLM_OUT_Y_L_M = 0x08,
DLHC_OUT_X_H_M = 0x03,
DLHC_OUT_X_L_M = 0x04,
DLHC_OUT_Z_H_M = 0x05,
DLHC_OUT_Z_L_M = 0x06,
DLHC_OUT_Y_H_M = 0x07,
DLHC_OUT_Y_L_M = 0x08,
D_OUT_X_L_M = 0x08,
D_OUT_X_H_M = 0x09,
D_OUT_Y_L_M = 0x0A,
D_OUT_Y_H_M = 0x0B,
D_OUT_Z_L_M = 0x0C,
D_OUT_Z_H_M = 0x0D
};
vector<int16_t> a; // accelerometer readings
vector<int16_t> m; // magnetometer readings
vector<int16_t> m_max; // maximum magnetometer values, used for calibration
vector<int16_t> m_min; // minimum magnetometer values, used for calibration
byte last_status; // status of last I2C transmission
LSM303(void);
bool init(deviceType device = device_auto, sa0State sa0 = sa0_auto);
deviceType getDeviceType(void) { return _device; }
void enableDefault(void);
void writeAccReg(byte reg, byte value);
byte readAccReg(byte reg);
void writeMagReg(byte reg, byte value);
byte readMagReg(int reg);
void writeReg(byte reg, byte value);
byte readReg(int reg);
void readAcc(void);
void readMag(void);
void read(void);
void setTimeout(unsigned int timeout);
unsigned int getTimeout(void);
bool timeoutOccurred(void);
float heading(void);
template <typename T> float heading(vector<T> from);
// vector functions
template <typename Ta, typename Tb, typename To> static void vector_cross(const vector<Ta> *a, const vector<Tb> *b, vector<To> *out);
template <typename Ta, typename Tb> static float vector_dot(const vector<Ta> *a, const vector<Tb> *b);
static void vector_normalize(vector<float> *a);
private:
deviceType _device; // chip type (D, DLHC, DLM, or DLH)
byte acc_address;
byte mag_address;
static const int dummy_reg_count = 6;
regAddr translated_regs[dummy_reg_count + 1]; // index 0 not used
unsigned int io_timeout;
bool did_timeout;
int testReg(byte address, regAddr reg);
};
/*
Returns the angular difference in the horizontal plane between the
"from" vector and north, in degrees.
Description of heading algorithm:
Shift and scale the magnetic reading based on calibration data to find
the North vector. Use the acceleration readings to determine the Up
vector (gravity is measured as an upward acceleration). The cross
product of North and Up vectors is East. The vectors East and North
form a basis for the horizontal plane. The From vector is projected
into the horizontal plane and the angle between the projected vector
and horizontal north is returned.
*/
template <typename T> float LSM303::heading(vector<T> from)
{
vector<int32_t> temp_m = {m.x, m.y, m.z};
// subtract offset (average of min and max) from magnetometer readings
temp_m.x -= ((int32_t)m_min.x + m_max.x) / 2;
temp_m.y -= ((int32_t)m_min.y + m_max.y) / 2;
temp_m.z -= ((int32_t)m_min.z + m_max.z) / 2;
// compute E and N
vector<float> E;
vector<float> N;
vector_cross(&temp_m, &a, &E);
vector_normalize(&E);
vector_cross(&a, &E, &N);
vector_normalize(&N);
// compute heading
float heading = atan2(vector_dot(&E, &from), vector_dot(&N, &from)) * 180 / M_PI;
if (heading < 0) heading += 360;
return heading;
}
template <typename Ta, typename Tb, typename To> void LSM303::vector_cross(const vector<Ta> *a, const vector<Tb> *b, vector<To> *out)
{
out->x = (a->y * b->z) - (a->z * b->y);
out->y = (a->z * b->x) - (a->x * b->z);
out->z = (a->x * b->y) - (a->y * b->x);
}
template <typename Ta, typename Tb> float LSM303::vector_dot(const vector<Ta> *a, const vector<Tb> *b)
{
return (a->x * b->x) + (a->y * b->y) + (a->z * b->z);
}
#endif