hige dura
/
ITG3200_I2C
Revision 0:b89a41f096ae, committed 2012-05-15
- Comitter:
- higedura
- Date:
- Tue May 15 08:31:45 2012 +0000
- Commit message:
Changed in this revision
diff -r 000000000000 -r b89a41f096ae ITG3200.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ITG3200.cpp Tue May 15 08:31:45 2012 +0000 @@ -0,0 +1,280 @@ +/** + * @author Aaron Berk + * + * @section LICENSE + * + * Copyright (c) 2010 ARM Limited + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * @section DESCRIPTION + * + * ITG-3200 triple axis, digital interface, gyroscope. + * + * Datasheet: + * + * http://invensense.com/mems/gyro/documents/PS-ITG-3200-00-01.4.pdf + */ + +/** + * Includes + */ +#include "ITG3200.h" + +ITG3200::ITG3200(PinName sda, PinName scl) : i2c_(sda, scl) { + + //400kHz, fast mode. + i2c_.frequency(400000); + + //Set FS_SEL to 0x03 for proper operation. + //See datasheet for details. + char tx[2]; + tx[0] = DLPF_FS_REG; + //FS_SEL bits sit in bits 4 and 3 of DLPF_FS register. + tx[1] = 0x03 << 3; + + i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2); + +} + +char ITG3200::getWhoAmI(void){ + + //WhoAmI Register address. + char tx = WHO_AM_I_REG; + char rx; + + i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1); + + i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1); + + return rx; + +} + +void ITG3200::setWhoAmI(char address){ + + char tx[2]; + tx[0] = WHO_AM_I_REG; + tx[1] = address; + + i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2); + +} + +char ITG3200::getSampleRateDivider(void){ + + char tx = SMPLRT_DIV_REG; + char rx; + + i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1); + + i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1); + + return rx; + +} + +void ITG3200::setSampleRateDivider(char divider){ + + char tx[2]; + tx[0] = SMPLRT_DIV_REG; + tx[1] = divider; + + i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2); + +} + +int ITG3200::getInternalSampleRate(void){ + + char tx = DLPF_FS_REG; + char rx; + + i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1); + + i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1); + + //DLPF_CFG == 0 -> sample rate = 8kHz. + if(rx == 0){ + return 8; + } + //DLPF_CFG = 1..7 -> sample rate = 1kHz. + else if(rx >= 1 && rx <= 7){ + return 1; + } + //DLPF_CFG = anything else -> something's wrong! + else{ + return -1; + } + +} + +void ITG3200::setLpBandwidth(char bandwidth){ + + char tx[2]; + tx[0] = DLPF_FS_REG; + //Bits 4,3 are required to be 0x03 for proper operation. + tx[1] = bandwidth | (0x03 << 3); + + i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2); + +} + +char ITG3200::getInterruptConfiguration(void){ + + char tx = INT_CFG_REG; + char rx; + + i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1); + + i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1); + + return rx; + +} + +void ITG3200::setInterruptConfiguration(char config){ + + char tx[2]; + tx[0] = INT_CFG_REG; + tx[1] = config; + + i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2); + +} + +bool ITG3200::isPllReady(void){ + + char tx = INT_STATUS; + char rx; + + i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1); + + i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1); + + //ITG_RDY bit is bit 4 of INT_STATUS register. + if(rx & 0x04){ + return true; + } + else{ + return false; + } + +} + +bool ITG3200::isRawDataReady(void){ + + char tx = INT_STATUS; + char rx; + + i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1); + + i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1); + + //RAW_DATA_RDY bit is bit 1 of INT_STATUS register. + if(rx & 0x01){ + return true; + } + else{ + return false; + } + +} + +float ITG3200::getTemperature(void){ + + char tx = TEMP_OUT_H_REG; + char rx[2]; + + i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1); + + i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, rx, 2); + + int16_t temperature = ((int) rx[0] << 8) | ((int) rx[1]); + //Offset = -35 degrees, 13200 counts. 280 counts/degrees C. + return 35.0 + ((temperature + 13200)/280.0); + +} + +int ITG3200::getGyroX(void){ + + char tx = GYRO_XOUT_H_REG; + char rx[2]; + + i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1); + + i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, rx, 2); + + int16_t output = ((int) rx[0] << 8) | ((int) rx[1]); + + return output; + +} + +int ITG3200::getGyroY(void){ + + char tx = GYRO_YOUT_H_REG; + char rx[2]; + + i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1); + + i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, rx, 2); + + int16_t output = ((int) rx[0] << 8) | ((int) rx[1]); + + return output; + +} + +int ITG3200::getGyroZ(void){ + + char tx = GYRO_ZOUT_H_REG; + char rx[2]; + + i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1); + + i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, rx, 2); + + int16_t output = ((int) rx[0] << 8) | ((int) rx[1]); + + return output; + +} + +char ITG3200::getPowerManagement(void){ + + char tx = PWR_MGM_REG; + char rx; + + i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1); + + i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1); + + return rx; + +} + +void ITG3200::setPowerManagement(char config){ + + char tx[2]; + tx[0] = PWR_MGM_REG; + tx[1] = config; + + i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2); + +}
diff -r 000000000000 -r b89a41f096ae ITG3200.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ITG3200.h Tue May 15 08:31:45 2012 +0000 @@ -0,0 +1,339 @@ +/** + * @author Aaron Berk + * + * @section LICENSE + * + * Copyright (c) 2010 ARM Limited + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * @section DESCRIPTION + * + * ITG-3200 triple axis, digital interface, gyroscope. + * + * Datasheet: + * + * http://invensense.com/mems/gyro/documents/PS-ITG-3200-00-01.4.pdf + */ + +#ifndef ITG3200_H +#define ITG3200_H + +/** + * Includes + */ +#include "mbed.h" + +/** + * Defines + */ +#define ITG3200_I2C_ADDRESS 0x68 //7-bit address. + +//----------- +// Registers +//----------- +#define WHO_AM_I_REG 0x00 +#define SMPLRT_DIV_REG 0x15 +#define DLPF_FS_REG 0x16 +#define INT_CFG_REG 0x17 +#define INT_STATUS 0x1A +#define TEMP_OUT_H_REG 0x1B +#define TEMP_OUT_L_REG 0x1C +#define GYRO_XOUT_H_REG 0x1D +#define GYRO_XOUT_L_REG 0x1E +#define GYRO_YOUT_H_REG 0x1F +#define GYRO_YOUT_L_REG 0x20 +#define GYRO_ZOUT_H_REG 0x21 +#define GYRO_ZOUT_L_REG 0x22 +#define PWR_MGM_REG 0x3E + +//---------------------------- +// Low Pass Filter Bandwidths +//---------------------------- +#define LPFBW_256HZ 0x00 +#define LPFBW_188HZ 0x01 +#define LPFBW_98HZ 0x02 +#define LPFBW_42HZ 0x03 +#define LPFBW_20HZ 0x04 +#define LPFBW_10HZ 0x05 +#define LPFBW_5HZ 0x06 + +/** + * ITG-3200 triple axis digital gyroscope. + */ +class ITG3200 { + +public: + + /** + * Constructor. + * + * Sets FS_SEL to 0x03 for proper opertaion. + * + * @param sda - mbed pin to use for the SDA I2C line. + * @param scl - mbed pin to use for the SCL I2C line. + */ + ITG3200(PinName sda, PinName scl); + + /** + * Get the identity of the device. + * + * @return The contents of the Who Am I register which contains the I2C + * address of the device. + */ + char getWhoAmI(void); + + /** + * Set the address of the device. + * + * @param address The I2C slave address to write to the Who Am I register + * on the device. + */ + void setWhoAmI(char address); + + /** + * Get the sample rate divider. + * + * @return The sample rate divider as a number from 0-255. + */ + char getSampleRateDivider(void); + + /** + * Set the sample rate divider. + * + * Fsample = Finternal / (divider + 1), where Finternal = 1kHz or 8kHz, + * as decidied by the DLPF_FS register. + * + * @param The sample rate divider as a number from 0-255. + */ + void setSampleRateDivider(char divider); + + /** + * Get the internal sample rate. + * + * @return The internal sample rate in kHz - either 1 or 8. + */ + int getInternalSampleRate(void); + + /** + * Set the low pass filter bandwidth. + * + * Also used to set the internal sample rate. + * Pass the #define bandwidth codes as a parameter. + * + * 256Hz -> 8kHz internal sample rate. + * Everything else -> 1kHz internal rate. + * + * @param bandwidth Low pass filter bandwidth code + */ + void setLpBandwidth(char bandwidth); + + /** + * Get the interrupt configuration. + * + * See datasheet for register contents details. + * + * 7 6 5 4 + * +------+------+--------------+------------------+ + * | ACTL | OPEN | LATCH_INT_EN | INT_ANYRD_2CLEAR | + * +------+------+--------------+------------------+ + * + * 3 2 1 0 + * +---+------------+------------+---+ + * | 0 | ITG_RDY_EN | RAW_RDY_EN | 0 | + * +---+------------+------------+---+ + * + * ACTL Logic level for INT output pin; 1 = active low, 0 = active high. + * OPEN Drive type for INT output pin; 1 = open drain, 0 = push-pull. + * LATCH_INT_EN Latch mode; 1 = latch until interrupt is cleared, + * 0 = 50us pulse. + * INT_ANYRD_2CLEAR Latch clear method; 1 = any register read, + * 0 = status register read only. + * ITG_RDY_EN Enable interrupt when device is ready, + * (PLL ready after changing clock source). + * RAW_RDY_EN Enable interrupt when data is available. + * 0 Bits 1 and 3 of the INT_CFG register should be zero. + * + * @return the contents of the INT_CFG register. + */ + char getInterruptConfiguration(void); + + /** + * Set the interrupt configuration. + * + * See datasheet for configuration byte details. + * + * 7 6 5 4 + * +------+------+--------------+------------------+ + * | ACTL | OPEN | LATCH_INT_EN | INT_ANYRD_2CLEAR | + * +------+------+--------------+------------------+ + * + * 3 2 1 0 + * +---+------------+------------+---+ + * | 0 | ITG_RDY_EN | RAW_RDY_EN | 0 | + * +---+------------+------------+---+ + * + * ACTL Logic level for INT output pin; 1 = active low, 0 = active high. + * OPEN Drive type for INT output pin; 1 = open drain, 0 = push-pull. + * LATCH_INT_EN Latch mode; 1 = latch until interrupt is cleared, + * 0 = 50us pulse. + * INT_ANYRD_2CLEAR Latch clear method; 1 = any register read, + * 0 = status register read only. + * ITG_RDY_EN Enable interrupt when device is ready, + * (PLL ready after changing clock source). + * RAW_RDY_EN Enable interrupt when data is available. + * 0 Bits 1 and 3 of the INT_CFG register should be zero. + * + * @param config Configuration byte to write to INT_CFG register. + */ + void setInterruptConfiguration(char config); + + /** + * Check the ITG_RDY bit of the INT_STATUS register. + * + * @return True if the ITG_RDY bit is set, corresponding to PLL ready, + * false if the ITG_RDY bit is not set, corresponding to PLL not + * ready. + */ + bool isPllReady(void); + + /** + * Check the RAW_DATA_RDY bit of the INT_STATUS register. + * + * @return True if the RAW_DATA_RDY bit is set, corresponding to new data + * in the sensor registers, false if the RAW_DATA_RDY bit is not + * set, corresponding to no new data yet in the sensor registers. + */ + bool isRawDataReady(void); + + /** + * Get the temperature of the device. + * + * @return The temperature in degrees celsius. + */ + float getTemperature(void); + + /** + * Get the output for the x-axis gyroscope. + * + * Typical sensitivity is 14.375 LSB/(degrees/sec). + * + * @return The output on the x-axis in raw ADC counts. + */ + int getGyroX(void); + + /** + * Get the output for the y-axis gyroscope. + * + * Typical sensitivity is 14.375 LSB/(degrees/sec). + * + * @return The output on the y-axis in raw ADC counts. + */ + int getGyroY(void); + + /** + * Get the output on the z-axis gyroscope. + * + * Typical sensitivity is 14.375 LSB/(degrees/sec). + * + * @return The output on the z-axis in raw ADC counts. + */ + int getGyroZ(void); + + /** + * Get the power management configuration. + * + * See the datasheet for register contents details. + * + * 7 6 5 4 + * +---------+-------+---------+---------+ + * | H_RESET | SLEEP | STBY_XG | STBY_YG | + * +---------+-------+---------+---------+ + * + * 3 2 1 0 + * +---------+----------+----------+----------+ + * | STBY_ZG | CLK_SEL2 | CLK_SEL1 | CLK_SEL0 | + * +---------+----------+----------+----------+ + * + * H_RESET Reset device and internal registers to the power-up-default settings. + * SLEEP Enable low power sleep mode. + * STBY_XG Put gyro X in standby mode (1=standby, 0=normal). + * STBY_YG Put gyro Y in standby mode (1=standby, 0=normal). + * STBY_ZG Put gyro Z in standby mode (1=standby, 0=normal). + * CLK_SEL Select device clock source: + * + * CLK_SEL | Clock Source + * --------+-------------- + * 0 Internal oscillator + * 1 PLL with X Gyro reference + * 2 PLL with Y Gyro reference + * 3 PLL with Z Gyro reference + * 4 PLL with external 32.768kHz reference + * 5 PLL with external 19.2MHz reference + * 6 Reserved + * 7 Reserved + * + * @return The contents of the PWR_MGM register. + */ + char getPowerManagement(void); + + /** + * Set power management configuration. + * + * See the datasheet for configuration byte details + * + * 7 6 5 4 + * +---------+-------+---------+---------+ + * | H_RESET | SLEEP | STBY_XG | STBY_YG | + * +---------+-------+---------+---------+ + * + * 3 2 1 0 + * +---------+----------+----------+----------+ + * | STBY_ZG | CLK_SEL2 | CLK_SEL1 | CLK_SEL0 | + * +---------+----------+----------+----------+ + * + * H_RESET Reset device and internal registers to the power-up-default settings. + * SLEEP Enable low power sleep mode. + * STBY_XG Put gyro X in standby mode (1=standby, 0=normal). + * STBY_YG Put gyro Y in standby mode (1=standby, 0=normal). + * STBY_ZG Put gyro Z in standby mode (1=standby, 0=normal). + * CLK_SEL Select device clock source: + * + * CLK_SEL | Clock Source + * --------+-------------- + * 0 Internal oscillator + * 1 PLL with X Gyro reference + * 2 PLL with Y Gyro reference + * 3 PLL with Z Gyro reference + * 4 PLL with external 32.768kHz reference + * 5 PLL with external 19.2MHz reference + * 6 Reserved + * 7 Reserved + * + * @param config The configuration byte to write to the PWR_MGM register. + */ + void setPowerManagement(char config); + +private: + + I2C i2c_; + +}; + +#endif /* ITG3200_H */
diff -r 000000000000 -r b89a41f096ae main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue May 15 08:31:45 2012 +0000 @@ -0,0 +1,51 @@ +#include "ITG3200.h" + +Serial pc(USBTX, USBRX); +ITG3200 gyro(p9, p10); + +int main() { + + float t = 0; + float dt = 0.1; + double Gyro0 [3] = {0, 0, 0}; + double Gyro1 [3] = {0, 0, 0}; + double Ang0 [3] = {0, 0, 0}; + double Ang1 [3] = {0, 0, 0}; + + pc.printf("Now starting ITG-3200 test\n"); + pc.printf(" Unit: Gyro[deg/s] Angle[deg]\n"); + pc.printf(" Time GyroX GyroY GyroZ AngX AngY AngZ\n"); + + //Set highest bandwidth. + gyro.setLpBandwidth(LPFBW_42HZ); + + //Arbitrary wait for printf clarity. + wait(0.1); + + while (1) { + //pc.printf("%x\n",gyro.getWhoAmI()); + // YAL 9DOF: x+16, y-39, z+40 + // My 9DOF: x, y, z + Gyro1[0] = (gyro.getGyroX()+16)/14.375; Gyro1[1] = (gyro.getGyroY()-39)/14.375; Gyro1[2] = (gyro.getGyroZ()+40)/14.375; + + // Low pass filter for gyro + if( -1.0<Gyro1[0] && Gyro1[0]<1.0 ){ Gyro1[0]=0; } + if( -1.0<Gyro1[1] && Gyro1[1]<1.0 ){ Gyro1[1]=0; } + if( -1.0<Gyro1[2] && Gyro1[2]<1.0 ){ Gyro1[2]=0; } + + // Trapezoidal integration + Ang1[0] = Ang0[0]+(Gyro0[0]+Gyro1[0])*dt/2; + Ang1[1] = Ang0[1]+(Gyro0[1]+Gyro1[1])*dt/2; + Ang1[2] = Ang0[2]+(Gyro0[2]+Gyro1[2])*dt/2; + + pc.printf("%6.1f, %5.0f, %5.0f, %5.0f, %6.1f, %6.1f, %6.1f\n\r", + t, Gyro1[0], Gyro1[1], Gyro1[2], Ang1[0], Ang1[1], Ang1[2]); + + Gyro0[0] = Gyro1[0]; Gyro0[1] = Gyro1[1]; Gyro0[2] = Gyro1[2]; + Ang0[0] = Ang1[0]; Ang0[1] = Ang1[1]; Ang0[2] = Ang1[2]; + t = t+dt; + wait(0.1); + + } + +}
diff -r 000000000000 -r b89a41f096ae mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Tue May 15 08:31:45 2012 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/9114680c05da