Diff: ITG3200.cpp

Revision:

9:05396b551a9a

Parent:

8:ac0365ab3cef

Child:

11:9a354f34d8e3

--- a/ITG3200.cpp	Tue Oct 02 17:09:20 2012 +0000
+++ b/ITG3200.cpp	Tue Oct 02 18:04:13 2012 +0000
@@ -68,6 +68,17 @@
 
 }
 
+void ITG3200::setCalibrationCurve(const float offset[3], const float slope[3]){
+    if(offset){
+        for(int i = 0; i < 3; i++)
+            this->foffset[i] = offset[i];
+    }
+    if(slope){
+        for(int i = 0; i < 3; i++)
+            this->slope[i] = slope[i];
+    }
+}
+
 char ITG3200::getWhoAmI(void){
 
     //WhoAmI Register address.
@@ -242,6 +253,24 @@
         readings[i] = swapExtend(&rx[i * 2]);
 }
 
+void ITG3200::getGyroXYZ(int readings[3], Correction corr){
+    getRawGyroXYZ(readings);
+    switch(corr){
+    case OffsetCorrection:
+        for(int i = 0; i < 3; i++)
+            readings[i] -= offset[i];
+        break;
+    case Calibration:
+        {
+            float temp = getTemperature();
+            for(int i = 0; i < 3; i++)
+                readings[i] -= slope[i] * temp + foffset[i];
+        }
+        break;
+    }
+}
+
+
 char ITG3200::getPowerManagement(void){
 
     char tx = PWR_MGM_REG;