Diff: ITG3200.cpp

Revision:

3:eea9733ca427

Parent:

2:f44a902ba081

Child:

4:155c44407af5

--- a/ITG3200.cpp	Wed Sep 12 11:36:48 2012 +0000
+++ b/ITG3200.cpp	Wed Sep 12 14:37:34 2012 +0000
@@ -32,13 +32,12 @@
  * http://invensense.com/mems/gyro/documents/PS-ITG-3200-00-01.4.pdf
  */
 
-/**
- * Includes
- */
 #include "ITG3200.h"
 
 ITG3200::ITG3200(PinName sda, PinName scl) : i2c_(sda, scl) {
 
+    offset[0] = offset[1] = offset[2] = 0;
+
     //400kHz, fast mode.
     i2c_.frequency(400000);
     
@@ -266,7 +265,7 @@
     
 }
 
-void ITG3200::getGyroXYZ(int readings[3]){
+void ITG3200::getRawGyroXYZ(int readings[3]){
 
     char tx = GYRO_XOUT_H_REG;
     char rx[2];
@@ -301,3 +300,22 @@
     i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
 
 }
+
+void ITG3200::calibrate(double time){
+    static const double deltaTime = 0.002;
+    long sum[3] = {0};
+    int sumCount = 0;
+    for(; 0 < time; time -= deltaTime){
+        int gyro[3];
+        getGyroXYZ(gyro);
+        for(int i = 0; i < 3; i++)
+            sum[i] += gyro[i];
+        sumCount++;
+    }
+    
+    // Avoid zero division
+    if(0 < sumCount){
+        for(int i = 0; i < 3; i++)
+            offset[i] = sum[i] / sumCount;
+    }
+}