Diff: LSM303.cpp

Revision:

5:9786e0a13a3a

Parent:

4:52892e52889a

Child:

6:22556393747b

--- a/LSM303.cpp	Mon Nov 11 08:46:17 2013 +0000
+++ b/LSM303.cpp	Tue Aug 12 11:43:45 2014 +0000
@@ -2,26 +2,40 @@
 #include <math.h>
 #include "LSM303.h"
 
-#define ACCELE_SCALE 2  // accelerometer full-scale, should be 2
 
 I2C i2c(P0_5, P0_4);
 
 int LSM303::setup()
 {
-#if 1 //set to 0 when calibrating
-    m_max.x = 517;
-    m_max.y = 469;
-    m_max.z = 667;
-    m_min.x = -688;
-    m_min.y = -749;
-    m_min.z = -472;
-#else 
+#ifdef CALIBRATING //set in LSM303.h
     m_max.x = 1;
     m_max.y = 1;
     m_max.z = 1;
     m_min.x = 0;
     m_min.y = 0;
     m_min.z = 0;
+    
+    a_max.x = 1;
+    a_max.y = 1;
+    a_max.z = 1;
+    a_min.x = 0;
+    a_min.y = 0;
+    a_min.z = 0;
+    
+#else 
+    m_min.x = -671;
+    m_min.y = -704;
+    m_min.z = -450;
+    m_max.x = 462;
+    m_max.y = 419;
+    m_max.z = 578; 
+    
+    a_min.x = -584;
+    a_min.y = -776;
+    a_min.z = -664; 
+    a_max.x = 48;
+    a_max.y = 32;
+    a_max.z = 992;
 #endif
     LSM303_write(0x27, CTRL_REG1_A);
     LSM303_write(0x00, CTRL_REG4_A);
@@ -50,16 +64,14 @@
 
 float LSM303::getTiltHeading()
 {
-    //shift and scale
-    a.x = a.x / 32768 * ACCELE_SCALE;
-    a.y = a.y / 32768 * ACCELE_SCALE;
-    a.z = a.z + 950;
-    a.z = a.z / 32768 * ACCELE_SCALE;
-
-    m.x = (m.x - m_min.x) / (m_max.x - m_min.x) * 2 - 1.0;
-    m.y = (m.y - m_min.y) / (m_max.y - m_min.y) * 2 - 1.0;
-    m.z = (m.z - m_min.z) / (m_max.z - m_min.z) * 2 - 1.0;
-
+    a.x -= ((int32_t)a_min.x + a_max.x) / 2;
+    a.y -= ((int32_t)a_min.y + a_max.y) / 2;
+    a.z -= ((int32_t)a_min.z + a_max.z) / 2;
+    // subtract offset (average of min and max) from magnetometer readings
+    m.x -= ((int32_t)m_min.x + m_max.x) / 2;
+    m.y -= ((int32_t)m_min.y + m_max.y) / 2;
+    m.z -= ((int32_t)m_min.z + m_max.z) / 2;
+    
     vector_normalize(&a);
     vector_normalize(&m);
     //see appendix A in app note AN3192
@@ -68,9 +80,10 @@
     float heading = 0;
     float xh = m.x * cos(pitch) + m.z * sin(pitch);
     float yh = m.x * sin(roll) * sin(pitch) + m.y * cos(roll) - m.z * sin(roll) * cos(pitch);
-    //float zh = -m.x * cos(roll) * sin(pitch) + m.y * sin(roll) + m.z * cos(roll) * cos(pitch);
+ 
     heading = 180 * atan2(yh, xh)/PI;
     if (heading < 0) heading += 360;
+    
     return heading;
 }