Subversion Repositories FlightCtrl

Rev

Go to most recent revision | Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1993 - 1
/****************************************************************************
2
* ITG3200.h - ITG-3200/I2C library v0.5 for Arduino                         *
3
* Copyright 2010-2011 Filipe Vieira & various contributors                  *
4
* http://code.google.com/p/itg-3200driver                                   *
5
* This file is part of ITG-3200 Arduino library.                            *
6
*                                                                           *
7
* This library is free software: you can redistribute it and/or modify      *
8
* it under the terms of the GNU Lesser General Public License as published  *
9
* by the Free Software Foundation, either version 3 of the License, or      *
10
* (at your option) any later version.                                       *
11
*                                                                           *
12
* This program is distributed in the hope that it will be useful,           *
13
* but WITHOUT ANY WARRANTY; without even the implied warranty of            *
14
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             *
15
* GNU Lesser General Public License for more details.                       *
16
*                                                                           *
17
* You should have received a copy of the GNU Lesser General Public License  *
18
* along with this program.  If not, see <http://www.gnu.org/licenses/>.     *
19
****************************************************************************/
20
/****************************************************************************
21
* Tested on Arduino Mega with ITG-3200 Breakout                             *
22
* SCL     -> pin 21     (no pull up resistors)                              *
23
* SDA     -> pin 20     (no pull up resistors)                              *
24
* CLK & GND -> pin GND                                                      *
25
* INT       -> not connected  (but can be used)                             *
26
* VIO & VDD -> pin 3.3V                                                     *
27
*****************************************************************************/
28
#include "ITG3200.h"
29
#include <Wire.h>
30
 
31
 
32
ITG3200::ITG3200() {
33
  setGains(1.0,1.0,1.0);
34
  setOffsets(0.0,0.0,0.0);
35
  setRevPolarity(0,0,0);
36
  //Wire.begin();       //Normally this code is called from setup() at user code
37
                        //but some people reported that joining I2C bus earlier
38
                        //apparently solved problems with master/slave conditions.
39
                        //Uncomment if needed.
40
}
41
 
42
void ITG3200::init(unsigned int  address) {
43
  // Uncomment or change your default ITG3200 initialization
44
 
45
  // fast sample rate - divisor = 0 filter = 0 clocksrc = 0, 1, 2, or 3  (raw values)
46
  init(address, NOSRDIVIDER, RANGE2000, BW256_SR8, PLL_XGYRO_REF, true, true);
47
 
48
  // slow sample rate - divisor = 0  filter = 1,2,3,4,5, or 6  clocksrc = 0, 1, 2, or 3  (raw values)
49
  //init(NOSRDIVIDER, RANGE2000, BW010_SR1, INTERNALOSC, true, true);
50
 
51
  // fast sample rate 32Khz external clock - divisor = 0  filter = 0  clocksrc = 4  (raw values)
52
  //init(NOSRDIVIDER, RANGE2000, BW256_SR8, PLL_EXTERNAL32, true, true);
53
 
54
  // slow sample rate 32Khz external clock - divisor = 0  filter = 1,2,3,4,5, or 6  clocksrc = 4  (raw values)
55
  //init(NOSRDIVIDER, RANGE2000, BW010_SR1, PLL_EXTERNAL32, true, true);
56
}
57
 
58
void ITG3200::init(unsigned int address, byte _SRateDiv, byte _Range, byte _filterBW, byte _ClockSrc, bool _ITGReady, bool _INTRawDataReady) {
59
  _dev_address = address;
60
  setSampleRateDiv(_SRateDiv);
61
  setFSRange(_Range);
62
  setFilterBW(_filterBW);
63
  setClockSource(_ClockSrc);
64
  setITGReady(_ITGReady);
65
  setRawDataReady(_INTRawDataReady);  
66
  delay(GYROSTART_UP_DELAY);  // startup 
67
}
68
 
69
byte ITG3200::getDevAddr() {
70
  /*readmem(WHO_AM_I, 1, &_buff[0]);
71
  return _buff[0];  */
72
  return _dev_address;
73
}
74
 
75
void ITG3200::setDevAddr(unsigned int  _addr) {
76
  writemem(WHO_AM_I, _addr);
77
  _dev_address = _addr;
78
}
79
 
80
byte ITG3200::getSampleRateDiv() {
81
  readmem(SMPLRT_DIV, 1, &_buff[0]);
82
  return _buff[0];
83
}
84
 
85
void ITG3200::setSampleRateDiv(byte _SampleRate) {
86
  writemem(SMPLRT_DIV, _SampleRate);
87
}
88
 
89
byte ITG3200::getFSRange() {
90
  readmem(DLPF_FS, 1, &_buff[0]);
91
  return ((_buff[0] & DLPFFS_FS_SEL) >> 3);
92
}
93
 
94
void ITG3200::setFSRange(byte _Range) {
95
  readmem(DLPF_FS, 1, &_buff[0]);  
96
  writemem(DLPF_FS, ((_buff[0] & ~DLPFFS_FS_SEL) | (_Range << 3)) );
97
}
98
 
99
byte ITG3200::getFilterBW() {  
100
  readmem(DLPF_FS, 1, &_buff[0]);
101
  return (_buff[0] & DLPFFS_DLPF_CFG);
102
}
103
 
104
void ITG3200::setFilterBW(byte _BW) {  
105
  readmem(DLPF_FS, 1, &_buff[0]);
106
  writemem(DLPF_FS, ((_buff[0] & ~DLPFFS_DLPF_CFG) | _BW));
107
}
108
 
109
bool ITG3200::isINTActiveOnLow() {  
110
  readmem(INT_CFG, 1, &_buff[0]);
111
  return ((_buff[0] & INTCFG_ACTL) >> 7);
112
}
113
 
114
void ITG3200::setINTLogiclvl(bool _State) {
115
  readmem(INT_CFG, 1, &_buff[0]);
116
  writemem(INT_CFG, ((_buff[0] & ~INTCFG_ACTL) | (_State << 7)));
117
}
118
 
119
bool ITG3200::isINTOpenDrain() {  
120
  readmem(INT_CFG, 1, &_buff[0]);
121
  return ((_buff[0] & INTCFG_OPEN) >> 6);
122
}
123
 
124
void ITG3200::setINTDriveType(bool _State) {
125
  readmem(INT_CFG, 1, &_buff[0]);
126
  writemem(INT_CFG, ((_buff[0] & ~INTCFG_OPEN) | _State << 6));
127
}
128
 
129
bool ITG3200::isLatchUntilCleared() {    
130
  readmem(INT_CFG, 1, &_buff[0]);
131
  return ((_buff[0] & INTCFG_LATCH_INT_EN) >> 5);
132
}
133
 
134
void ITG3200::setLatchMode(bool _State) {
135
  readmem(INT_CFG, 1, &_buff[0]);
136
  writemem(INT_CFG, ((_buff[0] & ~INTCFG_LATCH_INT_EN) | _State << 5));
137
}
138
 
139
bool ITG3200::isAnyRegClrMode() {    
140
  readmem(INT_CFG, 1, &_buff[0]);
141
  return ((_buff[0] & INTCFG_INT_ANYRD_2CLEAR) >> 4);
142
}
143
 
144
void ITG3200::setLatchClearMode(bool _State) {
145
  readmem(INT_CFG, 1, &_buff[0]);
146
  writemem(INT_CFG, ((_buff[0] & ~INTCFG_INT_ANYRD_2CLEAR) | _State << 4));
147
}
148
 
149
bool ITG3200::isITGReadyOn() {  
150
  readmem(INT_CFG, 1, &_buff[0]);
151
  return ((_buff[0] & INTCFG_ITG_RDY_EN) >> 2);
152
}
153
 
154
void ITG3200::setITGReady(bool _State) {
155
  readmem(INT_CFG, 1, &_buff[0]);
156
  writemem(INT_CFG, ((_buff[0] & ~INTCFG_ITG_RDY_EN) | _State << 2));
157
}
158
 
159
bool ITG3200::isRawDataReadyOn() {
160
  readmem(INT_CFG, 1, &_buff[0]);
161
  return (_buff[0] & INTCFG_RAW_RDY_EN);
162
}
163
 
164
void ITG3200::setRawDataReady(bool _State) {
165
  readmem(INT_CFG, 1, &_buff[0]);
166
  writemem(INT_CFG, ((_buff[0] & ~INTCFG_RAW_RDY_EN) | _State));
167
}
168
 
169
bool ITG3200::isITGReady() {
170
  readmem(INT_STATUS, 1, &_buff[0]);
171
  return ((_buff[0] & INTSTATUS_ITG_RDY) >> 2);
172
}
173
 
174
bool ITG3200::isRawDataReady() {
175
  readmem(INT_STATUS, 1, &_buff[0]);
176
  return (_buff[0] & INTSTATUS_RAW_DATA_RDY);
177
}
178
 
179
void ITG3200::readTemp(float *_Temp) {
180
  readmem(TEMP_OUT,2,_buff);
181
  *_Temp = 35 + (((_buff[0] << 8) | _buff[1]) + 13200) / 280.0;    // F=C*9/5+32  
182
}
183
 
184
void ITG3200::readGyroRaw(int *_GyroX, int *_GyroY, int *_GyroZ){
185
  readmem(GYRO_XOUT, 6, _buff);
186
  *_GyroX = ((_buff[0] << 8) | _buff[1]);
187
  *_GyroY = ((_buff[2] << 8) | _buff[3]);
188
  *_GyroZ = ((_buff[4] << 8) | _buff[5]);
189
}
190
 
191
void ITG3200::readGyroRaw(int *_GyroXYZ){
192
  readGyroRaw(_GyroXYZ, _GyroXYZ+1, _GyroXYZ+2);
193
}
194
 
195
void ITG3200::setRevPolarity(bool _Xpol, bool _Ypol, bool _Zpol) {
196
  polarities[0] = _Xpol ? -1 : 1;
197
  polarities[1] = _Ypol ? -1 : 1;
198
  polarities[2] = _Zpol ? -1 : 1;
199
}
200
 
201
void ITG3200::setGains(float _Xgain, float _Ygain, float _Zgain) {
202
  gains[0] = _Xgain;
203
  gains[1] = _Ygain;
204
  gains[2] = _Zgain;
205
}
206
 
207
void ITG3200::setOffsets(int _Xoffset, int _Yoffset, int _Zoffset) {
208
  offsets[0] = _Xoffset;
209
  offsets[1] = _Yoffset;
210
  offsets[2] = _Zoffset;
211
}
212
 
213
void ITG3200::zeroCalibrate(unsigned int totSamples, unsigned int sampleDelayMS) {
214
  int xyz[3];
215
  float tmpOffsets[] = {0,0,0};
216
 
217
  for (int i = 0;i < totSamples;i++){
218
    delay(sampleDelayMS);
219
    readGyroRaw(xyz);
220
    tmpOffsets[0] += xyz[0];
221
    tmpOffsets[1] += xyz[1];
222
    tmpOffsets[2] += xyz[2];  
223
  }
224
  setOffsets(-tmpOffsets[0] / totSamples, -tmpOffsets[1] / totSamples, -tmpOffsets[2] / totSamples);
225
}
226
 
227
void ITG3200::readGyroRawCal(int *_GyroX, int *_GyroY, int *_GyroZ) {
228
  readGyroRaw(_GyroX, _GyroY, _GyroZ);
229
  *_GyroX += offsets[0];
230
  *_GyroY += offsets[1];
231
  *_GyroZ += offsets[2];
232
}
233
 
234
void ITG3200::readGyroRawCal(int *_GyroXYZ) {
235
  readGyroRawCal(_GyroXYZ, _GyroXYZ+1, _GyroXYZ+2);
236
}
237
 
238
void ITG3200::readGyro(float *_GyroX, float *_GyroY, float *_GyroZ){
239
  int x, y, z;
240
 
241
  readGyroRawCal(&x, &y, &z); // x,y,z will contain calibrated integer values from the sensor
242
  *_GyroX =  x / 14.375 * polarities[0] * gains[0];
243
  *_GyroY =  y / 14.375 * polarities[1] * gains[1];
244
  *_GyroZ =  z / 14.375 * polarities[2] * gains[2];
245
}
246
 
247
void ITG3200::readGyro(float *_GyroXYZ){
248
  readGyro(_GyroXYZ, _GyroXYZ+1, _GyroXYZ+2);
249
}
250
 
251
void ITG3200::reset() {    
252
  writemem(PWR_MGM, PWRMGM_HRESET);
253
  delay(GYROSTART_UP_DELAY); //gyro startup 
254
}
255
 
256
bool ITG3200::isLowPower() {  
257
  readmem(PWR_MGM, 1, &_buff[0]);
258
  return (_buff[0] & PWRMGM_SLEEP) >> 6;
259
}
260
 
261
void ITG3200::setPowerMode(bool _State) {
262
  readmem(PWR_MGM, 1, &_buff[0]);
263
  writemem(PWR_MGM, ((_buff[0] & ~PWRMGM_SLEEP) | _State << 6));  
264
}
265
 
266
bool ITG3200::isXgyroStandby() {
267
  readmem(PWR_MGM, 1, &_buff[0]);
268
  return (_buff[0] & PWRMGM_STBY_XG) >> 5;
269
}
270
 
271
bool ITG3200::isYgyroStandby() {
272
  readmem(PWR_MGM, 1, &_buff[0]);
273
  return (_buff[0] & PWRMGM_STBY_YG) >> 4;
274
}
275
 
276
bool ITG3200::isZgyroStandby() {
277
  readmem(PWR_MGM, 1, &_buff[0]);
278
  return (_buff[0] & PWRMGM_STBY_ZG) >> 3;
279
}
280
 
281
void ITG3200::setXgyroStandby(bool _Status) {
282
  readmem(PWR_MGM, 1, &_buff[0]);
283
  writemem(PWR_MGM, ((_buff[0] & PWRMGM_STBY_XG) | _Status << 5));
284
}
285
 
286
void ITG3200::setYgyroStandby(bool _Status) {
287
  readmem(PWR_MGM, 1, &_buff[0]);
288
  writemem(PWR_MGM, ((_buff[0] & PWRMGM_STBY_YG) | _Status << 4));
289
}
290
 
291
void ITG3200::setZgyroStandby(bool _Status) {
292
  readmem(PWR_MGM, 1, &_buff[0]);
293
  writemem(PWR_MGM, ((_buff[0] & PWRMGM_STBY_ZG) | _Status << 3));
294
}
295
 
296
byte ITG3200::getClockSource() {  
297
  readmem(PWR_MGM, 1, &_buff[0]);
298
  return (_buff[0] & PWRMGM_CLK_SEL);
299
}
300
 
301
void ITG3200::setClockSource(byte _CLKsource) {  
302
  readmem(PWR_MGM, 1, &_buff[0]);
303
  writemem(PWR_MGM, ((_buff[0] & ~PWRMGM_CLK_SEL) | _CLKsource));
304
}
305
 
306
void ITG3200::writemem(uint8_t _addr, uint8_t _val) {
307
  Wire.beginTransmission(_dev_address);   // start transmission to device 
308
  Wire.send(_addr); // send register address
309
  Wire.send(_val); // send value to write
310
  Wire.endTransmission(); // end transmission
311
}
312
 
313
void ITG3200::readmem(uint8_t _addr, uint8_t _nbytes, uint8_t __buff[]) {
314
  Wire.beginTransmission(_dev_address); // start transmission to device 
315
  Wire.send(_addr); // sends register address to read from
316
  Wire.endTransmission(); // end transmission
317
 
318
  Wire.beginTransmission(_dev_address); // start transmission to device 
319
  Wire.requestFrom(_dev_address, _nbytes);// send data n-bytes read
320
  uint8_t i = 0;
321
  while (Wire.available()) {
322
    __buff[i] = Wire.receive(); // receive DATA
323
    i++;
324
  }
325
  Wire.endTransmission(); // end transmission
326
}
327
 
328