Claudio Donate / ITG3200

ITG3200 library with multiple configurable full-scale ranges. Based on the Sensor_test by Bo Carøe.

Dependents:   KalmanFilter

Fork of ITG3200 by Claudio Donate

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?

UserRevisionLine numberNew 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 }