Tyler Weaver / ITG3200

Modified getOffset for calibrating Thermal Drift coefficients.

Dependents:   9Dof_unit_testing

Fork of ITG3200 by James Watanabe

Modified to make getOffset() function easier to use.

ITG3200.cpp@15:e61fe4b74daa, 2012-11-08 (annotated)

Committer:
tylerjw
Date:
Thu Nov 08 20:48:30 2012 +0000
Revision:
15:e61fe4b74daa
Parent:
14:e4bf80188ba7 
with calibration curve

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
tylerjw 14:e4bf80188ba7 45 offset[0] = offset[1] = offset[2] = 0.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];
tylerjw 14:e4bf80188ba7 66
tylerjw 14:e4bf80188ba7 67 // Power up reset defaults
tylerjw 14:e4bf80188ba7 68 tx[0] = 0x3E; // Power management
tylerjw 14:e4bf80188ba7 69 tx[1] = 0x80; // ?
tylerjw 14:e4bf80188ba7 70 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
tylerjw 14:e4bf80188ba7 71 wait_ms(5);
tylerjw 14:e4bf80188ba7 72
tylerjw 14:e4bf80188ba7 73 // Select full-scale range of the gyro sensors
tylerjw 14:e4bf80188ba7 74 // Set LP filter bandwidth to 42Hz
tylerjw 14:e4bf80188ba7 75 tx[0] = 0x16; //
tylerjw 14:e4bf80188ba7 76 tx[1] = 0x1B; // DLPF_CFG = 3, FS_SEL = 3
gltest26 1:9bef044f45ad 77 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
tylerjw 14:e4bf80188ba7 78 wait_ms(5);
tylerjw 12:d624e9c6dae7 79
tylerjw 14:e4bf80188ba7 80 // set sample rate to 50 Hz
tylerjw 14:e4bf80188ba7 81 tx[0] = 0x15; //
tylerjw 14:e4bf80188ba7 82 tx[1] = 0x0A; // SMPLRT_DIV = 10 (50Hz)
tylerjw 14:e4bf80188ba7 83 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
tylerjw 14:e4bf80188ba7 84 wait_ms(5);
tylerjw 14:e4bf80188ba7 85
tylerjw 14:e4bf80188ba7 86 // Set clock to PLL with z gyro reference
tylerjw 14:e4bf80188ba7 87 tx[0] = 0x3E;
tylerjw 14:e4bf80188ba7 88 tx[1] = 0x00;
tylerjw 14:e4bf80188ba7 89 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
tylerjw 14:e4bf80188ba7 90 wait_ms(5);
tylerjw 15:e61fe4b74daa 91
tylerjw 12:d624e9c6dae7 92 const float offset[3] = {99.5, -45.0, -29.7}; // taken from itg3200.xls curve fit test
tylerjw 12:d624e9c6dae7 93 const float slope[3] = {-1.05, 0.95, 0.47};
gltest26 1:9bef044f45ad 94
tylerjw 12:d624e9c6dae7 95 setCalibrationCurve(offset, slope);
gltest26 1:9bef044f45ad 96 }
gltest26 1:9bef044f45ad 97
tylerjw 11:9a354f34d8e3 98 void ITG3200::setCalibrationCurve(const float offset[3], const float slope[3])
tylerjw 11:9a354f34d8e3 99 {
tylerjw 11:9a354f34d8e3 100 if(offset) {
gltest26 9:05396b551a9a 101 for(int i = 0; i < 3; i++)
gltest26 9:05396b551a9a 102 this->foffset[i] = offset[i];
gltest26 9:05396b551a9a 103 }
tylerjw 11:9a354f34d8e3 104 if(slope) {
gltest26 9:05396b551a9a 105 for(int i = 0; i < 3; i++)
gltest26 9:05396b551a9a 106 this->slope[i] = slope[i];
gltest26 9:05396b551a9a 107 }
gltest26 9:05396b551a9a 108 }
gltest26 9:05396b551a9a 109
tylerjw 11:9a354f34d8e3 110 char ITG3200::getWhoAmI(void)
tylerjw 11:9a354f34d8e3 111 {
gltest26 1:9bef044f45ad 112
gltest26 1:9bef044f45ad 113 //WhoAmI Register address.
gltest26 1:9bef044f45ad 114 char tx = WHO_AM_I_REG;
gltest26 1:9bef044f45ad 115 char rx;
gltest26 1:9bef044f45ad 116
tylerjw 11:9a354f34d8e3 117 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
gltest26 1:9bef044f45ad 118
gltest26 1:9bef044f45ad 119 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
gltest26 1:9bef044f45ad 120
gltest26 1:9bef044f45ad 121 return rx;
gltest26 1:9bef044f45ad 122
gltest26 1:9bef044f45ad 123 }
gltest26 1:9bef044f45ad 124
tylerjw 11:9a354f34d8e3 125 void ITG3200::setWhoAmI(char address)
tylerjw 11:9a354f34d8e3 126 {
tylerjw 11:9a354f34d8e3 127
tylerjw 11:9a354f34d8e3 128 char tx[2];
tylerjw 11:9a354f34d8e3 129 tx[0] = WHO_AM_I_REG;
tylerjw 11:9a354f34d8e3 130 tx[1] = address;
tylerjw 11:9a354f34d8e3 131
tylerjw 11:9a354f34d8e3 132 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
tylerjw 11:9a354f34d8e3 133
tylerjw 11:9a354f34d8e3 134 }
tylerjw 11:9a354f34d8e3 135
tylerjw 11:9a354f34d8e3 136 char ITG3200::getSampleRateDivider(void)
tylerjw 11:9a354f34d8e3 137 {
tylerjw 11:9a354f34d8e3 138
tylerjw 11:9a354f34d8e3 139 char tx = SMPLRT_DIV_REG;
tylerjw 11:9a354f34d8e3 140 char rx;
tylerjw 11:9a354f34d8e3 141
tylerjw 11:9a354f34d8e3 142 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
tylerjw 11:9a354f34d8e3 143
tylerjw 11:9a354f34d8e3 144 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
tylerjw 11:9a354f34d8e3 145
tylerjw 11:9a354f34d8e3 146 return rx;
tylerjw 11:9a354f34d8e3 147
tylerjw 11:9a354f34d8e3 148 }
tylerjw 11:9a354f34d8e3 149
tylerjw 11:9a354f34d8e3 150 void ITG3200::setSampleRateDivider(char divider)
tylerjw 11:9a354f34d8e3 151 {
gltest26 1:9bef044f45ad 152
gltest26 1:9bef044f45ad 153 char tx[2];
gltest26 1:9bef044f45ad 154 tx[0] = SMPLRT_DIV_REG;
gltest26 1:9bef044f45ad 155 tx[1] = divider;
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
tylerjw 11:9a354f34d8e3 161 int ITG3200::getInternalSampleRate(void)
tylerjw 11:9a354f34d8e3 162 {
gltest26 1:9bef044f45ad 163
gltest26 1:9bef044f45ad 164 char tx = DLPF_FS_REG;
gltest26 1:9bef044f45ad 165 char rx;
tylerjw 11:9a354f34d8e3 166
gltest26 1:9bef044f45ad 167 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
tylerjw 11:9a354f34d8e3 168
gltest26 1:9bef044f45ad 169 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
tylerjw 11:9a354f34d8e3 170
gltest26 1:9bef044f45ad 171 //DLPF_CFG == 0 -> sample rate = 8kHz.
tylerjw 11:9a354f34d8e3 172 if(rx == 0) {
gltest26 1:9bef044f45ad 173 return 8;
tylerjw 11:9a354f34d8e3 174 }
gltest26 1:9bef044f45ad 175 //DLPF_CFG = 1..7 -> sample rate = 1kHz.
tylerjw 11:9a354f34d8e3 176 else if(rx >= 1 && rx <= 7) {
gltest26 1:9bef044f45ad 177 return 1;
gltest26 1:9bef044f45ad 178 }
gltest26 1:9bef044f45ad 179 //DLPF_CFG = anything else -> something's wrong!
tylerjw 11:9a354f34d8e3 180 else {
gltest26 1:9bef044f45ad 181 return -1;
gltest26 1:9bef044f45ad 182 }
tylerjw 11:9a354f34d8e3 183
gltest26 1:9bef044f45ad 184 }
gltest26 1:9bef044f45ad 185
tylerjw 11:9a354f34d8e3 186 void ITG3200::setLpBandwidth(char bandwidth)
tylerjw 11:9a354f34d8e3 187 {
gltest26 1:9bef044f45ad 188
gltest26 1:9bef044f45ad 189 char tx[2];
gltest26 1:9bef044f45ad 190 tx[0] = DLPF_FS_REG;
gltest26 1:9bef044f45ad 191 //Bits 4,3 are required to be 0x03 for proper operation.
gltest26 1:9bef044f45ad 192 tx[1] = bandwidth | (0x03 << 3);
tylerjw 11:9a354f34d8e3 193
gltest26 1:9bef044f45ad 194 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
gltest26 1:9bef044f45ad 195
gltest26 1:9bef044f45ad 196 }
gltest26 1:9bef044f45ad 197
tylerjw 11:9a354f34d8e3 198 char ITG3200::getInterruptConfiguration(void)
tylerjw 11:9a354f34d8e3 199 {
gltest26 1:9bef044f45ad 200
gltest26 1:9bef044f45ad 201 char tx = INT_CFG_REG;
gltest26 1:9bef044f45ad 202 char rx;
tylerjw 11:9a354f34d8e3 203
gltest26 1:9bef044f45ad 204 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
tylerjw 11:9a354f34d8e3 205
gltest26 1:9bef044f45ad 206 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
tylerjw 11:9a354f34d8e3 207
gltest26 1:9bef044f45ad 208 return rx;
gltest26 1:9bef044f45ad 209
gltest26 1:9bef044f45ad 210 }
gltest26 1:9bef044f45ad 211
tylerjw 11:9a354f34d8e3 212 void ITG3200::setInterruptConfiguration(char config)
tylerjw 11:9a354f34d8e3 213 {
gltest26 1:9bef044f45ad 214
gltest26 1:9bef044f45ad 215 char tx[2];
gltest26 1:9bef044f45ad 216 tx[0] = INT_CFG_REG;
gltest26 1:9bef044f45ad 217 tx[1] = config;
tylerjw 11:9a354f34d8e3 218
gltest26 1:9bef044f45ad 219 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
gltest26 1:9bef044f45ad 220
gltest26 1:9bef044f45ad 221 }
gltest26 1:9bef044f45ad 222
tylerjw 11:9a354f34d8e3 223 bool ITG3200::isPllReady(void)
tylerjw 11:9a354f34d8e3 224 {
gltest26 1:9bef044f45ad 225
gltest26 1:9bef044f45ad 226 char tx = INT_STATUS;
gltest26 1:9bef044f45ad 227 char rx;
tylerjw 11:9a354f34d8e3 228
gltest26 1:9bef044f45ad 229 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
tylerjw 11:9a354f34d8e3 230
gltest26 1:9bef044f45ad 231 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
tylerjw 11:9a354f34d8e3 232
gltest26 1:9bef044f45ad 233 //ITG_RDY bit is bit 4 of INT_STATUS register.
tylerjw 11:9a354f34d8e3 234 if(rx & 0x04) {
gltest26 1:9bef044f45ad 235 return true;
tylerjw 11:9a354f34d8e3 236 } else {
gltest26 1:9bef044f45ad 237 return false;
gltest26 1:9bef044f45ad 238 }
tylerjw 11:9a354f34d8e3 239
gltest26 1:9bef044f45ad 240 }
gltest26 1:9bef044f45ad 241
tylerjw 11:9a354f34d8e3 242 bool ITG3200::isRawDataReady(void)
tylerjw 11:9a354f34d8e3 243 {
gltest26 1:9bef044f45ad 244
gltest26 1:9bef044f45ad 245 char tx = INT_STATUS;
gltest26 1:9bef044f45ad 246 char rx;
tylerjw 11:9a354f34d8e3 247
gltest26 1:9bef044f45ad 248 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
tylerjw 11:9a354f34d8e3 249
gltest26 1:9bef044f45ad 250 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
tylerjw 11:9a354f34d8e3 251
gltest26 1:9bef044f45ad 252 //RAW_DATA_RDY bit is bit 1 of INT_STATUS register.
tylerjw 11:9a354f34d8e3 253 if(rx & 0x01) {
gltest26 1:9bef044f45ad 254 return true;
tylerjw 11:9a354f34d8e3 255 } else {
gltest26 1:9bef044f45ad 256 return false;
gltest26 1:9bef044f45ad 257 }
tylerjw 11:9a354f34d8e3 258
gltest26 1:9bef044f45ad 259 }
gltest26 1:9bef044f45ad 260
tylerjw 11:9a354f34d8e3 261 int ITG3200::getWord(int regi)
tylerjw 11:9a354f34d8e3 262 {
gltest26 1:9bef044f45ad 263
gltest26 4:155c44407af5 264 char tx = regi;
gltest26 1:9bef044f45ad 265 char rx[2];
tylerjw 11:9a354f34d8e3 266
gltest26 2:f44a902ba081 267 i2c_.write(I2C_ADDRESS, &tx, 1);
tylerjw 11:9a354f34d8e3 268
gltest26 2:f44a902ba081 269 i2c_.read(I2C_ADDRESS, rx, 2);
tylerjw 11:9a354f34d8e3 270
gltest26 5:0a0315f0f34e 271 return swapExtend(rx);
gltest26 1:9bef044f45ad 272 }
gltest26 1:9bef044f45ad 273
tylerjw 11:9a354f34d8e3 274 float ITG3200::getTemperature()
tylerjw 11:9a354f34d8e3 275 {
gltest26 1:9bef044f45ad 276 //Offset = -35 degrees, 13200 counts. 280 counts/degrees C.
gltest26 1:9bef044f45ad 277 return 35.0 + ((getRawTemperature() + 13200)/280.0);
tylerjw 11:9a354f34d8e3 278
gltest26 1:9bef044f45ad 279 }
gltest26 1:9bef044f45ad 280
tylerjw 13:e886466d7d67 281 void ITG3200::getRawXYZ(int16_t readings[3])
tylerjw 11:9a354f34d8e3 282 {
gltest26 1:9bef044f45ad 283
gltest26 1:9bef044f45ad 284 char tx = GYRO_XOUT_H_REG;
gltest26 1:9bef044f45ad 285 char rx[2];
tylerjw 11:9a354f34d8e3 286
gltest26 1:9bef044f45ad 287 i2c_.write(I2C_ADDRESS, &tx, 1);
tylerjw 11:9a354f34d8e3 288
gltest26 1:9bef044f45ad 289 i2c_.read(I2C_ADDRESS, rx, 6);
tylerjw 11:9a354f34d8e3 290
gltest26 1:9bef044f45ad 291 for(int i = 0; i < 3; i++)
gltest26 5:0a0315f0f34e 292 readings[i] = swapExtend(&rx[i * 2]);
gltest26 1:9bef044f45ad 293 }
gltest26 1:9bef044f45ad 294
tylerjw 12:d624e9c6dae7 295 void ITG3200::getXYZ(int16_t readings[3], Correction corr)
tylerjw 11:9a354f34d8e3 296 {
tylerjw 12:d624e9c6dae7 297 getRawXYZ(readings);
tylerjw 11:9a354f34d8e3 298 switch(corr) {
tylerjw 11:9a354f34d8e3 299 case OffsetCorrection:
tylerjw 11:9a354f34d8e3 300 for(int i = 0; i < 3; i++)
tylerjw 14:e4bf80188ba7 301 readings[i] -= static_cast<int16_t>(offset[i]);
tylerjw 11:9a354f34d8e3 302 break;
tylerjw 11:9a354f34d8e3 303 case Calibration: {
gltest26 9:05396b551a9a 304 float temp = getTemperature();
gltest26 9:05396b551a9a 305 for(int i = 0; i < 3; i++)
gltest26 9:05396b551a9a 306 readings[i] -= slope[i] * temp + foffset[i];
gltest26 9:05396b551a9a 307 }
gltest26 9:05396b551a9a 308 break;
gltest26 9:05396b551a9a 309 }
gltest26 9:05396b551a9a 310 }
gltest26 9:05396b551a9a 311
gltest26 9:05396b551a9a 312
tylerjw 11:9a354f34d8e3 313 char ITG3200::getPowerManagement(void)
tylerjw 11:9a354f34d8e3 314 {
gltest26 1:9bef044f45ad 315
gltest26 1:9bef044f45ad 316 char tx = PWR_MGM_REG;
gltest26 1:9bef044f45ad 317 char rx;
tylerjw 11:9a354f34d8e3 318
gltest26 1:9bef044f45ad 319 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
tylerjw 11:9a354f34d8e3 320
gltest26 1:9bef044f45ad 321 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
tylerjw 11:9a354f34d8e3 322
gltest26 1:9bef044f45ad 323 return rx;
gltest26 1:9bef044f45ad 324
gltest26 1:9bef044f45ad 325 }
gltest26 1:9bef044f45ad 326
tylerjw 11:9a354f34d8e3 327 void ITG3200::setPowerManagement(char config)
tylerjw 11:9a354f34d8e3 328 {
gltest26 1:9bef044f45ad 329
gltest26 1:9bef044f45ad 330 char tx[2];
gltest26 1:9bef044f45ad 331 tx[0] = PWR_MGM_REG;
gltest26 1:9bef044f45ad 332 tx[1] = config;
gltest26 1:9bef044f45ad 333
gltest26 1:9bef044f45ad 334 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
gltest26 1:9bef044f45ad 335
gltest26 1:9bef044f45ad 336 }
gltest26 3:eea9733ca427 337
tylerjw 11:9a354f34d8e3 338 void ITG3200::calibrate(double time)
tylerjw 11:9a354f34d8e3 339 {
gltest26 3:eea9733ca427 340 long sum[3] = {0};
gltest26 3:eea9733ca427 341 int sumCount = 0;
gltest26 6:a7ad6046824c 342 Timer t;
gltest26 6:a7ad6046824c 343 t.start();
tylerjw 14:e4bf80188ba7 344 float start_time;
tylerjw 14:e4bf80188ba7 345 float period = 1.0 / 50.0; // 50Hz
tylerjw 14:e4bf80188ba7 346
tylerjw 11:9a354f34d8e3 347 while(t.read() < time) {
tylerjw 14:e4bf80188ba7 348 start_time = t.read();
tylerjw 13:e886466d7d67 349 int16_t gyro[3];
tylerjw 12:d624e9c6dae7 350 getRawXYZ(gyro);
gltest26 3:eea9733ca427 351 for(int i = 0; i < 3; i++)
gltest26 3:eea9733ca427 352 sum[i] += gyro[i];
gltest26 3:eea9733ca427 353 sumCount++;
tylerjw 14:e4bf80188ba7 354 wait(period - (t.read()-start_time));
gltest26 3:eea9733ca427 355 }
gltest26 6:a7ad6046824c 356 t.stop();
gltest26 6:a7ad6046824c 357
gltest26 3:eea9733ca427 358 // Avoid zero division
tylerjw 11:9a354f34d8e3 359 if(0 < sumCount) {
gltest26 3:eea9733ca427 360 for(int i = 0; i < 3; i++)
tylerjw 14:e4bf80188ba7 361 offset[i] = (double)sum[i] / (double)sumCount;
gltest26 6:a7ad6046824c 362 // Update member variable only if successful
gltest26 6:a7ad6046824c 363 calibSamples = sumCount;
gltest26 3:eea9733ca427 364 }
gltest26 3:eea9733ca427 365 }