Revision 2:044664f2cc01, committed 2012-08-15

Comitter:

cdonate

Date:

Wed Aug 15 22:17:36 2012 +0000

Parent:

1:1208ffc3ace9

Commit message:

No change

Changed in this revision

--- a/ITG3200.cpp	Wed Aug 15 01:33:01 2012 +0000
+++ b/ITG3200.cpp	Wed Aug 15 22:17:36 2012 +0000
@@ -1,117 +1,117 @@
-#include "mbed.h"
-#include "ITG3200.h"
-
-#define I2CADR_W(ADR)           (ADR<<1&0xFE)
-#define I2CADR_R(ADR)           (ADR<<1|0x01)
-
-//Initialization
-ITG3200::ITG3200(I2C & I2CBus_, Timer & GlobalTime_)
-    : I2CBus(I2CBus_),
-      GlobalTime(GlobalTime_)
-{}
-
-void ITG3200::Init()
-{
-    //Zeroing
-    for(int i= 0; i < 3; i++)
-    {
-        RawRate[i]= 0;
-        Offset[i]= 0.0;
-        Rate[i]= 0.0;
-    }
-    
-       
-    //ITG3200 initialization
-    char tx[2];
-        
-    //Full-scale use (250 deg/sec)
-    //Digital low-pass filter set at 10 Hz
-    //Internal resample at 1 kHz
-    tx[0]= 0x16;
-    tx[1]= 0x05;//0x0D(500 dg/s) - 0x1D(2000 deg/sec)
-    I2CBus.write(I2CADR_W(ITG3200_ADRESS), tx, 2);
-    
-    //Use the internal clock from (X-gyro)
-    tx[0]= 0x3E;
-    tx[1]= 0x01;
-    I2CBus.write(I2CADR_W(ITG3200_ADRESS), tx, 2);
-    
-    //Initial wait
-    wait_ms(100);
-    
-    //Pick first value
-    Update();
-}
-
-//Get Full-scale range, Low Pass Filter Bandwidth and Internal Sample Rate
-// 5: +-250deg/sec, 10Hz, 1kHz - 13: +- 500deg/sec, 10Hz, 1kHz
-char ITG3200::getInfo(void){ 
-
-    char tx = 0x16;
-    char rx;
-    
-    I2CBus.write(I2CADR_W(ITG3200_ADRESS), &tx, 1);
-    I2CBus.read (I2CADR_R(ITG3200_ADRESS), &rx, 1);
-
-    return rx;
-
-}
-
-//Read raw data
-void ITG3200::ReadRawData()
-{
-    //Getting rotation rate of all axes
-    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); 
-    
-    //Assemble the individual bytes of the 16-bit value
-    RawRate[0]= rx[0]<<8|rx[1];
-    RawRate[1]= rx[2]<<8|rx[3];
-    RawRate[2]= rx[4]<<8|rx[5];
-}
-
-
-//Update-Method
-void ITG3200::Update()
-{
-    //Read raw data
-    ReadRawData();    
-   
-    //Calculating rotation rate
-    Rate[0]= fConvRPS * ((float)RawRate[0] + Offset[0]);
-    Rate[1]= fConvRPS * ((float)RawRate[1] + Offset[1]);
-    Rate[2]= fConvRPS * ((float)RawRate[2] + Offset[2]);
-}
-
-//Calibration
-void ITG3200::Calibrate(int ms)
-{
-    float AvgCalibRate[3]= {0.0, 0.0, 0.0};    
-    float AvgCalibSampels= 0.0;
-    
-    //End of calibration calculated in milliseconds
-    int CalibEnd= GlobalTime.read_ms() + ms;
-    
-    while(GlobalTime.read_ms() < CalibEnd)
-    {    
-        //Read raw data
-        ReadRawData();
-        
-        //Averaging
-        AvgCalibRate[0]+= (float)RawRate[0];
-        AvgCalibRate[1]+= (float)RawRate[1];
-        AvgCalibRate[2]+= (float)RawRate[2];
-        AvgCalibSampels+= 1.0;
-        
-        wait_ms(2);    
-    }
-    
-    //Collected enough data now form the average
-    Offset[0]= -(AvgCalibRate[0] / AvgCalibSampels);
-    Offset[1]= -(AvgCalibRate[1] / AvgCalibSampels);
-    Offset[2]= -(AvgCalibRate[2] / AvgCalibSampels);
+#include "mbed.h"
+#include "ITG3200.h"
+
+#define I2CADR_W(ADR)           (ADR<<1&0xFE)
+#define I2CADR_R(ADR)           (ADR<<1|0x01)
+
+//Initialization
+ITG3200::ITG3200(I2C & I2CBus_, Timer & GlobalTime_)
+    : I2CBus(I2CBus_),
+      GlobalTime(GlobalTime_)
+{}
+
+void ITG3200::Init()
+{
+    //Zeroing
+    for(int i= 0; i < 3; i++)
+    {
+        RawRate[i]= 0;
+        Offset[i]= 0.0;
+        Rate[i]= 0.0;
+    }
+    
+       
+    //ITG3200 initialization
+    char tx[2];
+        
+    //Full-scale use (250 deg/sec)
+    //Digital low-pass filter set at 10 Hz
+    //Internal resample at 1 kHz
+    tx[0]= 0x16;
+    tx[1]= 0x05;//0x0D(500 dg/s) - 0x1D(2000 deg/sec)
+    I2CBus.write(I2CADR_W(ITG3200_ADRESS), tx, 2);
+    
+    //Use the internal clock from (X-gyro)
+    tx[0]= 0x3E;
+    tx[1]= 0x01;
+    I2CBus.write(I2CADR_W(ITG3200_ADRESS), tx, 2);
+    
+    //Initial wait
+    wait_ms(100);
+    
+    //Pick first value
+    Update();
+}
+
+//Get Full-scale range, Low Pass Filter Bandwidth and Internal Sample Rate
+// 5: +-250deg/sec, 10Hz, 1kHz - 13: +- 500deg/sec, 10Hz, 1kHz
+char ITG3200::getInfo(void){ 
+
+    char tx = 0x16;
+    char rx;
+    
+    I2CBus.write(I2CADR_W(ITG3200_ADRESS), &tx, 1);
+    I2CBus.read (I2CADR_R(ITG3200_ADRESS), &rx, 1);
+
+    return rx;
+
+}
+
+//Read raw data
+void ITG3200::ReadRawData()
+{
+    //Getting rotation rate of all axes
+    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); 
+    
+    //Assemble the individual bytes of the 16-bit value
+    RawRate[0]= rx[0]<<8|rx[1];
+    RawRate[1]= rx[2]<<8|rx[3];
+    RawRate[2]= rx[4]<<8|rx[5];
+}
+
+
+//Update-Method
+void ITG3200::Update()
+{
+    //Read raw data
+    ReadRawData();    
+   
+    //Calculating rotation rate
+    Rate[0]= fConvRPS * ((float)RawRate[0] + Offset[0]);
+    Rate[1]= fConvRPS * ((float)RawRate[1] + Offset[1]);
+    Rate[2]= fConvRPS * ((float)RawRate[2] + Offset[2]);
+}
+
+//Calibration
+void ITG3200::Calibrate(int ms)
+{
+    float AvgCalibRate[3]= {0.0, 0.0, 0.0};    
+    float AvgCalibSampels= 0.0;
+    
+    //End of calibration calculated in milliseconds
+    int CalibEnd= GlobalTime.read_ms() + ms;
+    
+    while(GlobalTime.read_ms() < CalibEnd)
+    {    
+        //Read raw data
+        ReadRawData();
+        
+        //Averaging
+        AvgCalibRate[0]+= (float)RawRate[0];
+        AvgCalibRate[1]+= (float)RawRate[1];
+        AvgCalibRate[2]+= (float)RawRate[2];
+        AvgCalibSampels+= 1.0;
+        
+        wait_ms(2);    
+    }
+    
+    //Collected enough data now form the average
+    Offset[0]= -(AvgCalibRate[0] / AvgCalibSampels);
+    Offset[1]= -(AvgCalibRate[1] / AvgCalibSampels);
+    Offset[2]= -(AvgCalibRate[2] / AvgCalibSampels);
 }
\ No newline at end of file

--- a/ITG3200.h	Wed Aug 15 01:33:01 2012 +0000
+++ b/ITG3200.h	Wed Aug 15 22:17:36 2012 +0000
@@ -1,44 +1,44 @@
-#pragma once
-
-//I2C address is either 0x68 or 0x69 , depending on pin AD0, check block diagram for your device
-#define ITG3200_ADRESS 0x69
-
-//Convert gyro raw data to radians per second, 1 degree/s = 0,017453293 rad/s.
-//The value is 0,017453293/LSB (+- 250 degrees/s)
-
-//const float fConvRPS= 2.6646248854961832061068702290076e-4; //For +- 500 degrees/s
-//const float fConvRPS= 1.2141420883e-3; //For +- 2000 degrees/s
-
-const float fConvRPS= 1.3323124427480916030534351145038e-4; //For +- 250 degrees/s
-
-class ITG3200 {
-protected:
-    I2C & I2CBus;
-    Timer & GlobalTime;
-
-public:
-    //Offset
-    float Offset[3];
-
-    //Rotational speed around all three axes
-    short RawRate[3];       //Raw data
-    float Rate[3];          //Calibrated rotation rate in radians per second
-
-
-    //Initialization
-    ITG3200(I2C & I2CBus_, Timer & GlobalTime_);
-    void Init();
-
-
-private:
-    //Read raw data
-    void ReadRawData();
-
-public:
-    //Update Method
-    void Update();
-    char getInfo(void);
-
-    //Calibration
-    void Calibrate(int ms);
-};
+#pragma once
+
+//I2C address is either 0x68 or 0x69 , depending on pin AD0, check block diagram for your device
+#define ITG3200_ADRESS 0x69
+
+//Convert gyro raw data to radians per second, 1 degree/s = 0,017453293 rad/s.
+//The value is 0,017453293/LSB (+- 250 degrees/s)
+
+//const float fConvRPS= 2.6646248854961832061068702290076e-4; //For +- 500 degrees/s
+//const float fConvRPS= 1.2141420883e-3; //For +- 2000 degrees/s
+
+const float fConvRPS= 1.3323124427480916030534351145038e-4; //For +- 250 degrees/s
+
+class ITG3200 {
+protected:
+    I2C & I2CBus;
+    Timer & GlobalTime;
+
+public:
+    //Offset
+    float Offset[3];
+
+    //Rotational speed around all three axes
+    short RawRate[3];       //Raw data
+    float Rate[3];          //Calibrated rotation rate in radians per second
+
+
+    //Initialization
+    ITG3200(I2C & I2CBus_, Timer & GlobalTime_);
+    void Init();
+
+
+private:
+    //Read raw data
+    void ReadRawData();
+
+public:
+    //Update Method
+    void Update();
+    char getInfo(void);
+
+    //Calibration
+    void Calibrate(int ms);
+};