James Watanabe / ITG3200

Forked from Aaron Berk's ITG3200 driver class library, customized for my specific application using 9DoF-Stick by Sparkfun.

Dependents:   HARP

Fork of ITG3200 by Aaron Berk

ITG-3200 is triple axis, digital interface, gyro sensor.

This library is forked from Aaron Berk's work.

This library is for specific application using 9DoF-Stick.

Datasheet:

http://invensense.com/mems/gyro/documents/PS-ITG-3200-00-01.4.pdf

This library has a feature to correct thermal drift of the device. For details, see Thermal Drift.

ITG-3200は3軸のデジタルインターフェースを備えたジャイロセンサです。

このライブラリは 9DoF-Stick を使用した特定の企画のために保守しています。

mbed IDEが日本語をサポートするまでは英語でコメントを書いていきますが、サポートした後もきっと英語で書いていくでしょう。

このライブラリはデバイスの熱ドリフトを補正する機能を持っています。詳しくは Thermal Drift。

ITG3200.cpp@6:a7ad6046824c, 2012-09-13 (annotated)

Committer:
gltest26
Date:
Thu Sep 13 14:36:13 2012 +0000
Revision:
6:a7ad6046824c
Parent:
5:0a0315f0f34e
Child:
7:43b936a53b64
Made ITG3200::calibrate precisely measures time for calibration. Also added calibSamples variable that indicate number of samples for the last calibration.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gltest26 1:9bef044f45ad 1 /**
gltest26 4:155c44407af5 2 * @file ITG3200.cpp
gltest26 1:9bef044f45ad 3 * @author Aaron Berk
gltest26 1:9bef044f45ad 4 *
gltest26 1:9bef044f45ad 5 * @section LICENSE
gltest26 1:9bef044f45ad 6 *
gltest26 1:9bef044f45ad 7 * Copyright (c) 2010 ARM Limited
gltest26 1:9bef044f45ad 8 *
gltest26 1:9bef044f45ad 9 * Permission is hereby granted, free of charge, to any person obtaining a copy
gltest26 1:9bef044f45ad 10 * of this software and associated documentation files (the "Software"), to deal
gltest26 1:9bef044f45ad 11 * in the Software without restriction, including without limitation the rights
gltest26 1:9bef044f45ad 12 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
gltest26 1:9bef044f45ad 13 * copies of the Software, and to permit persons to whom the Software is
gltest26 1:9bef044f45ad 14 * furnished to do so, subject to the following conditions:
gltest26 1:9bef044f45ad 15 *
gltest26 1:9bef044f45ad 16 * The above copyright notice and this permission notice shall be included in
gltest26 1:9bef044f45ad 17 * all copies or substantial portions of the Software.
gltest26 1:9bef044f45ad 18 *
gltest26 1:9bef044f45ad 19 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
gltest26 1:9bef044f45ad 20 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
gltest26 1:9bef044f45ad 21 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
gltest26 1:9bef044f45ad 22 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
gltest26 1:9bef044f45ad 23 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
gltest26 1:9bef044f45ad 24 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
gltest26 1:9bef044f45ad 25 * THE SOFTWARE.
gltest26 1:9bef044f45ad 26 *
gltest26 1:9bef044f45ad 27 * @section DESCRIPTION
gltest26 1:9bef044f45ad 28 *
gltest26 1:9bef044f45ad 29 * ITG-3200 triple axis, digital interface, gyroscope.
gltest26 1:9bef044f45ad 30 *
gltest26 1:9bef044f45ad 31 * Datasheet:
gltest26 1:9bef044f45ad 32 *
gltest26 1:9bef044f45ad 33 * http://invensense.com/mems/gyro/documents/PS-ITG-3200-00-01.4.pdf
gltest26 1:9bef044f45ad 34 */
gltest26 1:9bef044f45ad 35
gltest26 1:9bef044f45ad 36 #include "ITG3200.h"
gltest26 1:9bef044f45ad 37
gltest26 6:a7ad6046824c 38 ITG3200::ITG3200(PinName sda, PinName scl) : calibSamples(0), i2c_(sda, scl){
gltest26 1:9bef044f45ad 39
gltest26 3:eea9733ca427 40 offset[0] = offset[1] = offset[2] = 0;
gltest26 3:eea9733ca427 41
gltest26 1:9bef044f45ad 42 //400kHz, fast mode.
gltest26 1:9bef044f45ad 43 i2c_.frequency(400000);
gltest26 1:9bef044f45ad 44
gltest26 1:9bef044f45ad 45 //Set FS_SEL to 0x03 for proper operation.
gltest26 1:9bef044f45ad 46 //See datasheet for details.
gltest26 1:9bef044f45ad 47 char tx[2];
gltest26 1:9bef044f45ad 48 tx[0] = DLPF_FS_REG;
gltest26 1:9bef044f45ad 49 //FS_SEL bits sit in bits 4 and 3 of DLPF_FS register.
gltest26 1:9bef044f45ad 50 tx[1] = 0x03 << 3;
gltest26 1:9bef044f45ad 51
gltest26 1:9bef044f45ad 52 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
gltest26 1:9bef044f45ad 53
gltest26 1:9bef044f45ad 54 }
gltest26 1:9bef044f45ad 55
gltest26 1:9bef044f45ad 56 char ITG3200::getWhoAmI(void){
gltest26 1:9bef044f45ad 57
gltest26 1:9bef044f45ad 58 //WhoAmI Register address.
gltest26 1:9bef044f45ad 59 char tx = WHO_AM_I_REG;
gltest26 1:9bef044f45ad 60 char rx;
gltest26 1:9bef044f45ad 61
gltest26 1:9bef044f45ad 62 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
gltest26 1:9bef044f45ad 63
gltest26 1:9bef044f45ad 64 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
gltest26 1:9bef044f45ad 65
gltest26 1:9bef044f45ad 66 return rx;
gltest26 1:9bef044f45ad 67
gltest26 1:9bef044f45ad 68 }
gltest26 1:9bef044f45ad 69
gltest26 1:9bef044f45ad 70 void ITG3200::setWhoAmI(char address){
gltest26 1:9bef044f45ad 71
gltest26 1:9bef044f45ad 72 char tx[2];
gltest26 1:9bef044f45ad 73 tx[0] = WHO_AM_I_REG;
gltest26 1:9bef044f45ad 74 tx[1] = address;
gltest26 1:9bef044f45ad 75
gltest26 1:9bef044f45ad 76 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
gltest26 1:9bef044f45ad 77
gltest26 1:9bef044f45ad 78 }
gltest26 1:9bef044f45ad 79
gltest26 1:9bef044f45ad 80 char ITG3200::getSampleRateDivider(void){
gltest26 1:9bef044f45ad 81
gltest26 1:9bef044f45ad 82 char tx = SMPLRT_DIV_REG;
gltest26 1:9bef044f45ad 83 char rx;
gltest26 1:9bef044f45ad 84
gltest26 1:9bef044f45ad 85 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
gltest26 1:9bef044f45ad 86
gltest26 1:9bef044f45ad 87 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
gltest26 1:9bef044f45ad 88
gltest26 1:9bef044f45ad 89 return rx;
gltest26 1:9bef044f45ad 90
gltest26 1:9bef044f45ad 91 }
gltest26 1:9bef044f45ad 92
gltest26 1:9bef044f45ad 93 void ITG3200::setSampleRateDivider(char divider){
gltest26 1:9bef044f45ad 94
gltest26 1:9bef044f45ad 95 char tx[2];
gltest26 1:9bef044f45ad 96 tx[0] = SMPLRT_DIV_REG;
gltest26 1:9bef044f45ad 97 tx[1] = divider;
gltest26 1:9bef044f45ad 98
gltest26 1:9bef044f45ad 99 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
gltest26 1:9bef044f45ad 100
gltest26 1:9bef044f45ad 101 }
gltest26 1:9bef044f45ad 102
gltest26 1:9bef044f45ad 103 int ITG3200::getInternalSampleRate(void){
gltest26 1:9bef044f45ad 104
gltest26 1:9bef044f45ad 105 char tx = DLPF_FS_REG;
gltest26 1:9bef044f45ad 106 char rx;
gltest26 1:9bef044f45ad 107
gltest26 1:9bef044f45ad 108 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
gltest26 1:9bef044f45ad 109
gltest26 1:9bef044f45ad 110 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
gltest26 1:9bef044f45ad 111
gltest26 1:9bef044f45ad 112 //DLPF_CFG == 0 -> sample rate = 8kHz.
gltest26 1:9bef044f45ad 113 if(rx == 0){
gltest26 1:9bef044f45ad 114 return 8;
gltest26 1:9bef044f45ad 115 }
gltest26 1:9bef044f45ad 116 //DLPF_CFG = 1..7 -> sample rate = 1kHz.
gltest26 1:9bef044f45ad 117 else if(rx >= 1 && rx <= 7){
gltest26 1:9bef044f45ad 118 return 1;
gltest26 1:9bef044f45ad 119 }
gltest26 1:9bef044f45ad 120 //DLPF_CFG = anything else -> something's wrong!
gltest26 1:9bef044f45ad 121 else{
gltest26 1:9bef044f45ad 122 return -1;
gltest26 1:9bef044f45ad 123 }
gltest26 1:9bef044f45ad 124
gltest26 1:9bef044f45ad 125 }
gltest26 1:9bef044f45ad 126
gltest26 1:9bef044f45ad 127 void ITG3200::setLpBandwidth(char bandwidth){
gltest26 1:9bef044f45ad 128
gltest26 1:9bef044f45ad 129 char tx[2];
gltest26 1:9bef044f45ad 130 tx[0] = DLPF_FS_REG;
gltest26 1:9bef044f45ad 131 //Bits 4,3 are required to be 0x03 for proper operation.
gltest26 1:9bef044f45ad 132 tx[1] = bandwidth | (0x03 << 3);
gltest26 1:9bef044f45ad 133
gltest26 1:9bef044f45ad 134 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
gltest26 1:9bef044f45ad 135
gltest26 1:9bef044f45ad 136 }
gltest26 1:9bef044f45ad 137
gltest26 1:9bef044f45ad 138 char ITG3200::getInterruptConfiguration(void){
gltest26 1:9bef044f45ad 139
gltest26 1:9bef044f45ad 140 char tx = INT_CFG_REG;
gltest26 1:9bef044f45ad 141 char rx;
gltest26 1:9bef044f45ad 142
gltest26 1:9bef044f45ad 143 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
gltest26 1:9bef044f45ad 144
gltest26 1:9bef044f45ad 145 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
gltest26 1:9bef044f45ad 146
gltest26 1:9bef044f45ad 147 return rx;
gltest26 1:9bef044f45ad 148
gltest26 1:9bef044f45ad 149 }
gltest26 1:9bef044f45ad 150
gltest26 1:9bef044f45ad 151 void ITG3200::setInterruptConfiguration(char config){
gltest26 1:9bef044f45ad 152
gltest26 1:9bef044f45ad 153 char tx[2];
gltest26 1:9bef044f45ad 154 tx[0] = INT_CFG_REG;
gltest26 1:9bef044f45ad 155 tx[1] = config;
gltest26 1:9bef044f45ad 156
gltest26 1:9bef044f45ad 157 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
gltest26 1:9bef044f45ad 158
gltest26 1:9bef044f45ad 159 }
gltest26 1:9bef044f45ad 160
gltest26 1:9bef044f45ad 161 bool ITG3200::isPllReady(void){
gltest26 1:9bef044f45ad 162
gltest26 1:9bef044f45ad 163 char tx = INT_STATUS;
gltest26 1:9bef044f45ad 164 char rx;
gltest26 1:9bef044f45ad 165
gltest26 1:9bef044f45ad 166 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
gltest26 1:9bef044f45ad 167
gltest26 1:9bef044f45ad 168 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
gltest26 1:9bef044f45ad 169
gltest26 1:9bef044f45ad 170 //ITG_RDY bit is bit 4 of INT_STATUS register.
gltest26 1:9bef044f45ad 171 if(rx & 0x04){
gltest26 1:9bef044f45ad 172 return true;
gltest26 1:9bef044f45ad 173 }
gltest26 1:9bef044f45ad 174 else{
gltest26 1:9bef044f45ad 175 return false;
gltest26 1:9bef044f45ad 176 }
gltest26 1:9bef044f45ad 177
gltest26 1:9bef044f45ad 178 }
gltest26 1:9bef044f45ad 179
gltest26 1:9bef044f45ad 180 bool ITG3200::isRawDataReady(void){
gltest26 1:9bef044f45ad 181
gltest26 1:9bef044f45ad 182 char tx = INT_STATUS;
gltest26 1:9bef044f45ad 183 char rx;
gltest26 1:9bef044f45ad 184
gltest26 1:9bef044f45ad 185 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
gltest26 1:9bef044f45ad 186
gltest26 1:9bef044f45ad 187 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
gltest26 1:9bef044f45ad 188
gltest26 1:9bef044f45ad 189 //RAW_DATA_RDY bit is bit 1 of INT_STATUS register.
gltest26 1:9bef044f45ad 190 if(rx & 0x01){
gltest26 1:9bef044f45ad 191 return true;
gltest26 1:9bef044f45ad 192 }
gltest26 1:9bef044f45ad 193 else{
gltest26 1:9bef044f45ad 194 return false;
gltest26 1:9bef044f45ad 195 }
gltest26 1:9bef044f45ad 196
gltest26 1:9bef044f45ad 197 }
gltest26 1:9bef044f45ad 198
gltest26 4:155c44407af5 199 int ITG3200::getWord(int regi){
gltest26 1:9bef044f45ad 200
gltest26 4:155c44407af5 201 char tx = regi;
gltest26 1:9bef044f45ad 202 char rx[2];
gltest26 1:9bef044f45ad 203
gltest26 2:f44a902ba081 204 i2c_.write(I2C_ADDRESS, &tx, 1);
gltest26 2:f44a902ba081 205
gltest26 2:f44a902ba081 206 i2c_.read(I2C_ADDRESS, rx, 2);
gltest26 1:9bef044f45ad 207
gltest26 5:0a0315f0f34e 208 return swapExtend(rx);
gltest26 1:9bef044f45ad 209 }
gltest26 1:9bef044f45ad 210
gltest26 1:9bef044f45ad 211 float ITG3200::getTemperature(){
gltest26 1:9bef044f45ad 212 //Offset = -35 degrees, 13200 counts. 280 counts/degrees C.
gltest26 1:9bef044f45ad 213 return 35.0 + ((getRawTemperature() + 13200)/280.0);
gltest26 1:9bef044f45ad 214
gltest26 1:9bef044f45ad 215 }
gltest26 1:9bef044f45ad 216
gltest26 3:eea9733ca427 217 void ITG3200::getRawGyroXYZ(int readings[3]){
gltest26 1:9bef044f45ad 218
gltest26 1:9bef044f45ad 219 char tx = GYRO_XOUT_H_REG;
gltest26 1:9bef044f45ad 220 char rx[2];
gltest26 1:9bef044f45ad 221
gltest26 1:9bef044f45ad 222 i2c_.write(I2C_ADDRESS, &tx, 1);
gltest26 1:9bef044f45ad 223
gltest26 1:9bef044f45ad 224 i2c_.read(I2C_ADDRESS, rx, 6);
gltest26 1:9bef044f45ad 225
gltest26 1:9bef044f45ad 226 for(int i = 0; i < 3; i++)
gltest26 5:0a0315f0f34e 227 readings[i] = swapExtend(&rx[i * 2]);
gltest26 1:9bef044f45ad 228 }
gltest26 1:9bef044f45ad 229
gltest26 1:9bef044f45ad 230 char ITG3200::getPowerManagement(void){
gltest26 1:9bef044f45ad 231
gltest26 1:9bef044f45ad 232 char tx = PWR_MGM_REG;
gltest26 1:9bef044f45ad 233 char rx;
gltest26 1:9bef044f45ad 234
gltest26 1:9bef044f45ad 235 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
gltest26 1:9bef044f45ad 236
gltest26 1:9bef044f45ad 237 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
gltest26 1:9bef044f45ad 238
gltest26 1:9bef044f45ad 239 return rx;
gltest26 1:9bef044f45ad 240
gltest26 1:9bef044f45ad 241 }
gltest26 1:9bef044f45ad 242
gltest26 1:9bef044f45ad 243 void ITG3200::setPowerManagement(char config){
gltest26 1:9bef044f45ad 244
gltest26 1:9bef044f45ad 245 char tx[2];
gltest26 1:9bef044f45ad 246 tx[0] = PWR_MGM_REG;
gltest26 1:9bef044f45ad 247 tx[1] = config;
gltest26 1:9bef044f45ad 248
gltest26 1:9bef044f45ad 249 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
gltest26 1:9bef044f45ad 250
gltest26 1:9bef044f45ad 251 }
gltest26 3:eea9733ca427 252
gltest26 3:eea9733ca427 253 void ITG3200::calibrate(double time){
gltest26 3:eea9733ca427 254 long sum[3] = {0};
gltest26 3:eea9733ca427 255 int sumCount = 0;
gltest26 6:a7ad6046824c 256 Timer t;
gltest26 6:a7ad6046824c 257 t.start();
gltest26 6:a7ad6046824c 258 while(t.read() < time){
gltest26 3:eea9733ca427 259 int gyro[3];
gltest26 6:a7ad6046824c 260 getRawGyroXYZ(gyro);
gltest26 3:eea9733ca427 261 for(int i = 0; i < 3; i++)
gltest26 3:eea9733ca427 262 sum[i] += gyro[i];
gltest26 3:eea9733ca427 263 sumCount++;
gltest26 3:eea9733ca427 264 }
gltest26 6:a7ad6046824c 265 t.stop();
gltest26 6:a7ad6046824c 266
gltest26 3:eea9733ca427 267 // Avoid zero division
gltest26 3:eea9733ca427 268 if(0 < sumCount){
gltest26 3:eea9733ca427 269 for(int i = 0; i < 3; i++)
gltest26 3:eea9733ca427 270 offset[i] = sum[i] / sumCount;
gltest26 6:a7ad6046824c 271 // Update member variable only if successful
gltest26 6:a7ad6046824c 272 calibSamples = sumCount;
gltest26 3:eea9733ca427 273 }
gltest26 3:eea9733ca427 274 }