Revision 6:22556393747b, committed 2014-09-08

Comitter:

fin4478

Date:

Mon Sep 08 01:46:11 2014 +0000

Parent:

5:9786e0a13a3a

Commit message:

soft iron correction scale added

Changed in this revision

--- 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

--- a/LSM303.h	Tue Aug 12 11:43:45 2014 +0000
+++ b/LSM303.h	Mon Sep 08 01:46:11 2014 +0000
@@ -1,54 +1,6 @@
 #ifndef LSM303_h
 #define LSM303_h
 #include "mbed.h"
-/* LSM303DLM  mbed code based on AN3192 Application note and LSM303DLH example code by Jim Lindblom SparkFun Electronics 
-  modified by Frankie.Chu to arduino in year 2012.
-
-   date: 13/10/13
-   license: Use this with your own risk:-)
-
-   Calibration is a must to make your compass to work:
-   
-   //displays integer value with sign in 4 digit led display and waits delay seconds
-   void displayInt(TM1637 &tm, int value, float delay)...
-   // displays a digit in position of 4 digit led display
-   void TM1637::display(uint8_t position,int8_t digit)...
-   //calibration loop
-   for(int i = 0; i <200; i++) {
-
-        lsm.getLSM303_mag();
-
-// Mmin handler
-        if(lsm.m.x  <  lsm.m_min.x)
-            lsm.m_min.x  =  lsm.m.x;
-
-        if(lsm.m.y  <  lsm.m_min.y)
-            lsm.m_min.y  =  lsm.m.y;
-
-        if(lsm.m.z  <  lsm.m_min.z)
-            lsm.m_min.z  =  lsm.m.z;
-
-// Mmax handler
-        if(lsm.m.x  >  lsm.m_max.x)
-            lsm.m_max.x  =  lsm.m.x ;
-
-        if(lsm.m.y  >  lsm.m_max.y)
-            lsm.m_max.y =  lsm.m.y;
-
-        if(lsm.m.z  >  lsm.m_max.z)
-            lsm.m_max.z  =  lsm.m.z;
-
-        tm.clearDisplay();
-        tm.display(i%4,0);
-        wait(0.1);
-    }
-    displayInt(tm, lsm.m_min.x, 8);
-    displayInt(tm, lsm.m_min.y, 8);
-    displayInt(tm, lsm.m_min.z, 8);
-    displayInt(tm, lsm.m_max.x, 8);
-    displayInt(tm, lsm.m_max.y, 8);
-    displayInt(tm, lsm.m_max.z, 8);
-*/
 
 /* LSM303 Address definitions */
 #define LSM303_MAG  0x3C  // assuming SA0 grounded
@@ -99,42 +51,85 @@
 #define PI                    3.14159265
 //#define CALIBRATING //uncomment when calibrating
 
+/** LSM303DLM  mbed code based on AN3192 Application note and LSM303DLH example code by Jim Lindblom SparkFun Electronics 
+  modified by Frankie.Chu to arduino in year 2012.
+
+   date: 13/10/13
+   license: Use this with your own risk:-)
+
+   Calibration the compass and accelometer is a must to make your compass to work:
+@code
+   //calibration loop
+   for(int i = 0; i <200; i++) {
+
+        lsm.getLSM303_mag();
+
+// Mmin handler
+        if(lsm.m.x  <  lsm.m_min.x)
+            lsm.m_min.x  =  lsm.m.x;
+
+        if(lsm.m.y  <  lsm.m_min.y)
+            lsm.m_min.y  =  lsm.m.y;
+
+        if(lsm.m.z  <  lsm.m_min.z)
+            lsm.m_min.z  =  lsm.m.z;
+
+// Mmax handler
+        if(lsm.m.x  >  lsm.m_max.x)
+            lsm.m_max.x  =  lsm.m.x ;
+
+        if(lsm.m.y  >  lsm.m_max.y)
+            lsm.m_max.y =  lsm.m.y;
+
+        if(lsm.m.z  >  lsm.m_max.z)
+            lsm.m_max.z  =  lsm.m.z;
+        wait(0.1);
+    }
+@endcode
+*/
 class LSM303
-{// I am  LSM303DLM
+{
 public:
+    //! A plane with x,y and z axis
     typedef struct Plane {
         float x, y, z;
     } Plane;
     
-    Plane a; // accelerometer readings
+    //! accelerometer readings
+    Plane a; 
     Plane a_max;
     Plane a_min;
-    Plane m; // magnetometer readings
+    //! magnetometer readings
+    Plane m; 
     Plane m_max; // maximum magnetometer values, used for calibration
     Plane m_min; // minimum magnetometer values, used for calibration
+    Plane scale; //soft magneting field scaling
+    //! Initialises LSM303DLM chip
+    void setup();
+    //!Tests Accelometer. Returns 0xFF on error, 0x30 if succesful.
+    int testAcc();
+    //!Tests Compass. Returns 0xFF on error, 0x0F if succesful and LSM303DLM; 0x3C if LSM303DH.
+    int testMag();   
+    //! Returns compass heading in degrees   
+    float getTiltHeading();
+    //! Reads magnetometer values to m
+    void getLSM303_mag();
+    //! Reads accelerometer values to a
+    void getLSM303_accel();
+    
+private:
+    int _i2c_address;
     float pitch;
     float roll;
-    int setup();
-    
-    void getLSM303_mag();
-    void getLSM303_accel();
+ 
     int LSM303_read(int address);
-    int LSM303_write(int data, int address);
-    
-    int testAcc();
-    int testMag();
-    
-    float getTiltHeading();
-    
+    int LSM303_write(int data, int address);    
     // Plane functions
     static void vector_cross(const Plane *a, const Plane *b, Plane *out);
     static float vector_dot(const Plane *a,const Plane *b);
     static void vector_normalize(Plane *a);
-
-
-private:
- 
-    int _i2c_address;
+    
+    void getScale(Plane *scale);
 };
 
 #endif