File content as of revision 2:044664f2cc01:

#include "mbed.h"
#include "ITG3200.h"

#define I2CADR_W(ADR)           (ADR<<1&0xFE)
#define I2CADR_R(ADR)           (ADR<<1|0x01)

//Initialization
ITG3200::ITG3200(I2C & I2CBus_, Timer & GlobalTime_)
    : I2CBus(I2CBus_),
      GlobalTime(GlobalTime_)
{}

void ITG3200::Init()
{
    //Zeroing
    for(int i= 0; i < 3; i++)
    {
        RawRate[i]= 0;
        Offset[i]= 0.0;
        Rate[i]= 0.0;
    }
    
       
    //ITG3200 initialization
    char tx[2];
        
    //Full-scale use (250 deg/sec)
    //Digital low-pass filter set at 10 Hz
    //Internal resample at 1 kHz
    tx[0]= 0x16;
    tx[1]= 0x05;//0x0D(500 dg/s) - 0x1D(2000 deg/sec)
    I2CBus.write(I2CADR_W(ITG3200_ADRESS), tx, 2);
    
    //Use the internal clock from (X-gyro)
    tx[0]= 0x3E;
    tx[1]= 0x01;
    I2CBus.write(I2CADR_W(ITG3200_ADRESS), tx, 2);
    
    //Initial wait
    wait_ms(100);
    
    //Pick first value
    Update();
}

//Get Full-scale range, Low Pass Filter Bandwidth and Internal Sample Rate
// 5: +-250deg/sec, 10Hz, 1kHz - 13: +- 500deg/sec, 10Hz, 1kHz
char ITG3200::getInfo(void){ 

    char tx = 0x16;
    char rx;
    
    I2CBus.write(I2CADR_W(ITG3200_ADRESS), &tx, 1);
    I2CBus.read (I2CADR_R(ITG3200_ADRESS), &rx, 1);

    return rx;

}

//Read raw data
void ITG3200::ReadRawData()
{
    //Getting rotation rate of all axes
    char tx[1];
    char rx[6];
     
    tx[0]= 0x1D;
    I2CBus.write(I2CADR_W(ITG3200_ADRESS), tx, 1);
    I2CBus.read (I2CADR_R(ITG3200_ADRESS), rx, 6); 
    
    //Assemble the individual bytes of the 16-bit value
    RawRate[0]= rx[0]<<8|rx[1];
    RawRate[1]= rx[2]<<8|rx[3];
    RawRate[2]= rx[4]<<8|rx[5];
}


//Update-Method
void ITG3200::Update()
{
    //Read raw data
    ReadRawData();    
   
    //Calculating rotation rate
    Rate[0]= fConvRPS * ((float)RawRate[0] + Offset[0]);
    Rate[1]= fConvRPS * ((float)RawRate[1] + Offset[1]);
    Rate[2]= fConvRPS * ((float)RawRate[2] + Offset[2]);
}

//Calibration
void ITG3200::Calibrate(int ms)
{
    float AvgCalibRate[3]= {0.0, 0.0, 0.0};    
    float AvgCalibSampels= 0.0;
    
    //End of calibration calculated in milliseconds
    int CalibEnd= GlobalTime.read_ms() + ms;
    
    while(GlobalTime.read_ms() < CalibEnd)
    {    
        //Read raw data
        ReadRawData();
        
        //Averaging
        AvgCalibRate[0]+= (float)RawRate[0];
        AvgCalibRate[1]+= (float)RawRate[1];
        AvgCalibRate[2]+= (float)RawRate[2];
        AvgCalibSampels+= 1.0;
        
        wait_ms(2);    
    }
    
    //Collected enough data now form the average
    Offset[0]= -(AvgCalibRate[0] / AvgCalibSampels);
    Offset[1]= -(AvgCalibRate[1] / AvgCalibSampels);
    Offset[2]= -(AvgCalibRate[2] / AvgCalibSampels);
}