LSM303DLHC library with magneometer calibration support
Fork of LSM303DLHC by
Diff: LSM303DLHC.cpp
- Revision:
- 6:8b1fada3fc6d
- Parent:
- 3:4d9465e7e10e
diff -r 612f7d5a822d -r 8b1fada3fc6d LSM303DLHC.cpp --- 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; }