ITG3200 library with multiple configurable full-scale ranges. Based on the Sensor_test by Bo Carøe.
Fork of ITG3200 by
ITG3200.cpp@1:1208ffc3ace9, 2012-08-15 (annotated)
- Committer:
- cdonate
- Date:
- Wed Aug 15 01:33:01 2012 +0000
- Revision:
- 1:1208ffc3ace9
- Parent:
- 0:a3a0caa8e802
- Child:
- 2:044664f2cc01
Translate comments to english, change full-scale range and convertions values
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 | |
cdonate | 1:1208ffc3ace9 | 7 | //Initialization |
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 | { |
cdonate | 1:1208ffc3ace9 | 15 | //Zeroing |
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 | |
cdonate | 1:1208ffc3ace9 | 24 | //ITG3200 initialization |
caroe | 0:a3a0caa8e802 | 25 | char tx[2]; |
caroe | 0:a3a0caa8e802 | 26 | |
cdonate | 1:1208ffc3ace9 | 27 | //Full-scale use (250 deg/sec) |
cdonate | 1:1208ffc3ace9 | 28 | //Digital low-pass filter set at 10 Hz |
cdonate | 1:1208ffc3ace9 | 29 | //Internal resample at 1 kHz |
caroe | 0:a3a0caa8e802 | 30 | tx[0]= 0x16; |
cdonate | 1:1208ffc3ace9 | 31 | tx[1]= 0x05;//0x0D(500 dg/s) - 0x1D(2000 deg/sec) |
caroe | 0:a3a0caa8e802 | 32 | I2CBus.write(I2CADR_W(ITG3200_ADRESS), tx, 2); |
caroe | 0:a3a0caa8e802 | 33 | |
cdonate | 1:1208ffc3ace9 | 34 | //Use the internal clock from (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 | |
cdonate | 1:1208ffc3ace9 | 39 | //Initial wait |
caroe | 0:a3a0caa8e802 | 40 | wait_ms(100); |
caroe | 0:a3a0caa8e802 | 41 | |
cdonate | 1:1208ffc3ace9 | 42 | //Pick first value |
caroe | 0:a3a0caa8e802 | 43 | Update(); |
caroe | 0:a3a0caa8e802 | 44 | } |
caroe | 0:a3a0caa8e802 | 45 | |
cdonate | 1:1208ffc3ace9 | 46 | //Get Full-scale range, Low Pass Filter Bandwidth and Internal Sample Rate |
cdonate | 1:1208ffc3ace9 | 47 | // 5: +-250deg/sec, 10Hz, 1kHz - 13: +- 500deg/sec, 10Hz, 1kHz |
cdonate | 1:1208ffc3ace9 | 48 | char ITG3200::getInfo(void){ |
caroe | 0:a3a0caa8e802 | 49 | |
cdonate | 1:1208ffc3ace9 | 50 | char tx = 0x16; |
cdonate | 1:1208ffc3ace9 | 51 | char rx; |
cdonate | 1:1208ffc3ace9 | 52 | |
cdonate | 1:1208ffc3ace9 | 53 | I2CBus.write(I2CADR_W(ITG3200_ADRESS), &tx, 1); |
cdonate | 1:1208ffc3ace9 | 54 | I2CBus.read (I2CADR_R(ITG3200_ADRESS), &rx, 1); |
cdonate | 1:1208ffc3ace9 | 55 | |
cdonate | 1:1208ffc3ace9 | 56 | return rx; |
cdonate | 1:1208ffc3ace9 | 57 | |
cdonate | 1:1208ffc3ace9 | 58 | } |
cdonate | 1:1208ffc3ace9 | 59 | |
cdonate | 1:1208ffc3ace9 | 60 | //Read raw data |
caroe | 0:a3a0caa8e802 | 61 | void ITG3200::ReadRawData() |
caroe | 0:a3a0caa8e802 | 62 | { |
cdonate | 1:1208ffc3ace9 | 63 | //Getting rotation rate of all axes |
caroe | 0:a3a0caa8e802 | 64 | char tx[1]; |
caroe | 0:a3a0caa8e802 | 65 | char rx[6]; |
caroe | 0:a3a0caa8e802 | 66 | |
caroe | 0:a3a0caa8e802 | 67 | tx[0]= 0x1D; |
caroe | 0:a3a0caa8e802 | 68 | I2CBus.write(I2CADR_W(ITG3200_ADRESS), tx, 1); |
caroe | 0:a3a0caa8e802 | 69 | I2CBus.read (I2CADR_R(ITG3200_ADRESS), rx, 6); |
caroe | 0:a3a0caa8e802 | 70 | |
cdonate | 1:1208ffc3ace9 | 71 | //Assemble the individual bytes of the 16-bit value |
caroe | 0:a3a0caa8e802 | 72 | RawRate[0]= rx[0]<<8|rx[1]; |
caroe | 0:a3a0caa8e802 | 73 | RawRate[1]= rx[2]<<8|rx[3]; |
caroe | 0:a3a0caa8e802 | 74 | RawRate[2]= rx[4]<<8|rx[5]; |
caroe | 0:a3a0caa8e802 | 75 | } |
caroe | 0:a3a0caa8e802 | 76 | |
caroe | 0:a3a0caa8e802 | 77 | |
cdonate | 1:1208ffc3ace9 | 78 | //Update-Method |
caroe | 0:a3a0caa8e802 | 79 | void ITG3200::Update() |
caroe | 0:a3a0caa8e802 | 80 | { |
cdonate | 1:1208ffc3ace9 | 81 | //Read raw data |
caroe | 0:a3a0caa8e802 | 82 | ReadRawData(); |
caroe | 0:a3a0caa8e802 | 83 | |
cdonate | 1:1208ffc3ace9 | 84 | //Calculating rotation rate |
caroe | 0:a3a0caa8e802 | 85 | Rate[0]= fConvRPS * ((float)RawRate[0] + Offset[0]); |
caroe | 0:a3a0caa8e802 | 86 | Rate[1]= fConvRPS * ((float)RawRate[1] + Offset[1]); |
caroe | 0:a3a0caa8e802 | 87 | Rate[2]= fConvRPS * ((float)RawRate[2] + Offset[2]); |
caroe | 0:a3a0caa8e802 | 88 | } |
caroe | 0:a3a0caa8e802 | 89 | |
cdonate | 1:1208ffc3ace9 | 90 | //Calibration |
caroe | 0:a3a0caa8e802 | 91 | void ITG3200::Calibrate(int ms) |
caroe | 0:a3a0caa8e802 | 92 | { |
caroe | 0:a3a0caa8e802 | 93 | float AvgCalibRate[3]= {0.0, 0.0, 0.0}; |
caroe | 0:a3a0caa8e802 | 94 | float AvgCalibSampels= 0.0; |
caroe | 0:a3a0caa8e802 | 95 | |
cdonate | 1:1208ffc3ace9 | 96 | //End of calibration calculated in milliseconds |
caroe | 0:a3a0caa8e802 | 97 | int CalibEnd= GlobalTime.read_ms() + ms; |
caroe | 0:a3a0caa8e802 | 98 | |
caroe | 0:a3a0caa8e802 | 99 | while(GlobalTime.read_ms() < CalibEnd) |
caroe | 0:a3a0caa8e802 | 100 | { |
cdonate | 1:1208ffc3ace9 | 101 | //Read raw data |
caroe | 0:a3a0caa8e802 | 102 | ReadRawData(); |
caroe | 0:a3a0caa8e802 | 103 | |
cdonate | 1:1208ffc3ace9 | 104 | //Averaging |
caroe | 0:a3a0caa8e802 | 105 | AvgCalibRate[0]+= (float)RawRate[0]; |
caroe | 0:a3a0caa8e802 | 106 | AvgCalibRate[1]+= (float)RawRate[1]; |
caroe | 0:a3a0caa8e802 | 107 | AvgCalibRate[2]+= (float)RawRate[2]; |
caroe | 0:a3a0caa8e802 | 108 | AvgCalibSampels+= 1.0; |
caroe | 0:a3a0caa8e802 | 109 | |
caroe | 0:a3a0caa8e802 | 110 | wait_ms(2); |
caroe | 0:a3a0caa8e802 | 111 | } |
caroe | 0:a3a0caa8e802 | 112 | |
cdonate | 1:1208ffc3ace9 | 113 | //Collected enough data now form the average |
caroe | 0:a3a0caa8e802 | 114 | Offset[0]= -(AvgCalibRate[0] / AvgCalibSampels); |
caroe | 0:a3a0caa8e802 | 115 | Offset[1]= -(AvgCalibRate[1] / AvgCalibSampels); |
caroe | 0:a3a0caa8e802 | 116 | Offset[2]= -(AvgCalibRate[2] / AvgCalibSampels); |
cdonate | 1:1208ffc3ace9 | 117 | } |