ITG3200.cpp

00001 /**
00002  * @file ITG3200.cpp
00003  * @author Tyler Weaver
00004  *
00005  * @section LICENSE
00006  *
00007  * Copyright (c) 2010 ARM Limited
00008  *
00009  * Permission is hereby granted, free of charge, to any person obtaining a copy
00010  * of this software and associated documentation files (the "Software"), to deal
00011  * in the Software without restriction, including without limitation the rights
00012  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00013  * copies of the Software, and to permit persons to whom the Software is
00014  * furnished to do so, subject to the following conditions:
00015  *
00016  * The above copyright notice and this permission notice shall be included in
00017  * all copies or substantial portions of the Software.
00018  *
00019  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00020  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00021  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00022  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00023  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00024  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00025  * THE SOFTWARE.
00026  *
00027  * @section DESCRIPTION
00028  *
00029  * Modified version of Aaron Berk's library.
00030  * ITG-3200 triple axis, digital interface, gyroscope.
00031  *
00032  * Datasheet:
00033  *
00034  * http://invensense.com/mems/gyro/documents/PS-ITG-3200-00-01.4.pdf
00035  */
00036 
00037 #include "ITG3200.h "
00038 #include <new>
00039 
00040 ITG3200::ITG3200(PinName sda, PinName scl, bool fastmode) : calibSamples(0), i2c_(*reinterpret_cast<I2C*>(i2cRaw))
00041 {
00042     // Placement new to avoid additional heap memory allocation.
00043     new(i2cRaw) I2C(sda, scl);
00044 
00045     offset[0] = offset[1] = offset[2] = 0.0;
00046 
00047     if(fastmode) {
00048         //400kHz, fast mode.
00049         i2c_.frequency(400000);
00050     } else
00051         i2c_.frequency(100000);
00052 }
00053 
00054 ITG3200::~ITG3200()
00055 {
00056     // If the I2C object is initialized in the buffer in this object, call destructor of it.
00057     if(&i2c_ == reinterpret_cast<I2C*>(&i2cRaw))
00058         reinterpret_cast<I2C*>(&i2cRaw)->~I2C();
00059 }
00060 
00061 void ITG3200::init()
00062 {
00063     //Set FS_SEL to 0x03 for proper operation.
00064     //See datasheet for details.
00065     char tx[2];
00066     
00067     // Power up reset defaults
00068     tx[0] = 0x3E; // Power management
00069     tx[1] = 0x80; // ?
00070     i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
00071     wait_ms(5);
00072     
00073     // Select full-scale range of the gyro sensors
00074     // Set LP filter bandwidth to 42Hz
00075     tx[0] = 0x16; //
00076     tx[1] = 0x1B; // DLPF_CFG = 3, FS_SEL = 3
00077     i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
00078     wait_ms(5);
00079     
00080     // set sample rate to 50 Hz
00081     tx[0] = 0x15; //
00082     tx[1] = 0x0A; //  SMPLRT_DIV = 10 (50Hz)
00083     i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
00084     wait_ms(5);
00085     
00086     // Set clock to PLL with z gyro reference
00087     tx[0] = 0x3E;
00088     tx[1] = 0x00;
00089     i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
00090     wait_ms(5);
00091     
00092     const float offset[3] = {99.5, -45.0, -29.7}; // taken from itg3200.xls curve fit test
00093     const float slope[3] = {-1.05, 0.95, 0.47};
00094 
00095     setCalibrationCurve(offset, slope);
00096 }
00097 
00098 void ITG3200::setCalibrationCurve(const float offset[3], const float slope[3])
00099 {
00100     if(offset) {
00101         for(int i = 0; i < 3; i++)
00102             this->foffset[i] = offset[i];
00103     }
00104     if(slope) {
00105         for(int i = 0; i < 3; i++)
00106             this->slope[i] = slope[i];
00107     }
00108 }
00109 
00110 char ITG3200::getWhoAmI(void)
00111 {
00112 
00113     //WhoAmI Register address.
00114     char tx = WHO_AM_I_REG;
00115     char rx;
00116 
00117     i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
00118 
00119     i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
00120 
00121     return rx;
00122 
00123 }
00124 
00125 void ITG3200::setWhoAmI(char address)
00126 {
00127 
00128     char tx[2];
00129     tx[0] = WHO_AM_I_REG;
00130     tx[1] = address;
00131 
00132     i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
00133 
00134 }
00135 
00136 char ITG3200::getSampleRateDivider(void)
00137 {
00138 
00139     char tx = SMPLRT_DIV_REG;
00140     char rx;
00141 
00142     i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
00143 
00144     i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
00145 
00146     return rx;
00147 
00148 }
00149 
00150 void ITG3200::setSampleRateDivider(char divider)
00151 {
00152 
00153     char tx[2];
00154     tx[0] = SMPLRT_DIV_REG;
00155     tx[1] = divider;
00156 
00157     i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
00158 
00159 }
00160 
00161 int ITG3200::getInternalSampleRate(void)
00162 {
00163 
00164     char tx = DLPF_FS_REG;
00165     char rx;
00166 
00167     i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
00168 
00169     i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
00170 
00171     //DLPF_CFG == 0 -> sample rate = 8kHz.
00172     if(rx == 0) {
00173         return 8;
00174     }
00175     //DLPF_CFG = 1..7 -> sample rate = 1kHz.
00176     else if(rx >= 1 && rx <= 7) {
00177         return 1;
00178     }
00179     //DLPF_CFG = anything else -> something's wrong!
00180     else {
00181         return -1;
00182     }
00183 
00184 }
00185 
00186 void ITG3200::setLpBandwidth(char bandwidth)
00187 {
00188 
00189     char tx[2];
00190     tx[0] = DLPF_FS_REG;
00191     //Bits 4,3 are required to be 0x03 for proper operation.
00192     tx[1] = bandwidth | (0x03 << 3);
00193 
00194     i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
00195 
00196 }
00197 
00198 char ITG3200::getInterruptConfiguration(void)
00199 {
00200 
00201     char tx = INT_CFG_REG;
00202     char rx;
00203 
00204     i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
00205 
00206     i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
00207 
00208     return rx;
00209 
00210 }
00211 
00212 void ITG3200::setInterruptConfiguration(char config)
00213 {
00214 
00215     char tx[2];
00216     tx[0] = INT_CFG_REG;
00217     tx[1] = config;
00218 
00219     i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
00220 
00221 }
00222 
00223 bool ITG3200::isPllReady(void)
00224 {
00225 
00226     char tx = INT_STATUS;
00227     char rx;
00228 
00229     i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
00230 
00231     i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
00232 
00233     //ITG_RDY bit is bit 4 of INT_STATUS register.
00234     if(rx & 0x04) {
00235         return true;
00236     } else {
00237         return false;
00238     }
00239 
00240 }
00241 
00242 bool ITG3200::isRawDataReady(void)
00243 {
00244 
00245     char tx = INT_STATUS;
00246     char rx;
00247 
00248     i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
00249 
00250     i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
00251 
00252     //RAW_DATA_RDY bit is bit 1 of INT_STATUS register.
00253     if(rx & 0x01) {
00254         return true;
00255     } else {
00256         return false;
00257     }
00258 
00259 }
00260 
00261 int ITG3200::getWord(int regi)
00262 {
00263 
00264     char tx = regi;
00265     char rx[2];
00266 
00267     i2c_.write(I2C_ADDRESS, &tx, 1);
00268 
00269     i2c_.read(I2C_ADDRESS, rx, 2);
00270 
00271     return swapExtend(rx);
00272 }
00273 
00274 float ITG3200::getTemperature()
00275 {
00276     //Offset = -35 degrees, 13200 counts. 280 counts/degrees C.
00277     return 35.0 + ((getRawTemperature() + 13200)/280.0);
00278 
00279 }
00280 
00281 void ITG3200::getRawXYZ(int16_t readings[3])
00282 {
00283 
00284     char tx = GYRO_XOUT_H_REG;
00285     char rx[2];
00286 
00287     i2c_.write(I2C_ADDRESS, &tx, 1);
00288 
00289     i2c_.read(I2C_ADDRESS, rx, 6);
00290 
00291     for(int i = 0; i < 3; i++)
00292         readings[i] = swapExtend(&rx[i * 2]);
00293 }
00294 
00295 void ITG3200::getXYZ(int16_t readings[3], Correction corr)
00296 {
00297     getRawXYZ(readings);
00298     switch(corr) {
00299         case OffsetCorrection:
00300             for(int i = 0; i < 3; i++)
00301                 readings[i] -= static_cast<int16_t>(offset[i]);
00302             break;
00303         case Calibration: {
00304             float temp = getTemperature();
00305             for(int i = 0; i < 3; i++)
00306                 readings[i] -= slope[i] * temp + foffset[i];
00307         }
00308         break;
00309     }
00310 }
00311 
00312 
00313 char ITG3200::getPowerManagement(void)
00314 {
00315 
00316     char tx = PWR_MGM_REG;
00317     char rx;
00318 
00319     i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
00320 
00321     i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
00322 
00323     return rx;
00324 
00325 }
00326 
00327 void ITG3200::setPowerManagement(char config)
00328 {
00329 
00330     char tx[2];
00331     tx[0] = PWR_MGM_REG;
00332     tx[1] = config;
00333 
00334     i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
00335 
00336 }
00337 
00338 void ITG3200::calibrate(double time)
00339 {
00340     long sum[3] = {0};
00341     int sumCount = 0;
00342     Timer t;
00343     t.start();
00344     float start_time;
00345     float period = 1.0 / 50.0; // 50Hz
00346     
00347     while(t.read() < time) {
00348         start_time = t.read();
00349         int16_t gyro[3];
00350         getRawXYZ(gyro);
00351         for(int i = 0; i < 3; i++)
00352             sum[i] += gyro[i];
00353         sumCount++;
00354         wait(period - (t.read()-start_time));
00355     }
00356     t.stop();
00357 
00358     // Avoid zero division
00359     if(0 < sumCount) {
00360         for(int i = 0; i < 3; i++)
00361             offset[i] = (double)sum[i] / (double)sumCount;
00362         // Update member variable only if successful
00363         calibSamples = sumCount;
00364     }
00365 }