ITG3200.cpp

00001 #include "mbed.h"
00002 #include "ITG3200.h"
00003 
00004 #define I2CADR_W(ADR)           (ADR<<1&0xFE)
00005 #define I2CADR_R(ADR)           (ADR<<1|0x01)
00006 
00007 //Initialization
00008 ITG3200::ITG3200(I2C & I2CBus_, Timer & GlobalTime_)
00009     : I2CBus(I2CBus_),
00010       GlobalTime(GlobalTime_)
00011 {}
00012 
00013 void ITG3200::Init()
00014 {
00015     //Zeroing
00016     for(int i= 0; i < 3; i++)
00017     {
00018         RawRate[i]= 0;
00019         Offset[i]= 0.0;
00020         Rate[i]= 0.0;
00021     }
00022     
00023        
00024     //ITG3200 initialization
00025     char tx[2];
00026         
00027     //Full-scale use (250 deg/sec)
00028     //Digital low-pass filter set at 10 Hz
00029     //Internal resample at 1 kHz
00030     tx[0]= 0x16;
00031     tx[1]= 0x05;//0x0D(500 dg/s) - 0x1D(2000 deg/sec)
00032     I2CBus.write(I2CADR_W(ITG3200_ADRESS), tx, 2);
00033     
00034     //Use the internal clock from (X-gyro)
00035     tx[0]= 0x3E;
00036     tx[1]= 0x01;
00037     I2CBus.write(I2CADR_W(ITG3200_ADRESS), tx, 2);
00038     
00039     //Initial wait
00040     wait_ms(100);
00041     
00042     //Pick first value
00043     Update();
00044 }
00045 
00046 //Get Full-scale range, Low Pass Filter Bandwidth and Internal Sample Rate
00047 // 5: +-250deg/sec, 10Hz, 1kHz - 13: +- 500deg/sec, 10Hz, 1kHz
00048 char ITG3200::getInfo(void){ 
00049 
00050     char tx = 0x16;
00051     char rx;
00052     
00053     I2CBus.write(I2CADR_W(ITG3200_ADRESS), &tx, 1);
00054     I2CBus.read (I2CADR_R(ITG3200_ADRESS), &rx, 1);
00055 
00056     return rx;
00057 
00058 }
00059 
00060 //Read raw data
00061 void ITG3200::ReadRawData()
00062 {
00063     //Getting rotation rate of all axes
00064     char tx[1];
00065     char rx[6];
00066      
00067     tx[0]= 0x1D;
00068     I2CBus.write(I2CADR_W(ITG3200_ADRESS), tx, 1);
00069     I2CBus.read (I2CADR_R(ITG3200_ADRESS), rx, 6); 
00070     
00071     //Assemble the individual bytes of the 16-bit value
00072     RawRate[0]= rx[0]<<8|rx[1];
00073     RawRate[1]= rx[2]<<8|rx[3];
00074     RawRate[2]= rx[4]<<8|rx[5];
00075 }
00076 
00077 
00078 //Update-Method
00079 void ITG3200::Update()
00080 {
00081     //Read raw data
00082     ReadRawData();    
00083    
00084     //Calculating rotation rate
00085     Rate[0]= fConvRPS * ((float)RawRate[0] + Offset[0]);
00086     Rate[1]= fConvRPS * ((float)RawRate[1] + Offset[1]);
00087     Rate[2]= fConvRPS * ((float)RawRate[2] + Offset[2]);
00088 }
00089 
00090 //Calibration
00091 void ITG3200::Calibrate(int ms)
00092 {
00093     float AvgCalibRate[3]= {0.0, 0.0, 0.0};    
00094     float AvgCalibSampels= 0.0;
00095     
00096     //End of calibration calculated in milliseconds
00097     int CalibEnd= GlobalTime.read_ms() + ms;
00098     
00099     while(GlobalTime.read_ms() < CalibEnd)
00100     {    
00101         //Read raw data
00102         ReadRawData();
00103         
00104         //Averaging
00105         AvgCalibRate[0]+= (float)RawRate[0];
00106         AvgCalibRate[1]+= (float)RawRate[1];
00107         AvgCalibRate[2]+= (float)RawRate[2];
00108         AvgCalibSampels+= 1.0;
00109         
00110         wait_ms(2);    
00111     }
00112     
00113     //Collected enough data now form the average
00114     Offset[0]= -(AvgCalibRate[0] / AvgCalibSampels);
00115     Offset[1]= -(AvgCalibRate[1] / AvgCalibSampels);
00116     Offset[2]= -(AvgCalibRate[2] / AvgCalibSampels);
00117 }