Tyler Weaver / ITG3200

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   9Dof_unit_testing

Fork of ITG3200 by James Watanabe

ITG3200.cpp@11:9a354f34d8e3, 2012-10-31 (annotated)

Committer:
tylerjw
Date:
Wed Oct 31 15:42:01 2012 +0000
Revision:
11:9a354f34d8e3
Parent:
9:05396b551a9a
Child:
12:d624e9c6dae7
updated header comments

Who changed what in which revision?

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