Diff: LSM303.cpp

Revision:

6:22556393747b

Parent:

5:9786e0a13a3a

--- a/LSM303.cpp	Tue Aug 12 11:43:45 2014 +0000
+++ b/LSM303.cpp	Mon Sep 08 01:46:11 2014 +0000
@@ -5,7 +5,7 @@
 
 I2C i2c(P0_5, P0_4);
 
-int LSM303::setup()
+void LSM303::setup()
 {
 #ifdef CALIBRATING //set in LSM303.h
     m_max.x = 1;
@@ -22,28 +22,28 @@
     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; 
+#else
+    m_min.x = -690;
+    m_min.y = -702;
+    m_min.z = -433;
+    m_max.x = 480;
+    m_max.y = 414;
+    m_max.z = 589; 
     
-    a_min.x = -584;
-    a_min.y = -776;
-    a_min.z = -664; 
-    a_max.x = 48;
-    a_max.y = 32;
-    a_max.z = 992;
+    a_min.x = -542;
+    a_min.y = -644;
+    a_min.z = -632; 
+    a_max.x = 496;
+    a_max.y = 472;
+    a_max.z = 566;
 #endif
+    getScale(&scale);
     LSM303_write(0x27, CTRL_REG1_A);
     LSM303_write(0x00, CTRL_REG4_A);
-    LSM303_write(MAG_SCALE_1_3, CRB_REG_M); //magnetic scale = +/-1.3Gauss
+    LSM303_write(MAG_SCALE_1_3 , CRB_REG_M); //magnetic scale = +/-1.3Gauss
     LSM303_write(0x00, MR_REG_M);  // 0x00 = continouous conversion mode
+}
 
-    return 1; //success
-}
 int LSM303::testAcc()
 {
     if (i2c.write(LSM303_ACC, NULL, 0) ==0) return LSM303_ACC;
@@ -62,16 +62,54 @@
     return 255;
 }
 
+
+void LSM303::getScale(Plane *scale)  {
+    Plane vmax;
+    Plane vmin;
+    Plane avgs;
+    //First the hard iron errors are removed from the maximums and minimum magnetometer vectors. 
+    //These minimum and maximum vectors are the same as the ones being used to correct for hard iron errors.
+    vmax.x= m_max.x - ((m_min.x + m_max.x)/2.0);
+    vmax.y= m_max.y - ((m_min.y + m_max.y)/2.0);
+    vmax.z =m_max.z - ((m_min.z + m_max.z)/2.0);
+
+    vmin.x = m_min.x - ((m_min.x + m_max.x)/2.0);
+    vmin.y = m_min.y - ((m_min.y + m_max.y)/2.0);
+    vmin.z = m_min.z - ((m_min.z + m_max.z)/2.0);
+
+    //The average distance from the centre is now calculated. We want to know how far from the centre, so the negative values are inverted.
+    //avgs = vmax + (vmin*-1); //multiply by -1 to make negative values positive
+    //avgs = avgs / 2.0;
+    avgs.x = (vmax.x + vmin.x*-1)/2.0;
+    avgs.y = (vmax.y + vmin.y*-1)/2.0;
+    avgs.z = (vmax.z + vmin.z*-1)/2.0;
+    //The components are now averaged out
+    float avg_rad = avgs.x + avgs.y + avgs.z;
+    avg_rad /= 3.0;
+    //Finally calculate the scale factor by dividing average radius by average value for that axis.
+    scale->x = (avg_rad/avgs.x);
+    scale->y = (avg_rad/avgs.y);
+    scale->z = (avg_rad/avgs.z);
+}
+
 float LSM303::getTiltHeading()
 {
+    getLSM303_accel();        
+    getLSM303_mag();  // get the accel and magnetometer values, store them in a and m
+    
     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;
     
+    m.x *= scale.x;
+    m.y *= scale.y;
+    m.z *= scale.z;
+    
     vector_normalize(&a);
     vector_normalize(&m);
     //see appendix A in app note AN3192