Claudio Donate
ITG3200 library with multiple configurable full-scale ranges. Based on the Sensor_test by Bo Carøe.
Fork of
ITG3200
by 
Claudio Donate
ITG3200.cpp@0:a3a0caa8e802, 2012-05-30 (annotated)
- Committer:
- caroe
- Date:
- Wed May 30 10:42:14 2012 +0000
- Revision:
- 0:a3a0caa8e802
- Child:
- 1:1208ffc3ace9
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| caroe | 0:a3a0caa8e802 | 1 | #include "mbed.h" |
| caroe | 0:a3a0caa8e802 | 2 | #include "ITG3200.h" |
| caroe | 0:a3a0caa8e802 | 3 | |
| caroe | 0:a3a0caa8e802 | 4 | #define I2CADR_W(ADR) (ADR<<1&0xFE) |
| caroe | 0:a3a0caa8e802 | 5 | #define I2CADR_R(ADR) (ADR<<1|0x01) |
| caroe | 0:a3a0caa8e802 | 6 | |
| caroe | 0:a3a0caa8e802 | 7 | //Initialisieren |
| caroe | 0:a3a0caa8e802 | 8 | ITG3200::ITG3200(I2C & I2CBus_, Timer & GlobalTime_) |
| caroe | 0:a3a0caa8e802 | 9 | : I2CBus(I2CBus_), |
| caroe | 0:a3a0caa8e802 | 10 | GlobalTime(GlobalTime_) |
| caroe | 0:a3a0caa8e802 | 11 | {} |
| caroe | 0:a3a0caa8e802 | 12 | |
| caroe | 0:a3a0caa8e802 | 13 | void ITG3200::Init() |
| caroe | 0:a3a0caa8e802 | 14 | { |
| caroe | 0:a3a0caa8e802 | 15 | //Nullsetzen |
| caroe | 0:a3a0caa8e802 | 16 | for(int i= 0; i < 3; i++) |
| caroe | 0:a3a0caa8e802 | 17 | { |
| caroe | 0:a3a0caa8e802 | 18 | RawRate[i]= 0; |
| caroe | 0:a3a0caa8e802 | 19 | Offset[i]= 0.0; |
| caroe | 0:a3a0caa8e802 | 20 | Rate[i]= 0.0; |
| caroe | 0:a3a0caa8e802 | 21 | } |
| caroe | 0:a3a0caa8e802 | 22 | |
| caroe | 0:a3a0caa8e802 | 23 | |
| caroe | 0:a3a0caa8e802 | 24 | //ITG3200 initialisieren |
| caroe | 0:a3a0caa8e802 | 25 | char tx[2]; |
| caroe | 0:a3a0caa8e802 | 26 | |
| caroe | 0:a3a0caa8e802 | 27 | //Full-Scale (2000deg/sec) nutzen |
| caroe | 0:a3a0caa8e802 | 28 | //Digitalen Low Pass Filter auf 10 Hz setzen |
| caroe | 0:a3a0caa8e802 | 29 | //Intern mit 1 kHz sampeln |
| caroe | 0:a3a0caa8e802 | 30 | tx[0]= 0x16; |
| caroe | 0:a3a0caa8e802 | 31 | tx[1]= 0x1D; |
| caroe | 0:a3a0caa8e802 | 32 | I2CBus.write(I2CADR_W(ITG3200_ADRESS), tx, 2); |
| caroe | 0:a3a0caa8e802 | 33 | |
| caroe | 0:a3a0caa8e802 | 34 | //Einen internen Taktgeber verwenden (X-Gyro) |
| caroe | 0:a3a0caa8e802 | 35 | tx[0]= 0x3E; |
| caroe | 0:a3a0caa8e802 | 36 | tx[1]= 0x01; |
| caroe | 0:a3a0caa8e802 | 37 | I2CBus.write(I2CADR_W(ITG3200_ADRESS), tx, 2); |
| caroe | 0:a3a0caa8e802 | 38 | |
| caroe | 0:a3a0caa8e802 | 39 | //kurz warten |
| caroe | 0:a3a0caa8e802 | 40 | wait_ms(100); |
| caroe | 0:a3a0caa8e802 | 41 | |
| caroe | 0:a3a0caa8e802 | 42 | //erste Werte abholen |
| caroe | 0:a3a0caa8e802 | 43 | Update(); |
| caroe | 0:a3a0caa8e802 | 44 | } |
| caroe | 0:a3a0caa8e802 | 45 | |
| caroe | 0:a3a0caa8e802 | 46 | |
| caroe | 0:a3a0caa8e802 | 47 | //Rohdaten lesen |
| caroe | 0:a3a0caa8e802 | 48 | void ITG3200::ReadRawData() |
| caroe | 0:a3a0caa8e802 | 49 | { |
| caroe | 0:a3a0caa8e802 | 50 | //Drehrate aller Axen abholen |
| caroe | 0:a3a0caa8e802 | 51 | char tx[1]; |
| caroe | 0:a3a0caa8e802 | 52 | char rx[6]; |
| caroe | 0:a3a0caa8e802 | 53 | |
| caroe | 0:a3a0caa8e802 | 54 | tx[0]= 0x1D; |
| caroe | 0:a3a0caa8e802 | 55 | I2CBus.write(I2CADR_W(ITG3200_ADRESS), tx, 1); |
| caroe | 0:a3a0caa8e802 | 56 | I2CBus.read (I2CADR_R(ITG3200_ADRESS), rx, 6); |
| caroe | 0:a3a0caa8e802 | 57 | |
| caroe | 0:a3a0caa8e802 | 58 | //Aus den einzelnen Bytes den 16-Bit-Wert zusammenbauen |
| caroe | 0:a3a0caa8e802 | 59 | RawRate[0]= rx[0]<<8|rx[1]; |
| caroe | 0:a3a0caa8e802 | 60 | RawRate[1]= rx[2]<<8|rx[3]; |
| caroe | 0:a3a0caa8e802 | 61 | RawRate[2]= rx[4]<<8|rx[5]; |
| caroe | 0:a3a0caa8e802 | 62 | } |
| caroe | 0:a3a0caa8e802 | 63 | |
| caroe | 0:a3a0caa8e802 | 64 | |
| caroe | 0:a3a0caa8e802 | 65 | //Update-Methode |
| caroe | 0:a3a0caa8e802 | 66 | void ITG3200::Update() |
| caroe | 0:a3a0caa8e802 | 67 | { |
| caroe | 0:a3a0caa8e802 | 68 | //Rohdaten lesen |
| caroe | 0:a3a0caa8e802 | 69 | ReadRawData(); |
| caroe | 0:a3a0caa8e802 | 70 | |
| caroe | 0:a3a0caa8e802 | 71 | //Drehrate berechnen |
| caroe | 0:a3a0caa8e802 | 72 | Rate[0]= fConvRPS * ((float)RawRate[0] + Offset[0]); |
| caroe | 0:a3a0caa8e802 | 73 | Rate[1]= fConvRPS * ((float)RawRate[1] + Offset[1]); |
| caroe | 0:a3a0caa8e802 | 74 | Rate[2]= fConvRPS * ((float)RawRate[2] + Offset[2]); |
| caroe | 0:a3a0caa8e802 | 75 | } |
| caroe | 0:a3a0caa8e802 | 76 | |
| caroe | 0:a3a0caa8e802 | 77 | //Kalibrieren |
| caroe | 0:a3a0caa8e802 | 78 | void ITG3200::Calibrate(int ms) |
| caroe | 0:a3a0caa8e802 | 79 | { |
| caroe | 0:a3a0caa8e802 | 80 | float AvgCalibRate[3]= {0.0, 0.0, 0.0}; |
| caroe | 0:a3a0caa8e802 | 81 | float AvgCalibSampels= 0.0; |
| caroe | 0:a3a0caa8e802 | 82 | |
| caroe | 0:a3a0caa8e802 | 83 | //Ende der Kalibrierung in ms Millisekunden berechnen |
| caroe | 0:a3a0caa8e802 | 84 | int CalibEnd= GlobalTime.read_ms() + ms; |
| caroe | 0:a3a0caa8e802 | 85 | |
| caroe | 0:a3a0caa8e802 | 86 | while(GlobalTime.read_ms() < CalibEnd) |
| caroe | 0:a3a0caa8e802 | 87 | { |
| caroe | 0:a3a0caa8e802 | 88 | //Rohdaten lesen |
| caroe | 0:a3a0caa8e802 | 89 | ReadRawData(); |
| caroe | 0:a3a0caa8e802 | 90 | |
| caroe | 0:a3a0caa8e802 | 91 | //Durchschnitt bilden |
| caroe | 0:a3a0caa8e802 | 92 | AvgCalibRate[0]+= (float)RawRate[0]; |
| caroe | 0:a3a0caa8e802 | 93 | AvgCalibRate[1]+= (float)RawRate[1]; |
| caroe | 0:a3a0caa8e802 | 94 | AvgCalibRate[2]+= (float)RawRate[2]; |
| caroe | 0:a3a0caa8e802 | 95 | AvgCalibSampels+= 1.0; |
| caroe | 0:a3a0caa8e802 | 96 | |
| caroe | 0:a3a0caa8e802 | 97 | wait_ms(2); |
| caroe | 0:a3a0caa8e802 | 98 | } |
| caroe | 0:a3a0caa8e802 | 99 | |
| caroe | 0:a3a0caa8e802 | 100 | //Genug Daten gesammelt, jetzt den Durchschnitt bilden |
| caroe | 0:a3a0caa8e802 | 101 | Offset[0]= -(AvgCalibRate[0] / AvgCalibSampels); |
| caroe | 0:a3a0caa8e802 | 102 | Offset[1]= -(AvgCalibRate[1] / AvgCalibSampels); |
| caroe | 0:a3a0caa8e802 | 103 | Offset[2]= -(AvgCalibRate[2] / AvgCalibSampels); |
| caroe | 0:a3a0caa8e802 | 104 | } |