Diff: LSM303DLHC.cpp

Revision:

6:8b1fada3fc6d

Parent:

3:4d9465e7e10e

--- a/LSM303DLHC.cpp	Thu Feb 21 00:20:17 2013 +0000
+++ b/LSM303DLHC.cpp	Sat Mar 25 16:46:50 2017 +0000
@@ -107,6 +107,19 @@
 }
 
 
+static float magn_ellipsoid_center_x = -0.0446786;
+static float magn_ellipsoid_center_y = 0.0251516;
+static float magn_ellipsoid_center_z = -0.44229;
+
+static float magn_ellipsoid_transform_0_x = 0.978841;
+static float magn_ellipsoid_transform_0_y = 0.0078251;
+static float magn_ellipsoid_transform_0_z = 0.0120719;
+static float magn_ellipsoid_transform_1_x = 0.0078251;
+static float magn_ellipsoid_transform_1_y = 0.996114;
+static float magn_ellipsoid_transform_1_z = 0.00302665;
+static float magn_ellipsoid_transform_2_x = 0.0120719;
+static float magn_ellipsoid_transform_2_y = 0.00302665;
+static float magn_ellipsoid_transform_2_z = 0.936561;
 bool LSM303DLHC::read(float *ax, float *ay, float *az, float *mx, float *my, float *mz) {
     char acc[6], mag[6];
  
@@ -115,10 +128,14 @@
         *ay =  float(short(acc[3] << 8 | acc[2]))/8192;
         *az =  float(short(acc[5] << 8 | acc[4]))/8192;
         //full scale magnetic readings are from -2048 to 2047
-        //gain is x,y =1100; z = 980 LSB/gauss
-        *mx = float(short(mag[0] << 8 | mag[1]))/1100;
-        *mz = float(short(mag[2] << 8 | mag[3]))/980;
-        *my = float(short(mag[4] << 8 | mag[5]))/1100;
+        
+        //gain is x,y = 1100; z = 980 LSB/gauss
+        float mx_tmp = float(short(mag[0] << 8 | mag[1]))/1100 - magn_ellipsoid_center_x;
+        float my_tmp = float(short(mag[2] << 8 | mag[3]))/1100 - magn_ellipsoid_center_y;
+        float mz_tmp = float(short(mag[4] << 8 | mag[5]))/980 - magn_ellipsoid_center_z;
+        *mx = magn_ellipsoid_transform_0_x * mx_tmp + magn_ellipsoid_transform_0_y * my_tmp + magn_ellipsoid_transform_0_z * mz_tmp - 0.0791235566;
+        *my = magn_ellipsoid_transform_1_x * mx_tmp + magn_ellipsoid_transform_1_y * my_tmp + magn_ellipsoid_transform_1_z * mz_tmp + 0.8915525079;
+        *mz = magn_ellipsoid_transform_2_x * mx_tmp + magn_ellipsoid_transform_2_y * my_tmp + magn_ellipsoid_transform_2_z * mz_tmp + 0.5963887572;
  
         return true;
     }