Diff: modSensData.h

Revision:

4:a397b44a0fe8

Parent:

3:ffa0e1429a72

--- a/modSensData.h	Mon Mar 09 20:41:28 2015 +0000
+++ b/modSensData.h	Tue Mar 17 01:13:17 2015 +0000
@@ -6,18 +6,46 @@
 L3GD20 gyro(PTE25, PTE24);
 LSM303DLHC compass(PTE25, PTE24);
 
-#define XBIAS 0
-#define YBIAS 0
-#define ZBIAS 0
-
 #define M_PI            3.1415926535897932384626433832795f
 #define TWO_PI          6.283185307179586476925286766559f
+#define RADTODEGREE     57.295779513082320876798154814105f
+#define GRAVITY       1.0f
+#define THRESHOLD 1.1f
 
-#define RADTODEGREE     57.295779513082320876798154814105f
+// "accel x,y,z (min/max) = X_MIN/X_MAX  Y_MIN/Y_MAX  Z_MIN/Z_MAX"
+#define ACCEL_X_MIN ((float) -276)
+#define ACCEL_X_MAX ((float) 236)
+#define ACCEL_Y_MIN ((float) -261)
+#define ACCEL_Y_MAX ((float) 245)
+#define ACCEL_Z_MIN ((float) -284)
+#define ACCEL_Z_MAX ((float) 220)
 
-#define GRAVITY       1.0f
+// "magn x,y,z (min/max) = X_MIN/X_MAX  Y_MIN/Y_MAX  Z_MIN/Z_MAX"
+#define MAGN_X_MIN (-0.370909f)
+#define MAGN_X_MAX (0.569091f)
+#define MAGN_Y_MIN (-0.516364f)
+#define MAGN_Y_MAX (0.392727f)
+#define MAGN_Z_MIN (-0.618367f)
+#define MAGN_Z_MAX (0.408163f)
 
-#define THRESHOLD 1.1f
+// Sensor calibration scale and offset values
+#define ACCEL_X_OFFSET ((ACCEL_X_MIN + ACCEL_X_MAX) / 2.0f)
+#define ACCEL_Y_OFFSET ((ACCEL_Y_MIN + ACCEL_Y_MAX) / 2.0f)
+#define ACCEL_Z_OFFSET ((ACCEL_Z_MIN + ACCEL_Z_MAX) / 2.0f)
+#define ACCEL_X_SCALE (GRAVITY / (ACCEL_X_MAX - ACCEL_X_OFFSET))
+#define ACCEL_Y_SCALE (GRAVITY / (ACCEL_Y_MAX - ACCEL_Y_OFFSET))
+#define ACCEL_Z_SCALE (GRAVITY / (ACCEL_Z_MAX - ACCEL_Z_OFFSET))
+
+#define MAGN_X_OFFSET ((MAGN_X_MIN + MAGN_X_MAX) / 2.0f)
+#define MAGN_Y_OFFSET ((MAGN_Y_MIN + MAGN_Y_MAX) / 2.0f)
+#define MAGN_Z_OFFSET ((MAGN_Z_MIN + MAGN_Z_MAX) / 2.0f)
+#define MAGN_X_SCALE (100.0f / (MAGN_X_MAX - MAGN_X_OFFSET))
+#define MAGN_Y_SCALE (100.0f / (MAGN_Y_MAX - MAGN_Y_OFFSET))
+#define MAGN_Z_SCALE (100.0f / (MAGN_Z_MAX - MAGN_Z_OFFSET))
+
+
+
+//calibrated reading = (reading - offset) * scale
 
 Timer t1;
 
@@ -42,6 +70,16 @@
 void readIMU()
 {
     compass.read(&accel[0], &accel[1], &accel[2], &magnetom[0], &magnetom[1], &magnetom[2]);
+    //Compensate magnetometer error
+    //magnetom[0] -= MAGN_X_OFFSET;
+    //magnetom[1] -= MAGN_Y_OFFSET;
+    //magnetom[2] -= MAGN_Z_OFFSET;
+    
+    //Compensate accelerometer error
+    //accel[0] = (accel[0] - ACCEL_X_OFFSET) * ACCEL_X_SCALE;
+    //accel[1] = (accel[1] - ACCEL_Y_OFFSET) * ACCEL_Y_SCALE;
+    //accel[2] = (accel[2] - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE;
+    
     gyro.read(&degGyro[0], &degGyro[1], &degGyro[2]);
 }
 
@@ -107,20 +145,6 @@
   roll = atan2(temp2[1], temp2[2]);
 }
 
-
-/********************************************************************************************************************************/
-//                                                      compensateAcc
-/********************************************************************************************************************************/
-/*
-void compensateAcc() 
-{
-    // Compensate accelerometer error
-    accel[0] = (accel[0] - ACCEL_X_OFFSET) * ACCEL_X_SCALE;
-    accel[1] = (accel[1] - ACCEL_Y_OFFSET) * ACCEL_Y_SCALE;
-    accel[2] = (accel[2] - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE;
-}
-*/
-
 /********************************************************************************************************************************/
 //                                                      rotateBTI
 /********************************************************************************************************************************/