Forked from Aaron Berk's ITG3200 driver class library, customized for my specific application using 9DoF-Stick by Sparkfun.
Fork of ITG3200 by
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 */
Generated on Wed Jul 20 2022 11:54:17 by 1.7.2