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
|
/*
MinIMU-9-Arduino-AHRS
Pololu MinIMU-9 + Arduino AHRS (Attitude and Heading Reference System)
Copyright (c) 2011-2016 Pololu Corporation.
http://www.pololu.com/
MinIMU-9-Arduino-AHRS is based on sf9domahrs by Doug Weibel and Jose Julio:
http://code.google.com/p/sf9domahrs/
sf9domahrs is based on ArduIMU v1.5 by Jordi Munoz and William Premerlani, Jose
Julio and Doug Weibel:
http://code.google.com/p/ardu-imu/
MinIMU-9-Arduino-AHRS is free software: you can redistribute it and/or modify it
under the terms of the GNU Lesser General Public License as published by the
Free Software Foundation, either version 3 of the License, or (at your option)
any later version.
MinIMU-9-Arduino-AHRS is distributed in the hope that it will be useful, but
WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for
more details.
You should have received a copy of the GNU Lesser General Public License along
with MinIMU-9-Arduino-AHRS. If not, see <http://www.gnu.org/licenses/>.
*/
#ifdef IMU_V5
#include <LSM6.h>
#include <LIS3MDL.h>
LSM6 gyro_acc;
LIS3MDL mag;
#else // older IMUs through v4
#include <L3G.h>
#include <LSM303.h>
L3G gyro;
LSM303 compass;
#endif
void I2C_Init()
{
Wire.begin();
}
void Gyro_Init()
{
#ifdef IMU_V5
// Accel_Init() should have already called gyro_acc.init() and enableDefault()
gyro_acc.writeReg(LSM6::CTRL2_G, 0x4C); // 104 Hz, 2000 dps full scale
#else
gyro.init();
gyro.enableDefault();
gyro.writeReg(L3G::CTRL_REG4, 0x20); // 2000 dps full scale
gyro.writeReg(L3G::CTRL_REG1, 0x0F); // normal power mode, all axes enabled, 100 Hz
#endif
}
void Read_Gyro()
{
#ifdef IMU_V5
gyro_acc.readGyro();
AN[0] = gyro_acc.g.x;
AN[1] = gyro_acc.g.y;
AN[2] = gyro_acc.g.z;
#else
gyro.read();
AN[0] = gyro.g.x;
AN[1] = gyro.g.y;
AN[2] = gyro.g.z;
#endif
gyro_x = SENSOR_SIGN[0] * (AN[0] - AN_OFFSET[0]);
gyro_y = SENSOR_SIGN[1] * (AN[1] - AN_OFFSET[1]);
gyro_z = SENSOR_SIGN[2] * (AN[2] - AN_OFFSET[2]);
}
void Accel_Init()
{
#ifdef IMU_V5
gyro_acc.init();
gyro_acc.enableDefault();
gyro_acc.writeReg(LSM6::CTRL1_XL, 0x3C); // 52 Hz, 8 g full scale
#else
compass.init();
compass.enableDefault();
switch (compass.getDeviceType())
{
case LSM303::device_D:
compass.writeReg(LSM303::CTRL2, 0x18); // 8 g full scale: AFS = 011
break;
case LSM303::device_DLHC:
compass.writeReg(LSM303::CTRL_REG4_A, 0x28); // 8 g full scale: FS = 10; high resolution output mode
break;
default: // DLM, DLH
compass.writeReg(LSM303::CTRL_REG4_A, 0x30); // 8 g full scale: FS = 11
}
#endif
}
// Reads x,y and z accelerometer registers
void Read_Accel()
{
#ifdef IMU_V5
gyro_acc.readAcc();
AN[3] = gyro_acc.a.x >> 4; // shift right 4 bits to use 12-bit representation (1 g = 256)
AN[4] = gyro_acc.a.y >> 4;
AN[5] = gyro_acc.a.z >> 4;
#else
compass.readAcc();
AN[3] = compass.a.x >> 4; // shift right 4 bits to use 12-bit representation (1 g = 256)
AN[4] = compass.a.y >> 4;
AN[5] = compass.a.z >> 4;
#endif
accel_x = SENSOR_SIGN[3] * (AN[3] - AN_OFFSET[3]);
accel_y = SENSOR_SIGN[4] * (AN[4] - AN_OFFSET[4]);
accel_z = SENSOR_SIGN[5] * (AN[5] - AN_OFFSET[5]);
}
void Compass_Init()
{
#ifdef IMU_V5
mag.init();
mag.enableDefault();
#else
// LSM303: doesn't need to do anything because Accel_Init() should have already called compass.enableDefault()
#endif
}
void Read_Compass()
{
#ifdef IMU_V5
mag.read();
magnetom_x = SENSOR_SIGN[6] * mag.m.x;
magnetom_y = SENSOR_SIGN[7] * mag.m.y;
magnetom_z = SENSOR_SIGN[8] * mag.m.z;
#else
compass.readMag();
magnetom_x = SENSOR_SIGN[6] * compass.m.x;
magnetom_y = SENSOR_SIGN[7] * compass.m.y;
magnetom_z = SENSOR_SIGN[8] * compass.m.z;
#endif
}
|
