Diff: LSM303.cpp

Revision:

1:322c80f884d3

Parent:

0:4d358fbeab6e

Child:

2:1052b1b97cc2

--- a/LSM303.cpp	Thu Oct 10 06:57:53 2013 +0000
+++ b/LSM303.cpp	Fri Oct 11 12:51:54 2013 +0000
@@ -6,14 +6,14 @@
 
 int LSM303::setup()
 {
-#if 1  
-    m_max.x = 239;
+#if 1
+    m_max.x = 242;
     m_max.y = 242;
     m_max.z = 158;
-    m_min.x = -637;
+    m_min.x = -498;
     m_min.y = -498;
     m_min.z = -538;
-#else    
+#else
     m_max.x = 1;
     m_max.y = 1;
     m_max.z = 1;
@@ -31,14 +31,13 @@
 
 int LSM303::testMag()
 {
-    if (i2c.write(LSM303_MAG, NULL, 0) ==0) 
+    if (i2c.write(LSM303_MAG, NULL, 0) ==0)
         if (LSM303_read(LSM303_WHO_AM_I_M)==0x3C) {
             return LSM303_WHO_AM_I_M;
-        }
-        else {
+        } else {
             return LSM303_MAG;
         }
-    
+
     return 255;
 }
 
@@ -63,12 +62,13 @@
     //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;
-    
+
     vector_normalize(&a);
     vector_normalize(&m);
     //see appendix A in app note AN3192
@@ -77,10 +77,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);      
+    //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;
+    return heading;
 }
 
 void LSM303::vector_cross( const Plane *a,const Plane *b, Plane *out )