ITG3200 from BlazeX
ITG3200.cpp
- Committer:
- caroe
- Date:
- 2012-05-30
- Revision:
- 0:a3a0caa8e802
File content as of revision 0:a3a0caa8e802:
#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); }