library for using LSM303DM chip
Diff: LSM303.cpp
- Revision:
- 0:4d358fbeab6e
- Child:
- 1:322c80f884d3
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LSM303.cpp Thu Oct 10 06:57:53 2013 +0000 @@ -0,0 +1,159 @@ +#include "mbed.h" +#include <math.h> +#include "LSM303.h" + +I2C i2c(P0_5, P0_4); + +int LSM303::setup() +{ +#if 1 + m_max.x = 239; + m_max.y = 242; + m_max.z = 158; + m_min.x = -637; + m_min.y = -498; + m_min.z = -538; +#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 + return initLSM303(ACCELE_SCALE); // Initialize the LSM303, using a SCALE full-scale range +} +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; +} + +int LSM303::initLSM303(int fs) +{ + if (LSM303_write(0x27, CTRL_REG1_A) == 1) { // 0x27 = normal power mode, all accel axes on + //LSM303_write(16, CTRL_REG2_A); // enable internal hp filter + } + if ((fs==8)||(fs==4)) + LSM303_write((0x00 | (fs-fs/2-1)<<4), CTRL_REG4_A); // set full-scale + else + LSM303_write(0x00, CTRL_REG4_A); + //LSM303_write(20, CRA_REG_M); // 20 = mag 30Hz output rate + 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; +} + +float LSM303::getTiltHeading() +{ + //shift and scale + a.x = a.x / 32768 * ACCELE_SCALE; + a.y = a.y / 32768 * ACCELE_SCALE; + 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; +} + +int 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] ); + return 0; +} + +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; +}