ITG3200 library with multiple configurable full-scale ranges. Based on the Sensor_test by Bo Carøe.
Fork of ITG3200 by
ITG3200.cpp
- Committer:
- cdonate
- Date:
- 2012-08-15
- Revision:
- 1:1208ffc3ace9
- Parent:
- 0:a3a0caa8e802
- Child:
- 2:044664f2cc01
File content as of revision 1:1208ffc3ace9:
#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); }