0,0 → 1,328 |
/**************************************************************************** |
* ITG3200.h - ITG-3200/I2C library v0.5 for Arduino * |
* Copyright 2010-2011 Filipe Vieira & various contributors * |
* http://code.google.com/p/itg-3200driver * |
* This file is part of ITG-3200 Arduino library. * |
* * |
* This library is free software: you can redistribute it and/or modify * |
* it under the terms of the GNU Lesser General Public License as published * |
* by the Free Software Foundation, either version 3 of the License, or * |
* (at your option) any later version. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU Lesser General Public License for more details. * |
* * |
* You should have received a copy of the GNU Lesser General Public License * |
* along with this program. If not, see <http://www.gnu.org/licenses/>. * |
****************************************************************************/ |
/**************************************************************************** |
* Tested on Arduino Mega with ITG-3200 Breakout * |
* SCL -> pin 21 (no pull up resistors) * |
* SDA -> pin 20 (no pull up resistors) * |
* CLK & GND -> pin GND * |
* INT -> not connected (but can be used) * |
* VIO & VDD -> pin 3.3V * |
*****************************************************************************/ |
#include "ITG3200.h" |
#include <Wire.h> |
|
|
ITG3200::ITG3200() { |
setGains(1.0,1.0,1.0); |
setOffsets(0.0,0.0,0.0); |
setRevPolarity(0,0,0); |
//Wire.begin(); //Normally this code is called from setup() at user code |
//but some people reported that joining I2C bus earlier |
//apparently solved problems with master/slave conditions. |
//Uncomment if needed. |
} |
|
void ITG3200::init(unsigned int address) { |
// Uncomment or change your default ITG3200 initialization |
|
// fast sample rate - divisor = 0 filter = 0 clocksrc = 0, 1, 2, or 3 (raw values) |
init(address, NOSRDIVIDER, RANGE2000, BW256_SR8, PLL_XGYRO_REF, true, true); |
|
// slow sample rate - divisor = 0 filter = 1,2,3,4,5, or 6 clocksrc = 0, 1, 2, or 3 (raw values) |
//init(NOSRDIVIDER, RANGE2000, BW010_SR1, INTERNALOSC, true, true); |
|
// fast sample rate 32Khz external clock - divisor = 0 filter = 0 clocksrc = 4 (raw values) |
//init(NOSRDIVIDER, RANGE2000, BW256_SR8, PLL_EXTERNAL32, true, true); |
|
// slow sample rate 32Khz external clock - divisor = 0 filter = 1,2,3,4,5, or 6 clocksrc = 4 (raw values) |
//init(NOSRDIVIDER, RANGE2000, BW010_SR1, PLL_EXTERNAL32, true, true); |
} |
|
void ITG3200::init(unsigned int address, byte _SRateDiv, byte _Range, byte _filterBW, byte _ClockSrc, bool _ITGReady, bool _INTRawDataReady) { |
_dev_address = address; |
setSampleRateDiv(_SRateDiv); |
setFSRange(_Range); |
setFilterBW(_filterBW); |
setClockSource(_ClockSrc); |
setITGReady(_ITGReady); |
setRawDataReady(_INTRawDataReady); |
delay(GYROSTART_UP_DELAY); // startup |
} |
|
byte ITG3200::getDevAddr() { |
/*readmem(WHO_AM_I, 1, &_buff[0]); |
return _buff[0]; */ |
return _dev_address; |
} |
|
void ITG3200::setDevAddr(unsigned int _addr) { |
writemem(WHO_AM_I, _addr); |
_dev_address = _addr; |
} |
|
byte ITG3200::getSampleRateDiv() { |
readmem(SMPLRT_DIV, 1, &_buff[0]); |
return _buff[0]; |
} |
|
void ITG3200::setSampleRateDiv(byte _SampleRate) { |
writemem(SMPLRT_DIV, _SampleRate); |
} |
|
byte ITG3200::getFSRange() { |
readmem(DLPF_FS, 1, &_buff[0]); |
return ((_buff[0] & DLPFFS_FS_SEL) >> 3); |
} |
|
void ITG3200::setFSRange(byte _Range) { |
readmem(DLPF_FS, 1, &_buff[0]); |
writemem(DLPF_FS, ((_buff[0] & ~DLPFFS_FS_SEL) | (_Range << 3)) ); |
} |
|
byte ITG3200::getFilterBW() { |
readmem(DLPF_FS, 1, &_buff[0]); |
return (_buff[0] & DLPFFS_DLPF_CFG); |
} |
|
void ITG3200::setFilterBW(byte _BW) { |
readmem(DLPF_FS, 1, &_buff[0]); |
writemem(DLPF_FS, ((_buff[0] & ~DLPFFS_DLPF_CFG) | _BW)); |
} |
|
bool ITG3200::isINTActiveOnLow() { |
readmem(INT_CFG, 1, &_buff[0]); |
return ((_buff[0] & INTCFG_ACTL) >> 7); |
} |
|
void ITG3200::setINTLogiclvl(bool _State) { |
readmem(INT_CFG, 1, &_buff[0]); |
writemem(INT_CFG, ((_buff[0] & ~INTCFG_ACTL) | (_State << 7))); |
} |
|
bool ITG3200::isINTOpenDrain() { |
readmem(INT_CFG, 1, &_buff[0]); |
return ((_buff[0] & INTCFG_OPEN) >> 6); |
} |
|
void ITG3200::setINTDriveType(bool _State) { |
readmem(INT_CFG, 1, &_buff[0]); |
writemem(INT_CFG, ((_buff[0] & ~INTCFG_OPEN) | _State << 6)); |
} |
|
bool ITG3200::isLatchUntilCleared() { |
readmem(INT_CFG, 1, &_buff[0]); |
return ((_buff[0] & INTCFG_LATCH_INT_EN) >> 5); |
} |
|
void ITG3200::setLatchMode(bool _State) { |
readmem(INT_CFG, 1, &_buff[0]); |
writemem(INT_CFG, ((_buff[0] & ~INTCFG_LATCH_INT_EN) | _State << 5)); |
} |
|
bool ITG3200::isAnyRegClrMode() { |
readmem(INT_CFG, 1, &_buff[0]); |
return ((_buff[0] & INTCFG_INT_ANYRD_2CLEAR) >> 4); |
} |
|
void ITG3200::setLatchClearMode(bool _State) { |
readmem(INT_CFG, 1, &_buff[0]); |
writemem(INT_CFG, ((_buff[0] & ~INTCFG_INT_ANYRD_2CLEAR) | _State << 4)); |
} |
|
bool ITG3200::isITGReadyOn() { |
readmem(INT_CFG, 1, &_buff[0]); |
return ((_buff[0] & INTCFG_ITG_RDY_EN) >> 2); |
} |
|
void ITG3200::setITGReady(bool _State) { |
readmem(INT_CFG, 1, &_buff[0]); |
writemem(INT_CFG, ((_buff[0] & ~INTCFG_ITG_RDY_EN) | _State << 2)); |
} |
|
bool ITG3200::isRawDataReadyOn() { |
readmem(INT_CFG, 1, &_buff[0]); |
return (_buff[0] & INTCFG_RAW_RDY_EN); |
} |
|
void ITG3200::setRawDataReady(bool _State) { |
readmem(INT_CFG, 1, &_buff[0]); |
writemem(INT_CFG, ((_buff[0] & ~INTCFG_RAW_RDY_EN) | _State)); |
} |
|
bool ITG3200::isITGReady() { |
readmem(INT_STATUS, 1, &_buff[0]); |
return ((_buff[0] & INTSTATUS_ITG_RDY) >> 2); |
} |
|
bool ITG3200::isRawDataReady() { |
readmem(INT_STATUS, 1, &_buff[0]); |
return (_buff[0] & INTSTATUS_RAW_DATA_RDY); |
} |
|
void ITG3200::readTemp(float *_Temp) { |
readmem(TEMP_OUT,2,_buff); |
*_Temp = 35 + (((_buff[0] << 8) | _buff[1]) + 13200) / 280.0; // F=C*9/5+32 |
} |
|
void ITG3200::readGyroRaw(int *_GyroX, int *_GyroY, int *_GyroZ){ |
readmem(GYRO_XOUT, 6, _buff); |
*_GyroX = ((_buff[0] << 8) | _buff[1]); |
*_GyroY = ((_buff[2] << 8) | _buff[3]); |
*_GyroZ = ((_buff[4] << 8) | _buff[5]); |
} |
|
void ITG3200::readGyroRaw(int *_GyroXYZ){ |
readGyroRaw(_GyroXYZ, _GyroXYZ+1, _GyroXYZ+2); |
} |
|
void ITG3200::setRevPolarity(bool _Xpol, bool _Ypol, bool _Zpol) { |
polarities[0] = _Xpol ? -1 : 1; |
polarities[1] = _Ypol ? -1 : 1; |
polarities[2] = _Zpol ? -1 : 1; |
} |
|
void ITG3200::setGains(float _Xgain, float _Ygain, float _Zgain) { |
gains[0] = _Xgain; |
gains[1] = _Ygain; |
gains[2] = _Zgain; |
} |
|
void ITG3200::setOffsets(int _Xoffset, int _Yoffset, int _Zoffset) { |
offsets[0] = _Xoffset; |
offsets[1] = _Yoffset; |
offsets[2] = _Zoffset; |
} |
|
void ITG3200::zeroCalibrate(unsigned int totSamples, unsigned int sampleDelayMS) { |
int xyz[3]; |
float tmpOffsets[] = {0,0,0}; |
|
for (int i = 0;i < totSamples;i++){ |
delay(sampleDelayMS); |
readGyroRaw(xyz); |
tmpOffsets[0] += xyz[0]; |
tmpOffsets[1] += xyz[1]; |
tmpOffsets[2] += xyz[2]; |
} |
setOffsets(-tmpOffsets[0] / totSamples, -tmpOffsets[1] / totSamples, -tmpOffsets[2] / totSamples); |
} |
|
void ITG3200::readGyroRawCal(int *_GyroX, int *_GyroY, int *_GyroZ) { |
readGyroRaw(_GyroX, _GyroY, _GyroZ); |
*_GyroX += offsets[0]; |
*_GyroY += offsets[1]; |
*_GyroZ += offsets[2]; |
} |
|
void ITG3200::readGyroRawCal(int *_GyroXYZ) { |
readGyroRawCal(_GyroXYZ, _GyroXYZ+1, _GyroXYZ+2); |
} |
|
void ITG3200::readGyro(float *_GyroX, float *_GyroY, float *_GyroZ){ |
int x, y, z; |
|
readGyroRawCal(&x, &y, &z); // x,y,z will contain calibrated integer values from the sensor |
*_GyroX = x / 14.375 * polarities[0] * gains[0]; |
*_GyroY = y / 14.375 * polarities[1] * gains[1]; |
*_GyroZ = z / 14.375 * polarities[2] * gains[2]; |
} |
|
void ITG3200::readGyro(float *_GyroXYZ){ |
readGyro(_GyroXYZ, _GyroXYZ+1, _GyroXYZ+2); |
} |
|
void ITG3200::reset() { |
writemem(PWR_MGM, PWRMGM_HRESET); |
delay(GYROSTART_UP_DELAY); //gyro startup |
} |
|
bool ITG3200::isLowPower() { |
readmem(PWR_MGM, 1, &_buff[0]); |
return (_buff[0] & PWRMGM_SLEEP) >> 6; |
} |
|
void ITG3200::setPowerMode(bool _State) { |
readmem(PWR_MGM, 1, &_buff[0]); |
writemem(PWR_MGM, ((_buff[0] & ~PWRMGM_SLEEP) | _State << 6)); |
} |
|
bool ITG3200::isXgyroStandby() { |
readmem(PWR_MGM, 1, &_buff[0]); |
return (_buff[0] & PWRMGM_STBY_XG) >> 5; |
} |
|
bool ITG3200::isYgyroStandby() { |
readmem(PWR_MGM, 1, &_buff[0]); |
return (_buff[0] & PWRMGM_STBY_YG) >> 4; |
} |
|
bool ITG3200::isZgyroStandby() { |
readmem(PWR_MGM, 1, &_buff[0]); |
return (_buff[0] & PWRMGM_STBY_ZG) >> 3; |
} |
|
void ITG3200::setXgyroStandby(bool _Status) { |
readmem(PWR_MGM, 1, &_buff[0]); |
writemem(PWR_MGM, ((_buff[0] & PWRMGM_STBY_XG) | _Status << 5)); |
} |
|
void ITG3200::setYgyroStandby(bool _Status) { |
readmem(PWR_MGM, 1, &_buff[0]); |
writemem(PWR_MGM, ((_buff[0] & PWRMGM_STBY_YG) | _Status << 4)); |
} |
|
void ITG3200::setZgyroStandby(bool _Status) { |
readmem(PWR_MGM, 1, &_buff[0]); |
writemem(PWR_MGM, ((_buff[0] & PWRMGM_STBY_ZG) | _Status << 3)); |
} |
|
byte ITG3200::getClockSource() { |
readmem(PWR_MGM, 1, &_buff[0]); |
return (_buff[0] & PWRMGM_CLK_SEL); |
} |
|
void ITG3200::setClockSource(byte _CLKsource) { |
readmem(PWR_MGM, 1, &_buff[0]); |
writemem(PWR_MGM, ((_buff[0] & ~PWRMGM_CLK_SEL) | _CLKsource)); |
} |
|
void ITG3200::writemem(uint8_t _addr, uint8_t _val) { |
Wire.beginTransmission(_dev_address); // start transmission to device |
Wire.send(_addr); // send register address |
Wire.send(_val); // send value to write |
Wire.endTransmission(); // end transmission |
} |
|
void ITG3200::readmem(uint8_t _addr, uint8_t _nbytes, uint8_t __buff[]) { |
Wire.beginTransmission(_dev_address); // start transmission to device |
Wire.send(_addr); // sends register address to read from |
Wire.endTransmission(); // end transmission |
|
Wire.beginTransmission(_dev_address); // start transmission to device |
Wire.requestFrom(_dev_address, _nbytes);// send data n-bytes read |
uint8_t i = 0; |
while (Wire.available()) { |
__buff[i] = Wire.receive(); // receive DATA |
i++; |
} |
Wire.endTransmission(); // end transmission |
} |
|
|