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@2:044664f2cc01, 2012-08-15 (annotated)

Committer:
cdonate
Date:
Wed Aug 15 22:17:36 2012 +0000
Revision:
2:044664f2cc01
Parent:
1:1208ffc3ace9 
No change

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cdonate 2:044664f2cc01 1 #include "mbed.h"
cdonate 2:044664f2cc01 2 #include "ITG3200.h"
cdonate 2:044664f2cc01 3
cdonate 2:044664f2cc01 4 #define I2CADR_W(ADR) (ADR<<1&0xFE)
cdonate 2:044664f2cc01 5 #define I2CADR_R(ADR) (ADR<<1|0x01)
cdonate 2:044664f2cc01 6
cdonate 2:044664f2cc01 7 //Initialization
cdonate 2:044664f2cc01 8 ITG3200::ITG3200(I2C & I2CBus_, Timer & GlobalTime_)
cdonate 2:044664f2cc01 9 : I2CBus(I2CBus_),
cdonate 2:044664f2cc01 10 GlobalTime(GlobalTime_)
cdonate 2:044664f2cc01 11 {}
cdonate 2:044664f2cc01 12
cdonate 2:044664f2cc01 13 void ITG3200::Init()
cdonate 2:044664f2cc01 14 {
cdonate 2:044664f2cc01 15 //Zeroing
cdonate 2:044664f2cc01 16 for(int i= 0; i < 3; i++)
cdonate 2:044664f2cc01 17 {
cdonate 2:044664f2cc01 18 RawRate[i]= 0;
cdonate 2:044664f2cc01 19 Offset[i]= 0.0;
cdonate 2:044664f2cc01 20 Rate[i]= 0.0;
cdonate 2:044664f2cc01 21 }
cdonate 2:044664f2cc01 22
cdonate 2:044664f2cc01 23
cdonate 2:044664f2cc01 24 //ITG3200 initialization
cdonate 2:044664f2cc01 25 char tx[2];
cdonate 2:044664f2cc01 26
cdonate 2:044664f2cc01 27 //Full-scale use (250 deg/sec)
cdonate 2:044664f2cc01 28 //Digital low-pass filter set at 10 Hz
cdonate 2:044664f2cc01 29 //Internal resample at 1 kHz
cdonate 2:044664f2cc01 30 tx[0]= 0x16;
cdonate 2:044664f2cc01 31 tx[1]= 0x05;//0x0D(500 dg/s) - 0x1D(2000 deg/sec)
cdonate 2:044664f2cc01 32 I2CBus.write(I2CADR_W(ITG3200_ADRESS), tx, 2);
cdonate 2:044664f2cc01 33
cdonate 2:044664f2cc01 34 //Use the internal clock from (X-gyro)
cdonate 2:044664f2cc01 35 tx[0]= 0x3E;
cdonate 2:044664f2cc01 36 tx[1]= 0x01;
cdonate 2:044664f2cc01 37 I2CBus.write(I2CADR_W(ITG3200_ADRESS), tx, 2);
cdonate 2:044664f2cc01 38
cdonate 2:044664f2cc01 39 //Initial wait
cdonate 2:044664f2cc01 40 wait_ms(100);
cdonate 2:044664f2cc01 41
cdonate 2:044664f2cc01 42 //Pick first value
cdonate 2:044664f2cc01 43 Update();
cdonate 2:044664f2cc01 44 }
cdonate 2:044664f2cc01 45
cdonate 2:044664f2cc01 46 //Get Full-scale range, Low Pass Filter Bandwidth and Internal Sample Rate
cdonate 2:044664f2cc01 47 // 5: +-250deg/sec, 10Hz, 1kHz - 13: +- 500deg/sec, 10Hz, 1kHz
cdonate 2:044664f2cc01 48 char ITG3200::getInfo(void){
cdonate 2:044664f2cc01 49
cdonate 2:044664f2cc01 50 char tx = 0x16;
cdonate 2:044664f2cc01 51 char rx;
cdonate 2:044664f2cc01 52
cdonate 2:044664f2cc01 53 I2CBus.write(I2CADR_W(ITG3200_ADRESS), &tx, 1);
cdonate 2:044664f2cc01 54 I2CBus.read (I2CADR_R(ITG3200_ADRESS), &rx, 1);
cdonate 2:044664f2cc01 55
cdonate 2:044664f2cc01 56 return rx;
cdonate 2:044664f2cc01 57
cdonate 2:044664f2cc01 58 }
cdonate 2:044664f2cc01 59
cdonate 2:044664f2cc01 60 //Read raw data
cdonate 2:044664f2cc01 61 void ITG3200::ReadRawData()
cdonate 2:044664f2cc01 62 {
cdonate 2:044664f2cc01 63 //Getting rotation rate of all axes
cdonate 2:044664f2cc01 64 char tx[1];
cdonate 2:044664f2cc01 65 char rx[6];
cdonate 2:044664f2cc01 66
cdonate 2:044664f2cc01 67 tx[0]= 0x1D;
cdonate 2:044664f2cc01 68 I2CBus.write(I2CADR_W(ITG3200_ADRESS), tx, 1);
cdonate 2:044664f2cc01 69 I2CBus.read (I2CADR_R(ITG3200_ADRESS), rx, 6);
cdonate 2:044664f2cc01 70
cdonate 2:044664f2cc01 71 //Assemble the individual bytes of the 16-bit value
cdonate 2:044664f2cc01 72 RawRate[0]= rx[0]<<8|rx[1];
cdonate 2:044664f2cc01 73 RawRate[1]= rx[2]<<8|rx[3];
cdonate 2:044664f2cc01 74 RawRate[2]= rx[4]<<8|rx[5];
cdonate 2:044664f2cc01 75 }
cdonate 2:044664f2cc01 76
cdonate 2:044664f2cc01 77
cdonate 2:044664f2cc01 78 //Update-Method
cdonate 2:044664f2cc01 79 void ITG3200::Update()
cdonate 2:044664f2cc01 80 {
cdonate 2:044664f2cc01 81 //Read raw data
cdonate 2:044664f2cc01 82 ReadRawData();
cdonate 2:044664f2cc01 83
cdonate 2:044664f2cc01 84 //Calculating rotation rate
cdonate 2:044664f2cc01 85 Rate[0]= fConvRPS * ((float)RawRate[0] + Offset[0]);
cdonate 2:044664f2cc01 86 Rate[1]= fConvRPS * ((float)RawRate[1] + Offset[1]);
cdonate 2:044664f2cc01 87 Rate[2]= fConvRPS * ((float)RawRate[2] + Offset[2]);
cdonate 2:044664f2cc01 88 }
cdonate 2:044664f2cc01 89
cdonate 2:044664f2cc01 90 //Calibration
cdonate 2:044664f2cc01 91 void ITG3200::Calibrate(int ms)
cdonate 2:044664f2cc01 92 {
cdonate 2:044664f2cc01 93 float AvgCalibRate[3]= {0.0, 0.0, 0.0};
cdonate 2:044664f2cc01 94 float AvgCalibSampels= 0.0;
cdonate 2:044664f2cc01 95
cdonate 2:044664f2cc01 96 //End of calibration calculated in milliseconds
cdonate 2:044664f2cc01 97 int CalibEnd= GlobalTime.read_ms() + ms;
cdonate 2:044664f2cc01 98
cdonate 2:044664f2cc01 99 while(GlobalTime.read_ms() < CalibEnd)
cdonate 2:044664f2cc01 100 {
cdonate 2:044664f2cc01 101 //Read raw data
cdonate 2:044664f2cc01 102 ReadRawData();
cdonate 2:044664f2cc01 103
cdonate 2:044664f2cc01 104 //Averaging
cdonate 2:044664f2cc01 105 AvgCalibRate[0]+= (float)RawRate[0];
cdonate 2:044664f2cc01 106 AvgCalibRate[1]+= (float)RawRate[1];
cdonate 2:044664f2cc01 107 AvgCalibRate[2]+= (float)RawRate[2];
cdonate 2:044664f2cc01 108 AvgCalibSampels+= 1.0;
cdonate 2:044664f2cc01 109
cdonate 2:044664f2cc01 110 wait_ms(2);
cdonate 2:044664f2cc01 111 }
cdonate 2:044664f2cc01 112
cdonate 2:044664f2cc01 113 //Collected enough data now form the average
cdonate 2:044664f2cc01 114 Offset[0]= -(AvgCalibRate[0] / AvgCalibSampels);
cdonate 2:044664f2cc01 115 Offset[1]= -(AvgCalibRate[1] / AvgCalibSampels);
cdonate 2:044664f2cc01 116 Offset[2]= -(AvgCalibRate[2] / AvgCalibSampels);
cdonate 1:1208ffc3ace9 117 }