library for using LSM303DM chip
Revision 5:9786e0a13a3a, committed 2014-08-12
- Comitter:
- fin4478
- Date:
- Tue Aug 12 11:43:45 2014 +0000
- Parent:
- 4:52892e52889a
- Child:
- 6:22556393747b
- Commit message:
- accelometer calibration added
Changed in this revision
LSM303.cpp | Show annotated file Show diff for this revision Revisions of this file |
LSM303.h | Show annotated file Show diff for this revision Revisions of this file |
--- a/LSM303.cpp Mon Nov 11 08:46:17 2013 +0000 +++ b/LSM303.cpp Tue Aug 12 11:43:45 2014 +0000 @@ -2,26 +2,40 @@ #include <math.h> #include "LSM303.h" -#define ACCELE_SCALE 2 // accelerometer full-scale, should be 2 I2C i2c(P0_5, P0_4); int LSM303::setup() { -#if 1 //set to 0 when calibrating - m_max.x = 517; - m_max.y = 469; - m_max.z = 667; - m_min.x = -688; - m_min.y = -749; - m_min.z = -472; -#else +#ifdef CALIBRATING //set in LSM303.h m_max.x = 1; m_max.y = 1; m_max.z = 1; m_min.x = 0; m_min.y = 0; m_min.z = 0; + + a_max.x = 1; + a_max.y = 1; + a_max.z = 1; + a_min.x = 0; + 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; + + a_min.x = -584; + a_min.y = -776; + a_min.z = -664; + a_max.x = 48; + a_max.y = 32; + a_max.z = 992; #endif LSM303_write(0x27, CTRL_REG1_A); LSM303_write(0x00, CTRL_REG4_A); @@ -50,16 +64,14 @@ float LSM303::getTiltHeading() { - //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; - + 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; + vector_normalize(&a); vector_normalize(&m); //see appendix A in app note AN3192 @@ -68,9 +80,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); + heading = 180 * atan2(yh, xh)/PI; if (heading < 0) heading += 360; + return heading; }
--- a/LSM303.h Mon Nov 11 08:46:17 2013 +0000 +++ b/LSM303.h Tue Aug 12 11:43:45 2014 +0000 @@ -97,6 +97,7 @@ #define LSM303_WHO_AM_I_M 0x0F // DLM only #define PI 3.14159265 +//#define CALIBRATING //uncomment when calibrating class LSM303 {// I am LSM303DLM @@ -106,6 +107,8 @@ } Plane; Plane a; // accelerometer readings + Plane a_max; + Plane a_min; Plane m; // magnetometer readings Plane m_max; // maximum magnetometer values, used for calibration Plane m_min; // minimum magnetometer values, used for calibration