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@9:05396b551a9a, 2012-10-02 (annotated)

Committer:
gltest26
Date:
Tue Oct 02 18:04:13 2012 +0000
Revision:
9:05396b551a9a
Parent:
8:ac0365ab3cef 
Added support for calibration curve on chip temperature.

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 8:ac0365ab3cef 37 #include <new>
gltest26 1:9bef044f45ad 38
gltest26 8:ac0365ab3cef 39 ITG3200::ITG3200(PinName sda, PinName scl, bool fastmode) : calibSamples(0), i2c_(*reinterpret_cast<I2C*>(i2cRaw)){
gltest26 8:ac0365ab3cef 40 // Placement new to avoid additional heap memory allocation.
gltest26 8:ac0365ab3cef 41 new(i2cRaw) I2C(sda, scl);
gltest26 1:9bef044f45ad 42
gltest26 3:eea9733ca427 43 offset[0] = offset[1] = offset[2] = 0;
gltest26 3:eea9733ca427 44
gltest26 7:43b936a53b64 45 if(fastmode){
gltest26 7:43b936a53b64 46 //400kHz, fast mode.
gltest26 7:43b936a53b64 47 i2c_.frequency(400000);
gltest26 7:43b936a53b64 48 }
gltest26 7:43b936a53b64 49 else
gltest26 7:43b936a53b64 50 i2c_.frequency(100000);
gltest26 8:ac0365ab3cef 51 }
gltest26 1:9bef044f45ad 52
gltest26 8:ac0365ab3cef 53 ITG3200::~ITG3200(){
gltest26 8:ac0365ab3cef 54 // If the I2C object is initialized in the buffer in this object, call destructor of it.
gltest26 8:ac0365ab3cef 55 if(&i2c_ == reinterpret_cast<I2C*>(&i2cRaw))
gltest26 8:ac0365ab3cef 56 reinterpret_cast<I2C*>(&i2cRaw)->~I2C();
gltest26 8:ac0365ab3cef 57 }
gltest26 8:ac0365ab3cef 58
gltest26 8:ac0365ab3cef 59 void ITG3200::init(){
gltest26 1:9bef044f45ad 60 //Set FS_SEL to 0x03 for proper operation.
gltest26 1:9bef044f45ad 61 //See datasheet for details.
gltest26 1:9bef044f45ad 62 char tx[2];
gltest26 1:9bef044f45ad 63 tx[0] = DLPF_FS_REG;
gltest26 1:9bef044f45ad 64 //FS_SEL bits sit in bits 4 and 3 of DLPF_FS register.
gltest26 1:9bef044f45ad 65 tx[1] = 0x03 << 3;
gltest26 1:9bef044f45ad 66
gltest26 1:9bef044f45ad 67 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
gltest26 1:9bef044f45ad 68
gltest26 1:9bef044f45ad 69 }
gltest26 1:9bef044f45ad 70
gltest26 9:05396b551a9a 71 void ITG3200::setCalibrationCurve(const float offset[3], const float slope[3]){
gltest26 9:05396b551a9a 72 if(offset){
gltest26 9:05396b551a9a 73 for(int i = 0; i < 3; i++)
gltest26 9:05396b551a9a 74 this->foffset[i] = offset[i];
gltest26 9:05396b551a9a 75 }
gltest26 9:05396b551a9a 76 if(slope){
gltest26 9:05396b551a9a 77 for(int i = 0; i < 3; i++)
gltest26 9:05396b551a9a 78 this->slope[i] = slope[i];
gltest26 9:05396b551a9a 79 }
gltest26 9:05396b551a9a 80 }
gltest26 9:05396b551a9a 81
gltest26 1:9bef044f45ad 82 char ITG3200::getWhoAmI(void){
gltest26 1:9bef044f45ad 83
gltest26 1:9bef044f45ad 84 //WhoAmI Register address.
gltest26 1:9bef044f45ad 85 char tx = WHO_AM_I_REG;
gltest26 1:9bef044f45ad 86 char rx;
gltest26 1:9bef044f45ad 87
gltest26 1:9bef044f45ad 88 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
gltest26 1:9bef044f45ad 89
gltest26 1:9bef044f45ad 90 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
gltest26 1:9bef044f45ad 91
gltest26 1:9bef044f45ad 92 return rx;
gltest26 1:9bef044f45ad 93
gltest26 1:9bef044f45ad 94 }
gltest26 1:9bef044f45ad 95
gltest26 1:9bef044f45ad 96 void ITG3200::setWhoAmI(char address){
gltest26 1:9bef044f45ad 97
gltest26 1:9bef044f45ad 98 char tx[2];
gltest26 1:9bef044f45ad 99 tx[0] = WHO_AM_I_REG;
gltest26 1:9bef044f45ad 100 tx[1] = address;
gltest26 1:9bef044f45ad 101
gltest26 1:9bef044f45ad 102 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
gltest26 1:9bef044f45ad 103
gltest26 1:9bef044f45ad 104 }
gltest26 1:9bef044f45ad 105
gltest26 1:9bef044f45ad 106 char ITG3200::getSampleRateDivider(void){
gltest26 1:9bef044f45ad 107
gltest26 1:9bef044f45ad 108 char tx = SMPLRT_DIV_REG;
gltest26 1:9bef044f45ad 109 char rx;
gltest26 1:9bef044f45ad 110
gltest26 1:9bef044f45ad 111 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
gltest26 1:9bef044f45ad 112
gltest26 1:9bef044f45ad 113 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
gltest26 1:9bef044f45ad 114
gltest26 1:9bef044f45ad 115 return rx;
gltest26 1:9bef044f45ad 116
gltest26 1:9bef044f45ad 117 }
gltest26 1:9bef044f45ad 118
gltest26 1:9bef044f45ad 119 void ITG3200::setSampleRateDivider(char divider){
gltest26 1:9bef044f45ad 120
gltest26 1:9bef044f45ad 121 char tx[2];
gltest26 1:9bef044f45ad 122 tx[0] = SMPLRT_DIV_REG;
gltest26 1:9bef044f45ad 123 tx[1] = divider;
gltest26 1:9bef044f45ad 124
gltest26 1:9bef044f45ad 125 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
gltest26 1:9bef044f45ad 126
gltest26 1:9bef044f45ad 127 }
gltest26 1:9bef044f45ad 128
gltest26 1:9bef044f45ad 129 int ITG3200::getInternalSampleRate(void){
gltest26 1:9bef044f45ad 130
gltest26 1:9bef044f45ad 131 char tx = DLPF_FS_REG;
gltest26 1:9bef044f45ad 132 char rx;
gltest26 1:9bef044f45ad 133
gltest26 1:9bef044f45ad 134 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
gltest26 1:9bef044f45ad 135
gltest26 1:9bef044f45ad 136 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
gltest26 1:9bef044f45ad 137
gltest26 1:9bef044f45ad 138 //DLPF_CFG == 0 -> sample rate = 8kHz.
gltest26 1:9bef044f45ad 139 if(rx == 0){
gltest26 1:9bef044f45ad 140 return 8;
gltest26 1:9bef044f45ad 141 }
gltest26 1:9bef044f45ad 142 //DLPF_CFG = 1..7 -> sample rate = 1kHz.
gltest26 1:9bef044f45ad 143 else if(rx >= 1 && rx <= 7){
gltest26 1:9bef044f45ad 144 return 1;
gltest26 1:9bef044f45ad 145 }
gltest26 1:9bef044f45ad 146 //DLPF_CFG = anything else -> something's wrong!
gltest26 1:9bef044f45ad 147 else{
gltest26 1:9bef044f45ad 148 return -1;
gltest26 1:9bef044f45ad 149 }
gltest26 1:9bef044f45ad 150
gltest26 1:9bef044f45ad 151 }
gltest26 1:9bef044f45ad 152
gltest26 1:9bef044f45ad 153 void ITG3200::setLpBandwidth(char bandwidth){
gltest26 1:9bef044f45ad 154
gltest26 1:9bef044f45ad 155 char tx[2];
gltest26 1:9bef044f45ad 156 tx[0] = DLPF_FS_REG;
gltest26 1:9bef044f45ad 157 //Bits 4,3 are required to be 0x03 for proper operation.
gltest26 1:9bef044f45ad 158 tx[1] = bandwidth | (0x03 << 3);
gltest26 1:9bef044f45ad 159
gltest26 1:9bef044f45ad 160 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
gltest26 1:9bef044f45ad 161
gltest26 1:9bef044f45ad 162 }
gltest26 1:9bef044f45ad 163
gltest26 1:9bef044f45ad 164 char ITG3200::getInterruptConfiguration(void){
gltest26 1:9bef044f45ad 165
gltest26 1:9bef044f45ad 166 char tx = INT_CFG_REG;
gltest26 1:9bef044f45ad 167 char rx;
gltest26 1:9bef044f45ad 168
gltest26 1:9bef044f45ad 169 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
gltest26 1:9bef044f45ad 170
gltest26 1:9bef044f45ad 171 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
gltest26 1:9bef044f45ad 172
gltest26 1:9bef044f45ad 173 return rx;
gltest26 1:9bef044f45ad 174
gltest26 1:9bef044f45ad 175 }
gltest26 1:9bef044f45ad 176
gltest26 1:9bef044f45ad 177 void ITG3200::setInterruptConfiguration(char config){
gltest26 1:9bef044f45ad 178
gltest26 1:9bef044f45ad 179 char tx[2];
gltest26 1:9bef044f45ad 180 tx[0] = INT_CFG_REG;
gltest26 1:9bef044f45ad 181 tx[1] = config;
gltest26 1:9bef044f45ad 182
gltest26 1:9bef044f45ad 183 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
gltest26 1:9bef044f45ad 184
gltest26 1:9bef044f45ad 185 }
gltest26 1:9bef044f45ad 186
gltest26 1:9bef044f45ad 187 bool ITG3200::isPllReady(void){
gltest26 1:9bef044f45ad 188
gltest26 1:9bef044f45ad 189 char tx = INT_STATUS;
gltest26 1:9bef044f45ad 190 char rx;
gltest26 1:9bef044f45ad 191
gltest26 1:9bef044f45ad 192 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
gltest26 1:9bef044f45ad 193
gltest26 1:9bef044f45ad 194 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
gltest26 1:9bef044f45ad 195
gltest26 1:9bef044f45ad 196 //ITG_RDY bit is bit 4 of INT_STATUS register.
gltest26 1:9bef044f45ad 197 if(rx & 0x04){
gltest26 1:9bef044f45ad 198 return true;
gltest26 1:9bef044f45ad 199 }
gltest26 1:9bef044f45ad 200 else{
gltest26 1:9bef044f45ad 201 return false;
gltest26 1:9bef044f45ad 202 }
gltest26 1:9bef044f45ad 203
gltest26 1:9bef044f45ad 204 }
gltest26 1:9bef044f45ad 205
gltest26 1:9bef044f45ad 206 bool ITG3200::isRawDataReady(void){
gltest26 1:9bef044f45ad 207
gltest26 1:9bef044f45ad 208 char tx = INT_STATUS;
gltest26 1:9bef044f45ad 209 char rx;
gltest26 1:9bef044f45ad 210
gltest26 1:9bef044f45ad 211 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
gltest26 1:9bef044f45ad 212
gltest26 1:9bef044f45ad 213 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
gltest26 1:9bef044f45ad 214
gltest26 1:9bef044f45ad 215 //RAW_DATA_RDY bit is bit 1 of INT_STATUS register.
gltest26 1:9bef044f45ad 216 if(rx & 0x01){
gltest26 1:9bef044f45ad 217 return true;
gltest26 1:9bef044f45ad 218 }
gltest26 1:9bef044f45ad 219 else{
gltest26 1:9bef044f45ad 220 return false;
gltest26 1:9bef044f45ad 221 }
gltest26 1:9bef044f45ad 222
gltest26 1:9bef044f45ad 223 }
gltest26 1:9bef044f45ad 224
gltest26 4:155c44407af5 225 int ITG3200::getWord(int regi){
gltest26 1:9bef044f45ad 226
gltest26 4:155c44407af5 227 char tx = regi;
gltest26 1:9bef044f45ad 228 char rx[2];
gltest26 1:9bef044f45ad 229
gltest26 2:f44a902ba081 230 i2c_.write(I2C_ADDRESS, &tx, 1);
gltest26 2:f44a902ba081 231
gltest26 2:f44a902ba081 232 i2c_.read(I2C_ADDRESS, rx, 2);
gltest26 1:9bef044f45ad 233
gltest26 5:0a0315f0f34e 234 return swapExtend(rx);
gltest26 1:9bef044f45ad 235 }
gltest26 1:9bef044f45ad 236
gltest26 1:9bef044f45ad 237 float ITG3200::getTemperature(){
gltest26 1:9bef044f45ad 238 //Offset = -35 degrees, 13200 counts. 280 counts/degrees C.
gltest26 1:9bef044f45ad 239 return 35.0 + ((getRawTemperature() + 13200)/280.0);
gltest26 1:9bef044f45ad 240
gltest26 1:9bef044f45ad 241 }
gltest26 1:9bef044f45ad 242
gltest26 3:eea9733ca427 243 void ITG3200::getRawGyroXYZ(int readings[3]){
gltest26 1:9bef044f45ad 244
gltest26 1:9bef044f45ad 245 char tx = GYRO_XOUT_H_REG;
gltest26 1:9bef044f45ad 246 char rx[2];
gltest26 1:9bef044f45ad 247
gltest26 1:9bef044f45ad 248 i2c_.write(I2C_ADDRESS, &tx, 1);
gltest26 1:9bef044f45ad 249
gltest26 1:9bef044f45ad 250 i2c_.read(I2C_ADDRESS, rx, 6);
gltest26 1:9bef044f45ad 251
gltest26 1:9bef044f45ad 252 for(int i = 0; i < 3; i++)
gltest26 5:0a0315f0f34e 253 readings[i] = swapExtend(&rx[i * 2]);
gltest26 1:9bef044f45ad 254 }
gltest26 1:9bef044f45ad 255
gltest26 9:05396b551a9a 256 void ITG3200::getGyroXYZ(int readings[3], Correction corr){
gltest26 9:05396b551a9a 257 getRawGyroXYZ(readings);
gltest26 9:05396b551a9a 258 switch(corr){
gltest26 9:05396b551a9a 259 case OffsetCorrection:
gltest26 9:05396b551a9a 260 for(int i = 0; i < 3; i++)
gltest26 9:05396b551a9a 261 readings[i] -= offset[i];
gltest26 9:05396b551a9a 262 break;
gltest26 9:05396b551a9a 263 case Calibration:
gltest26 9:05396b551a9a 264 {
gltest26 9:05396b551a9a 265 float temp = getTemperature();
gltest26 9:05396b551a9a 266 for(int i = 0; i < 3; i++)
gltest26 9:05396b551a9a 267 readings[i] -= slope[i] * temp + foffset[i];
gltest26 9:05396b551a9a 268 }
gltest26 9:05396b551a9a 269 break;
gltest26 9:05396b551a9a 270 }
gltest26 9:05396b551a9a 271 }
gltest26 9:05396b551a9a 272
gltest26 9:05396b551a9a 273
gltest26 1:9bef044f45ad 274 char ITG3200::getPowerManagement(void){
gltest26 1:9bef044f45ad 275
gltest26 1:9bef044f45ad 276 char tx = PWR_MGM_REG;
gltest26 1:9bef044f45ad 277 char rx;
gltest26 1:9bef044f45ad 278
gltest26 1:9bef044f45ad 279 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
gltest26 1:9bef044f45ad 280
gltest26 1:9bef044f45ad 281 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
gltest26 1:9bef044f45ad 282
gltest26 1:9bef044f45ad 283 return rx;
gltest26 1:9bef044f45ad 284
gltest26 1:9bef044f45ad 285 }
gltest26 1:9bef044f45ad 286
gltest26 1:9bef044f45ad 287 void ITG3200::setPowerManagement(char config){
gltest26 1:9bef044f45ad 288
gltest26 1:9bef044f45ad 289 char tx[2];
gltest26 1:9bef044f45ad 290 tx[0] = PWR_MGM_REG;
gltest26 1:9bef044f45ad 291 tx[1] = config;
gltest26 1:9bef044f45ad 292
gltest26 1:9bef044f45ad 293 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
gltest26 1:9bef044f45ad 294
gltest26 1:9bef044f45ad 295 }
gltest26 3:eea9733ca427 296
gltest26 3:eea9733ca427 297 void ITG3200::calibrate(double time){
gltest26 3:eea9733ca427 298 long sum[3] = {0};
gltest26 3:eea9733ca427 299 int sumCount = 0;
gltest26 6:a7ad6046824c 300 Timer t;
gltest26 6:a7ad6046824c 301 t.start();
gltest26 6:a7ad6046824c 302 while(t.read() < time){
gltest26 3:eea9733ca427 303 int gyro[3];
gltest26 6:a7ad6046824c 304 getRawGyroXYZ(gyro);
gltest26 3:eea9733ca427 305 for(int i = 0; i < 3; i++)
gltest26 3:eea9733ca427 306 sum[i] += gyro[i];
gltest26 3:eea9733ca427 307 sumCount++;
gltest26 3:eea9733ca427 308 }
gltest26 6:a7ad6046824c 309 t.stop();
gltest26 6:a7ad6046824c 310
gltest26 3:eea9733ca427 311 // Avoid zero division
gltest26 3:eea9733ca427 312 if(0 < sumCount){
gltest26 3:eea9733ca427 313 for(int i = 0; i < 3; i++)
gltest26 3:eea9733ca427 314 offset[i] = sum[i] / sumCount;
gltest26 6:a7ad6046824c 315 // Update member variable only if successful
gltest26 6:a7ad6046824c 316 calibSamples = sumCount;
gltest26 3:eea9733ca427 317 }
gltest26 3:eea9733ca427 318 }