Diff: ITG3200.cpp

Revision:

0:a3a0caa8e802

Child:

1:1208ffc3ace9

--- /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);
+}