ITG3200 from BlazeX
Diff: ITG3200.cpp
- Revision:
- 0:a3a0caa8e802
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ITG3200.cpp Wed May 30 10:42:14 2012 +0000 @@ -0,0 +1,104 @@ +#include "mbed.h" +#include "ITG3200.h" + +#define I2CADR_W(ADR) (ADR<<1&0xFE) +#define I2CADR_R(ADR) (ADR<<1|0x01) + +//Initialisieren +ITG3200::ITG3200(I2C & I2CBus_, Timer & GlobalTime_) + : I2CBus(I2CBus_), + GlobalTime(GlobalTime_) +{} + +void ITG3200::Init() +{ + //Nullsetzen + for(int i= 0; i < 3; i++) + { + RawRate[i]= 0; + Offset[i]= 0.0; + Rate[i]= 0.0; + } + + + //ITG3200 initialisieren + char tx[2]; + + //Full-Scale (2000deg/sec) nutzen + //Digitalen Low Pass Filter auf 10 Hz setzen + //Intern mit 1 kHz sampeln + tx[0]= 0x16; + tx[1]= 0x1D; + I2CBus.write(I2CADR_W(ITG3200_ADRESS), tx, 2); + + //Einen internen Taktgeber verwenden (X-Gyro) + tx[0]= 0x3E; + tx[1]= 0x01; + I2CBus.write(I2CADR_W(ITG3200_ADRESS), tx, 2); + + //kurz warten + wait_ms(100); + + //erste Werte abholen + Update(); +} + + +//Rohdaten lesen +void ITG3200::ReadRawData() +{ + //Drehrate aller Axen abholen + 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); + + //Aus den einzelnen Bytes den 16-Bit-Wert zusammenbauen + RawRate[0]= rx[0]<<8|rx[1]; + RawRate[1]= rx[2]<<8|rx[3]; + RawRate[2]= rx[4]<<8|rx[5]; +} + + +//Update-Methode +void ITG3200::Update() +{ + //Rohdaten lesen + ReadRawData(); + + //Drehrate berechnen + Rate[0]= fConvRPS * ((float)RawRate[0] + Offset[0]); + Rate[1]= fConvRPS * ((float)RawRate[1] + Offset[1]); + Rate[2]= fConvRPS * ((float)RawRate[2] + Offset[2]); +} + +//Kalibrieren +void ITG3200::Calibrate(int ms) +{ + float AvgCalibRate[3]= {0.0, 0.0, 0.0}; + float AvgCalibSampels= 0.0; + + //Ende der Kalibrierung in ms Millisekunden berechnen + int CalibEnd= GlobalTime.read_ms() + ms; + + while(GlobalTime.read_ms() < CalibEnd) + { + //Rohdaten lesen + ReadRawData(); + + //Durchschnitt bilden + AvgCalibRate[0]+= (float)RawRate[0]; + AvgCalibRate[1]+= (float)RawRate[1]; + AvgCalibRate[2]+= (float)RawRate[2]; + AvgCalibSampels+= 1.0; + + wait_ms(2); + } + + //Genug Daten gesammelt, jetzt den Durchschnitt bilden + Offset[0]= -(AvgCalibRate[0] / AvgCalibSampels); + Offset[1]= -(AvgCalibRate[1] / AvgCalibSampels); + Offset[2]= -(AvgCalibRate[2] / AvgCalibSampels); +}