Revision 3:eea9733ca427, committed 2012-09-12

Comitter:

gltest26

Date:

Wed Sep 12 14:37:34 2012 +0000

Parent:

2:f44a902ba081

Child:

4:155c44407af5

Commit message:

Added drift offset calibration capability.

Changed in this revision

--- 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;
+    }
+}

--- a/ITG3200.h	Wed Sep 12 11:36:48 2012 +0000
+++ b/ITG3200.h	Wed Sep 12 14:37:34 2012 +0000
@@ -1,4 +1,5 @@
 /**
+ * @file ITG3200.h
  * @author Aaron Berk
  *
  * @section LICENSE
@@ -26,6 +27,7 @@
  * @section DESCRIPTION
  *
  * ITG-3200 triple axis, digital interface, gyroscope.
+ * Forked from Aaron Berk's work.
  *
  * Datasheet:
  *
@@ -35,12 +37,12 @@
 #ifndef ITG3200_H
 #define ITG3200_H
 
-/**
+/*
  * Includes
  */
 #include "mbed.h"
 
-/**
+/*
  * Defines
  */
 #define ITG3200_I2C_ADDRESS 0x68 //7-bit address.
@@ -86,6 +88,12 @@
 
 public:
 
+    /**
+     * The I2C address that can be passed directly to i2c object (it's already shifted 1 bit left).
+     *
+     * You don't need to manually set or clear the LSB when calling I2C::read() or I2C::write(),
+     * the library takes care of it.  We just always clear the LSB.
+     */
     static const int I2C_ADDRESS = 0xD0;
 
     /**
@@ -366,6 +374,39 @@
      * @param config The configuration byte to write to the PWR_MGM register.
      */
     void setPowerManagement(char config);
+    
+    /**
+     * Calibrate the sensor drift by sampling zero offset.
+     *
+     * Be sure to keep the sensor stationary while sampling offset.
+     *
+     * Once this function is invoked, following getGyroXYZ*() functions return
+     * corrected values.
+     *
+     * If the drift value changes over time, you can call this function once in a while
+     * to follow it. But don't forget to fix the sensor while calibrating!
+     *
+     * @param time The time span to sample and average offset.
+     *             Sampling rate is limited, so giving long time to calibrate will improve
+     *             correction quality.
+     */
+    void calibrate(double time);
+
+protected:
+    /**
+     * An internal method to acquire gyro sensor readings before calibration correction.
+     *
+     * Protected for the time being, although there could be cases that raw values are
+     * appreciated by the user.
+     */
+    void getRawGyroXYZ(int readings[3]);
+
+    /**
+     * Offset values that will be subtracted from output.
+     *
+     * TODO: temperature drift calibration
+     */
+    int offset[3];
 
 private:
 
@@ -374,6 +415,12 @@
 };
 
 
+inline void ITG3200::getGyroXYZ(int readings[3]){
+    getRawGyroXYZ(readings);
+    for(int i = 0; i < 3; i++)
+        readings[i] -= offset[i];
+}
+
 inline void ITG3200::getGyroXYZDegrees(double readings[3]){
     int intData[3];
     getGyroXYZ(intData);