i2c.cpp:
#include "i2c.h"
#include <Wire.h>
I2C::I2C() {
}
void I2C::begin() {
Wire.begin();
/*PORTC |= ((1<<PORTC4) | (1<<PORTC5)); //set the i2c pins to internal pull up resistors
TWBR = I2C_BAUD; // We want 400kHz.
//TWSR = TWI_TWPS; // Not used. Driver presumes prescaler to be 00.
TWDR = 0xFF; // Default content = SDA released.
TWCR = (1<<TWEN)| // Enable TWI-interface and release TWI pins.
(0<<TWIE)|(0<<TWINT)| // Disable Interupt.
(0<<TWEA)|(0<<TWSTA)|(0<<TWSTO)| // No Signal requests.
(0<<TWWC); //
*/
}
void I2C::writeRegister(int device, unsigned char address, unsigned char value) {
Wire.beginTransmission(device);
Wire.send(address);
Wire.send(value);
Wire.endTransmission();
}
byte I2C::readRegister(int device, unsigned char address) {
Wire.beginTransmission(device);
Wire.send(address);
Wire.endTransmission();
Wire.requestFrom(device,1);
return Wire.receive();
}
I2C i2c=I2C();
ic2.h:
//i2c.h
//defines the constants for i2c operation
#ifndef i2c_h
#define i2c_h
#include "WProgram.h"
#define CLOCK_RATE 8000000
#define I2C_BAUD ((CLOCK_RATE/400000)-16)/2 //400kHz fast mode I2C
#define START 0x08
#define RESTART 0x10
#define MR_SLA_ACK 0x40
#define MR_SLA_NACK 0x48
#define MR_DATA_ACK 0x50
#define MR_DATA_NACK 0x58
#define MT_SLA_ACK 0x18
#define MT_SLA_NACK 0x20
#define MT_DATA_ACK 0x28
#define MT_DATA_NACK 0x30
class I2C {
public:
I2C();
void begin();
void writeRegister(int device, unsigned char address, unsigned char value);
byte readRegister(int device, unsigned char address);
};
extern I2C i2c;
#endif
itg3200.cpp:
#include "itg3200.h"
#include "i2c.h"
#include <Wire.h>
//#define ITG_PWR_MODE 0b00001011 //use z axis oscillator as clock source, z in standby
//#define ITG_PWR_MODE 0b00000011 //use z axis oscillator as clock source
#define ITG_PWR_MODE 0b00000000 //use internal oscillator as clock source
#define ITG_CALIBRATION_SAMPLES 512 //number of samples to average to work out the offset
ITG::ITG(char device) {
_device=device;
}
void ITG::init() {
i2c.writeRegister(_device, ITG_PWR_MGM, 1<<7); //reset the device
i2c.writeRegister(_device, ITG_SMPLRT_DIV, 9); //sample rate divider to give 100Hz
i2c.writeRegister(_device, ITG_DLPF_FS, 0b00011011); //42Hz low pass, 1kHz internal sample rate
i2c.writeRegister(_device, ITG_INT_CFG, 0b100101); //interrupt for data available and new clock
i2c.writeRegister(_device, ITG_PWR_MGM, ITG_PWR_MODE);
delay(70);
/*while ((i2c.readRegister(_device, ITG_INT_STATUS) & 0b100) ==0 )
{
Serial.println("Waiting for clock sync.");
}*/
calibrate();
sleep(); // start the device in sleep mode
}
int ITG::readInt(char address) {
Wire.beginTransmission(_device);
Wire.send(address);
Wire.endTransmission();
Wire.requestFrom(_device,2);
byte msb=Wire.receive();
byte lsb=Wire.receive();
return ((int)msb)<<8 | lsb;
}
float ITG::getX() {
return ((float)readInt(ITG_GYRO_XOUT_H)-_offsetX)/14.375;
}
float ITG::getY() {
return ((float)readInt(ITG_GYRO_YOUT_H)-_offsetY)/14.375;
}
float ITG::getZ() {
return ((float)readInt(ITG_GYRO_ZOUT_H)-_offsetZ)/14.375;
}
float ITG::getTemp() {
//return (float)readInt(ITG_GYRO_TEMP_OUT_H);
return ((float)readInt(ITG_GYRO_TEMP_OUT_H)+13200)/280.0+35.0;
}
void ITG::sleep() {
char power = i2c.readRegister(_device, ITG_PWR_MGM);
power = power | 0b01000000; //turn on the sleep bit
i2c.writeRegister(_device, ITG_PWR_MGM, power);
}
void ITG::wake() {
char power = i2c.readRegister(_device, ITG_PWR_MGM);
if (power & 0b01000000 ==0)
return;
power = power & ~0b01000000; //turn off the sleep bit
i2c.writeRegister(_device, ITG_PWR_MGM, power);
delay(70);
}
void ITG::calibrate() {
//delay(2000);
_offsetX=0, _offsetY=0, _offsetZ=0;
long ox=0,oy=0,oz=0;
char power = i2c.readRegister(_device, ITG_PWR_MGM);
char sample = i2c.readRegister(_device, ITG_SMPLRT_DIV);
//wake(); //in case we were asleep
//i2c::writeRegister(_device, ITG_SMPLRT_DIV, 0); //max sample rate
//get some readings to generate offset values
for (int i=0; i<ITG_CALIBRATION_SAMPLES; i++) {
while (!dataReady()) {
}
ox+=readInt(ITG_GYRO_XOUT_H);
oy+=readInt(ITG_GYRO_YOUT_H);
oz+=readInt(ITG_GYRO_ZOUT_H);
}
_offsetX=ox/ITG_CALIBRATION_SAMPLES;
_offsetY=oy/ITG_CALIBRATION_SAMPLES;
_offsetZ=oz/ITG_CALIBRATION_SAMPLES;
/*Serial.println("Offset");
Serial.print(_offsetX);
Serial.print('\t');
Serial.print(_offsetY);
Serial.print('\t');
Serial.println(_offsetZ);*/
//i2c::writeRegister(_device, ITG_SMPLRT_DIV, sample); //bring the sample rate back to what it was
i2c.writeRegister(_device, ITG_PWR_MGM, power); //return to previous power mode
//delay(2000);
}
bool ITG::dataReady() {
Wire.beginTransmission(_device);
Wire.send(ITG_INT_STATUS);
Wire.endTransmission();
Wire.requestFrom(_device,1);
char interrupt=Wire.receive();
//Serial.println(interrupt, BIN);
return (interrupt & 0b00000001);
}
itg3200.h:
#ifndef itg3200_h
#define itg3200_h
#define ITG_WHO_AM_I 0
#define ITG_SMPLRT_DIV 0x15
#define ITG_DLPF_FS 0x16
#define ITG_INT_CFG 0x17
#define ITG_INT_STATUS 0x1A
#define ITG_GYRO_TEMP_OUT_H 0x1B
#define ITG_GYRO_TEMP_OUT_L 0X1C
#define ITG_GYRO_XOUT_H 0x1D
#define ITG_GYRO_XOUT_L 0x1E
#define ITG_GYRO_YOUT_H 0x1F
#define ITG_GYRO_YOUT_L 0x20
#define ITG_GYRO_ZOUT_H 0x21
#define ITG_GYRO_ZOUT_L 0x22
#define ITG_PWR_MGM 0x3E
class ITG {
public:
ITG(char address);
void init();
float getX();
float getY();
float getZ();
float getTemp();
void sleep();
void wake();
bool dataReady();
private:
char _device;
int _offsetX, _offsetY, _offsetZ;
int readInt(char address);
void calibrate();
};
#endif