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@3:eea9733ca427, 2012-09-12 (annotated)

Committer:
gltest26
Date:
Wed Sep 12 14:37:34 2012 +0000
Revision:
3:eea9733ca427
Parent:
2:f44a902ba081
Child:
4:155c44407af5
Added drift offset calibration capability.

Who changed what in which revision?

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