ITG3200 library with multiple configurable full-scale ranges. Based on the Sensor_test by Bo Carøe.
Fork of ITG3200 by
Diff: ITG3200.cpp
- Revision:
- 2:044664f2cc01
- Parent:
- 1:1208ffc3ace9
--- a/ITG3200.cpp Wed Aug 15 01:33:01 2012 +0000 +++ b/ITG3200.cpp Wed Aug 15 22:17:36 2012 +0000 @@ -1,117 +1,117 @@ -#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); +#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); } \ No newline at end of file