library for using LSM303DM chip
LSM303.cpp
- Committer:
- fin4478
- Date:
- 2013-11-08
- Revision:
- 3:b2cc1d06e2f5
- Parent:
- 2:1052b1b97cc2
- Child:
- 4:52892e52889a
File content as of revision 3:b2cc1d06e2f5:
#include "mbed.h" #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 = 462; m_max.y = 416; m_max.z = 439; m_min.x = -645; m_min.y = -647; m_min.z = -589; #else m_max.x = 1; m_max.y = 1; m_max.z = 1; m_min.x = 0; m_min.y = 0; m_min.z = 0; #endif 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 return 1; //success } 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; } 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; 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); //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; } 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; }