library for using LSM303DM chip
LSM303.cpp
- Committer:
- fin4478
- Date:
- 2014-09-08
- Revision:
- 6:22556393747b
- Parent:
- 5:9786e0a13a3a
File content as of revision 6:22556393747b:
#include "mbed.h" #include <math.h> #include "LSM303.h" I2C i2c(P0_5, P0_4); void LSM303::setup() { #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 = -690; m_min.y = -702; m_min.z = -433; m_max.x = 480; m_max.y = 414; m_max.z = 589; 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(0x00, MR_REG_M); // 0x00 = continouous conversion mode } int LSM303::testAcc() { if (i2c.write(LSM303_ACC, NULL, 0) ==0) return LSM303_ACC; return 255; } int LSM303::testMag() { if (i2c.write(LSM303_MAG, NULL, 0) ==0) if (LSM303_read(LSM303_WHO_AM_I_M)==0x3C) { return LSM303_WHO_AM_I_M; } else { return LSM303_MAG; } 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 pitch = asin(-a.x); roll = asin(a.y/cos(pitch)); 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); heading = 180 * atan2(yh, xh)/PI; if (heading < 0) heading += 360; return heading; } void LSM303::vector_cross( const Plane *a,const Plane *b, Plane *out ) { out->x = a->y*b->z - a->z*b->y; out->y = a->z*b->x - a->x*b->z; out->z = a->x*b->y - a->y*b->x; } float LSM303::vector_dot( const Plane *a,const Plane *b ) { return a->x*b->x+a->y*b->y+a->z*b->z; } void LSM303::vector_normalize( Plane *a ) { float mag = sqrt(vector_dot(a,a)); a->x /= mag; a->y /= mag; a->z /= mag; } void LSM303::getLSM303_accel() { char data[1] = { OUT_X_L_A | (1<<7)}; char out[6] = {0,0,0,0,0,0}; i2c.write( LSM303_ACC, data,1); i2c.read( LSM303_ACC, out, 6); a.x = short( (((short)out[1]) << 8) | out[0] ); a.y = short( (((short)out[3]) << 8) | out[2] ); a.z = short( (((short)out[5]) << 8) | out[4] ); } void LSM303::getLSM303_mag() { char data[1] = { OUT_X_H_M }; char out[6]; i2c.write( LSM303_MAG, data, 1 ); i2c.read( LSM303_MAG, out, 6 ); // DLM, DLHC: register address for Z comes before Y m.x = short( out[0] << 8 | out[1] ); m.y = short( out[4] << 8 | out[5] ); m.z= short( out[2] << 8 | out[3] ); } int LSM303::LSM303_read(int address) { if (address >= 0x20) { _i2c_address = LSM303_ACC; } else { _i2c_address = LSM303_MAG; } char value[1]; char data[1] = { address }; i2c.write( _i2c_address, data, 1 ); i2c.read( _i2c_address, value, 1 ); return value[0]; } int LSM303::LSM303_write(int data, int address) { if (address >= 0x20) { _i2c_address = LSM303_ACC; } else { _i2c_address = LSM303_MAG; } char out[2] = { address, data }; i2c.write( _i2c_address, out, 2 ); return 0; }