ITG3200.h

00001 /**
00002  * @file ITG3200.h
00003  * @author Aaron Berk
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  * ITG-3200 triple axis, digital interface, gyroscope.
00030  * Forked from Aaron Berk's work.
00031  *
00032  * Datasheet:
00033  *
00034  * http://invensense.com/mems/gyro/documents/PS-ITG-3200-00-01.4.pdf
00035  */
00036 
00037 #ifndef ITG3200_H
00038 #define ITG3200_H
00039 
00040 /*
00041  * Includes
00042  */
00043 #include "mbed.h"
00044 
00045 /*
00046  * Defines
00047  */
00048 #define ITG3200_I2C_ADDRESS 0x68 //7-bit address.
00049 
00050 //-----------
00051 // Registers
00052 //-----------
00053 #define WHO_AM_I_REG    0x00
00054 #define SMPLRT_DIV_REG  0x15
00055 #define DLPF_FS_REG     0x16
00056 #define INT_CFG_REG     0x17
00057 #define INT_STATUS      0x1A
00058 #define TEMP_OUT_H_REG  0x1B
00059 #define TEMP_OUT_L_REG  0x1C
00060 #define GYRO_XOUT_H_REG 0x1D
00061 #define GYRO_XOUT_L_REG 0x1E
00062 #define GYRO_YOUT_H_REG 0x1F
00063 #define GYRO_YOUT_L_REG 0x20
00064 #define GYRO_ZOUT_H_REG 0x21
00065 #define GYRO_ZOUT_L_REG 0x22
00066 #define PWR_MGM_REG     0x3E
00067 
00068 //----------------------------
00069 // Low Pass Filter Bandwidths
00070 //----------------------------
00071 #define LPFBW_256HZ 0x00
00072 #define LPFBW_188HZ 0x01
00073 #define LPFBW_98HZ  0x02
00074 #define LPFBW_42HZ  0x03
00075 #define LPFBW_20HZ  0x04
00076 #define LPFBW_10HZ  0x05
00077 #define LPFBW_5HZ   0x06
00078 
00079 // Utility
00080 #ifndef M_PI
00081 #define M_PI 3.1415926535897932384626433832795
00082 #endif
00083 
00084 /**
00085  * ITG-3200 triple axis digital gyroscope.
00086  */
00087 class ITG3200 {
00088 
00089 public:
00090 
00091     /**
00092      * The I2C address that can be passed directly to i2c object (it's already shifted 1 bit left).
00093      *
00094      * You don't need to manually set or clear the LSB when calling I2C::read() or I2C::write(),
00095      * the library takes care of it.  We just always clear the LSB.
00096      */
00097     static const int I2C_ADDRESS = 0xD0;
00098     
00099     /**
00100      * @brief Zero offset correction mode that can be specified when calling getGyroXYZ().
00101      *
00102      * The device has a major drift in readings depending on ambient temperature.
00103      * You can measure the temperature with built-in thermometer to correct it, but you
00104      * must have calibration curves for each axes to do so.
00105      * Here are the options on how to correct the drift.
00106      */
00107     enum Correction{
00108         NoCorrection, ///< Do not correct zero offset at all; You would have trouble integrating the values to obtain rotation.
00109         OffsetCorrection, ///< Correct the outputs with single-point zero adjust.
00110         Calibration, ///< Use calibration curve (actually lines) to correct the outputs. You must provide coefficients with setCalibrationCurve().
00111         Num_Correction
00112     };
00113 
00114     /**
00115      * Constructor.
00116      *
00117      * Sets FS_SEL to 0x03 for proper opertaion.
00118      *
00119      * @param sda - mbed pin to use for the SDA I2C line.
00120      * @param scl - mbed pin to use for the SCL I2C line.
00121      * @param fastmode Sets the internal I2C interface to use 400kHz clock.
00122      */
00123     ITG3200(PinName sda, PinName scl, bool fastmode = false);
00124     
00125     /**
00126      * Constructor that accepts external i2c interface object.
00127      *
00128      * @param i2c The I2C interface object to use.
00129      */
00130     ITG3200(I2C &i2c) : i2c_(i2c){
00131         init();
00132     }
00133     
00134     ~ITG3200();
00135     
00136     void init();
00137     
00138     /**
00139      * Sets calibration curve parameters.
00140      *
00141      * @param offset An array holding calibration curve offsets (0th-order coefficient) for each axis, must have 3 elements.
00142      * @param slope An array holding calibration curve slopes (1st-order coefficient) for each axis, must have 3 elements.
00143      */
00144     void setCalibrationCurve(const float offset[3], const float slope[3]);
00145 
00146     /**
00147      * Get the identity of the device.
00148      *
00149      * @return The contents of the Who Am I register which contains the I2C
00150      *         address of the device.
00151      */
00152     char getWhoAmI(void);
00153 
00154     /**
00155      * Set the address of the device.
00156      *
00157      * @param address The I2C slave address to write to the Who Am I register
00158      *        on the device.
00159      */
00160     void setWhoAmI(char address);
00161 
00162     /**
00163      * Get the sample rate divider.
00164      *
00165      * @return The sample rate divider as a number from 0-255.
00166      */
00167     char getSampleRateDivider(void);
00168 
00169     /**
00170      * Set the sample rate divider.
00171      *
00172      * Fsample = Finternal / (divider + 1), where Finternal = 1kHz or 8kHz,
00173      * as decidied by the DLPF_FS register.
00174      *
00175      * @param The sample rate divider as a number from 0-255.
00176      */
00177     void setSampleRateDivider(char divider);
00178 
00179     /**
00180      * Get the internal sample rate.
00181      *
00182      * @return The internal sample rate in kHz - either 1 or 8.
00183      */
00184     int getInternalSampleRate(void);
00185 
00186     /**
00187      * Set the low pass filter bandwidth.
00188      *
00189      * Also used to set the internal sample rate.
00190      * Pass the #define bandwidth codes as a parameter.
00191      *
00192      * 256Hz -> 8kHz internal sample rate.
00193      * Everything else -> 1kHz internal rate.
00194      *
00195      * @param bandwidth Low pass filter bandwidth code
00196      */
00197     void setLpBandwidth(char bandwidth);
00198 
00199     /**
00200      * Get the interrupt configuration.
00201      *
00202      * See datasheet for register contents details.
00203      *
00204      *    7      6           5                 4
00205      * +------+------+--------------+------------------+
00206      * | ACTL | OPEN | LATCH_INT_EN | INT_ANYRD_2CLEAR |
00207      * +------+------+--------------+------------------+
00208      *
00209      *   3        2            1       0
00210      * +---+------------+------------+---+
00211      * | 0 | ITG_RDY_EN | RAW_RDY_EN | 0 |
00212      * +---+------------+------------+---+
00213      *
00214      * ACTL Logic level for INT output pin; 1 = active low, 0 = active high.
00215      * OPEN Drive type for INT output pin; 1 = open drain, 0 = push-pull.
00216      * LATCH_INT_EN Latch mode; 1 = latch until interrupt is cleared,
00217      *                          0 = 50us pulse.
00218      * INT_ANYRD_2CLEAR Latch clear method; 1 = any register read,
00219      *                                      0 = status register read only.
00220      * ITG_RDY_EN Enable interrupt when device is ready,
00221      *            (PLL ready after changing clock source).
00222      * RAW_RDY_EN Enable interrupt when data is available.
00223      * 0 Bits 1 and 3 of the INT_CFG register should be zero.
00224      *
00225      * @return the contents of the INT_CFG register.
00226      */
00227     char getInterruptConfiguration(void);
00228 
00229     /**
00230      * Set the interrupt configuration.
00231      *
00232      * See datasheet for configuration byte details.
00233      *
00234      *    7      6           5                 4
00235      * +------+------+--------------+------------------+
00236      * | ACTL | OPEN | LATCH_INT_EN | INT_ANYRD_2CLEAR |
00237      * +------+------+--------------+------------------+
00238      *
00239      *   3        2            1       0
00240      * +---+------------+------------+---+
00241      * | 0 | ITG_RDY_EN | RAW_RDY_EN | 0 |
00242      * +---+------------+------------+---+
00243      *
00244      * ACTL Logic level for INT output pin; 1 = active low, 0 = active high.
00245      * OPEN Drive type for INT output pin; 1 = open drain, 0 = push-pull.
00246      * LATCH_INT_EN Latch mode; 1 = latch until interrupt is cleared,
00247      *                          0 = 50us pulse.
00248      * INT_ANYRD_2CLEAR Latch clear method; 1 = any register read,
00249      *                                      0 = status register read only.
00250      * ITG_RDY_EN Enable interrupt when device is ready,
00251      *            (PLL ready after changing clock source).
00252      * RAW_RDY_EN Enable interrupt when data is available.
00253      * 0 Bits 1 and 3 of the INT_CFG register should be zero.
00254      *
00255      * @param config Configuration byte to write to INT_CFG register.
00256      */
00257     void setInterruptConfiguration(char config);
00258 
00259     /**
00260      * Check the ITG_RDY bit of the INT_STATUS register.
00261      *
00262      * @return True if the ITG_RDY bit is set, corresponding to PLL ready,
00263      *         false if the ITG_RDY bit is not set, corresponding to PLL not
00264      *         ready.
00265      */
00266     bool isPllReady(void);
00267 
00268     /**
00269      * Check the RAW_DATA_RDY bit of the INT_STATUS register.
00270      *
00271      * @return True if the RAW_DATA_RDY bit is set, corresponding to new data
00272      *         in the sensor registers, false if the RAW_DATA_RDY bit is not
00273      *         set, corresponding to no new data yet in the sensor registers.
00274      */
00275     bool isRawDataReady(void);
00276 
00277     /**
00278      * Get the temperature in raw format.
00279      *
00280      * @return The temperature in raw 16bit integer.
00281      */
00282     int getRawTemperature(void){ return getWord(TEMP_OUT_H_REG); }
00283 
00284     /**
00285      * Get the temperature of the device.
00286      *
00287      * @return The temperature in degrees celsius.
00288      */
00289     float getTemperature(void);
00290 
00291     /**
00292      * Get the output for the x-axis gyroscope.
00293      *
00294      * Typical sensitivity is 14.375 LSB/(degrees/sec).
00295      *
00296      * @return The output on the x-axis in raw ADC counts.
00297      */
00298     int getGyroX(void){ return getWord(GYRO_XOUT_H_REG); }
00299 
00300     /**
00301      * Get the output for the y-axis gyroscope.
00302      *
00303      * Typical sensitivity is 14.375 LSB/(degrees/sec).
00304      *
00305      * @return The output on the y-axis in raw ADC counts.
00306      */
00307     int getGyroY(void){ return getWord(GYRO_YOUT_H_REG); }
00308 
00309     /**
00310      * Get the output on the z-axis gyroscope.
00311      *
00312      * Typical sensitivity is 14.375 LSB/(degrees/sec).
00313      * 
00314      * @return The output on the z-axis in raw ADC counts.
00315      */
00316     int getGyroZ(void){ return getWord(GYRO_ZOUT_H_REG); }
00317 
00318     /**
00319      * Burst read the outputs on the x,y,z-axis gyroscope.
00320      *
00321      * Typical sensitivity is 14.375 LSB/(degrees/sec).
00322      * 
00323      * @param readings The output buffer array that has at least 3 length.
00324      * @param corr Correction method for returned values.
00325      */
00326     void getGyroXYZ(int readings[3], Correction corr = OffsetCorrection);
00327 
00328     /**
00329      * Burst read the outputs on the x,y,z-axis gyroscope and convert them into degrees per second.
00330      *
00331      * @param readings The output buffer array that has at least 3 length.
00332      * @param corr Correction method for returned values.
00333      */
00334     void getGyroXYZDegrees(double readings[3], Correction corr = OffsetCorrection);
00335 
00336     /**
00337      * Burst read the outputs on the x,y,z-axis gyroscope and convert them into degrees per second.
00338      *
00339      * @param readings The output buffer array that has at least 3 length.
00340      * @param corr Correction method for returned values.
00341      */
00342     void getGyroXYZRadians(double readings[3], Correction corr = OffsetCorrection);
00343 
00344     /**
00345      * Get the power management configuration.
00346      *
00347      * See the datasheet for register contents details.
00348      *
00349      *     7        6        5         4
00350      * +---------+-------+---------+---------+
00351      * | H_RESET | SLEEP | STBY_XG | STBY_YG |
00352      * +---------+-------+---------+---------+
00353      *
00354      *      3          2         1          0
00355      * +---------+----------+----------+----------+
00356      * | STBY_ZG | CLK_SEL2 | CLK_SEL1 | CLK_SEL0 |
00357      * +---------+----------+----------+----------+
00358      *
00359      * H_RESET Reset device and internal registers to the power-up-default settings.
00360      * SLEEP Enable low power sleep mode.
00361      * STBY_XG Put gyro X in standby mode (1=standby, 0=normal).
00362      * STBY_YG Put gyro Y in standby mode (1=standby, 0=normal).
00363      * STBY_ZG Put gyro Z in standby mode (1=standby, 0=normal).
00364      * CLK_SEL Select device clock source:
00365      *
00366      * CLK_SEL | Clock Source
00367      * --------+--------------
00368      *    0      Internal oscillator
00369      *    1      PLL with X Gyro reference
00370      *    2      PLL with Y Gyro reference
00371      *    3      PLL with Z Gyro reference
00372      *    4      PLL with external 32.768kHz reference
00373      *    5      PLL with external 19.2MHz reference
00374      *    6      Reserved
00375      *    7      Reserved
00376      *
00377      * @return The contents of the PWR_MGM register.
00378      */
00379     char getPowerManagement(void);
00380 
00381     /**
00382      * Set power management configuration.
00383      *
00384      * See the datasheet for configuration byte details
00385      *
00386      *      7        6        5         4
00387      * +---------+-------+---------+---------+
00388      * | H_RESET | SLEEP | STBY_XG | STBY_YG |
00389      * +---------+-------+---------+---------+
00390      *
00391      *      3          2         1          0
00392      * +---------+----------+----------+----------+
00393      * | STBY_ZG | CLK_SEL2 | CLK_SEL1 | CLK_SEL0 |
00394      * +---------+----------+----------+----------+
00395      *
00396      * H_RESET Reset device and internal registers to the power-up-default settings.
00397      * SLEEP Enable low power sleep mode.
00398      * STBY_XG Put gyro X in standby mode (1=standby, 0=normal).
00399      * STBY_YG Put gyro Y in standby mode (1=standby, 0=normal).
00400      * STBY_ZG Put gyro Z in standby mode (1=standby, 0=normal).
00401      * CLK_SEL Select device clock source:
00402      *
00403      * CLK_SEL | Clock Source
00404      * --------+--------------
00405      *    0      Internal oscillator
00406      *    1      PLL with X Gyro reference
00407      *    2      PLL with Y Gyro reference
00408      *    3      PLL with Z Gyro reference
00409      *    4      PLL with external 32.768kHz reference
00410      *    5      PLL with external 19.2MHz reference
00411      *    6      Reserved
00412      *    7      Reserved
00413      *
00414      * @param config The configuration byte to write to the PWR_MGM register.
00415      */
00416     void setPowerManagement(char config);
00417     
00418     /**
00419      * Calibrate the sensor drift by sampling zero offset.
00420      *
00421      * Be sure to keep the sensor stationary while sampling offset.
00422      *
00423      * Once this function is invoked, following getGyroXYZ*() functions return
00424      * corrected values.
00425      *
00426      * If the drift value changes over time, you can call this function once in a while
00427      * to follow it. But don't forget to fix the sensor while calibrating!
00428      *
00429      * @param time The time span to sample and average offset.
00430      *             Sampling rate is limited, so giving long time to calibrate will improve
00431      *             correction quality.
00432      */
00433     void calibrate(double time);
00434     
00435     long getCalibrationSamples()const{
00436         return calibSamples;
00437     }
00438 
00439     /**
00440      * Returns the I2C object that this object is using for communication.
00441      */
00442     I2C &getI2C(){
00443         return i2c_;
00444     }
00445     
00446     /**
00447      * Returns internal offset values for zero adjusting. Returned pointer is pointing an array of 3 elements.
00448      */
00449     const int *getOffset()const{
00450         return offset;
00451     }
00452 
00453 protected:
00454 
00455     /**
00456      * Reads a word (2 bytes) from the sensor via I2C bus.
00457      *
00458      * The queried value is assumed big-endian, 2's complement value.
00459      *
00460      * This protected function is added because we shouldn't write getGyroX(), getGyroY() and getGyroZ()
00461      * independently, but collect common codes.
00462      *
00463      * @param regi Register address to be read.
00464      */
00465     int getWord(int regi);
00466 
00467     /**
00468      * An internal method to acquire gyro sensor readings before calibration correction.
00469      *
00470      * Protected for the time being, although there could be cases that raw values are
00471      * appreciated by the user.
00472      */
00473     void getRawGyroXYZ(int readings[3]);
00474 
00475     /**
00476      * Offset values that will be subtracted from output.
00477      *
00478      * TODO: temperature drift calibration
00479      */
00480     int offset[3];
00481     
00482     float foffset[3];
00483     float slope[3];
00484     
00485     long calibSamples;
00486     
00487 private:
00488 
00489     I2C &i2c_;
00490     
00491     /**
00492      * The raw buffer for allocating I2C object in its own without heap memory.
00493      */
00494     char i2cRaw[sizeof(I2C)];
00495 
00496     /**
00497      * Converts big-endian 2's complement byte pair to native byte order of
00498      * the CPU and then sign extend it to the CPU's register size.
00499      *
00500      * Implemented here to make the compiler inline expand it.
00501      */
00502     int swapExtend(const char rx[2]){
00503         // Readings are expressed in 16bit 2's complement, so we must first
00504         // concatenate two bytes to make a word and sign extend it to obtain
00505         // correct negative values.
00506         // ARMCC compiles char as unsigned, which means no sign extension is
00507         // performed during bitwise operations to chars. But we should make sure
00508         // that lower byte won't extend its sign past upper byte for other
00509         // compilers if we want to keep it portable.
00510         return int16_t(((unsigned char)rx[0] << 8) | (unsigned char)rx[1]);
00511     }
00512 };
00513 
00514 
00515 inline void ITG3200::getGyroXYZDegrees(double readings[3], Correction corr){
00516     int intData[3];
00517     getGyroXYZ(intData, corr);
00518     for(int i = 0; i < 3; i++)
00519         readings[i] = intData[i] * 2000. / 32767.;
00520 }
00521 
00522 inline void ITG3200::getGyroXYZRadians(double readings[3], Correction corr){
00523     int intData[3];
00524     getGyroXYZ(intData, corr);
00525     for(int i = 0; i < 3; i++)
00526         readings[i] = intData[i] * 2000. / 32767. * 2. * M_PI / 360.;
00527 }
00528 
00529 #endif /* ITG3200_H */