AHRS library for the Polulu minIMU-9 Ability to interface with the Polulu Python minIMU-9 monitor
LSM303.cpp
- Committer:
- krmreynolds
- Date:
- 2012-04-23
- Revision:
- 1:3272ece36ce1
- Parent:
- 0:dc35364e2291
File content as of revision 1:3272ece36ce1:
#include <LSM303.h> #include "LSM303.h" #include <math.h> LSM303::LSM303( PinName sda = p9, PinName scl = p10 ) : i2c( sda, scl ) { // These are just some values for a particular unit; it is recommended that // a calibration be done for your particular unit. m_max.x = +540; m_max.y = +500; m_max.z = 180; m_min.x = -520; m_min.y = -570; m_min.z = -770; _device = LSM303DLM_DEVICE; acc_address = ACC_ADDRESS_SA0_A_LOW; } // Public Methods ////////////////////////////////////////////////////////////// void LSM303::init(byte device, byte sa0_a) { Serial pc(USBTX, USBRX); _device = device; switch (_device) { case LSM303DLH_DEVICE: case LSM303DLM_DEVICE: if (sa0_a == LSM303_SA0_A_LOW) acc_address = ACC_ADDRESS_SA0_A_LOW; else if (sa0_a == LSM303_SA0_A_HIGH) acc_address = ACC_ADDRESS_SA0_A_HIGH; else acc_address = (detectSA0_A() == LSM303_SA0_A_HIGH) ? ACC_ADDRESS_SA0_A_HIGH : ACC_ADDRESS_SA0_A_LOW; break; case LSM303DLHC_DEVICE: acc_address = ACC_ADDRESS_SA0_A_HIGH; break; default: // try to auto-detect device if (detectSA0_A() == LSM303_SA0_A_HIGH) { // if device responds on 0011001b (SA0_A is high), assume DLHC acc_address = ACC_ADDRESS_SA0_A_HIGH; _device = LSM303DLHC_DEVICE; } else { // otherwise, assume DLH or DLM (pulled low by default on Pololu boards); query magnetometer WHO_AM_I to differentiate these two acc_address = ACC_ADDRESS_SA0_A_LOW; _device = (readMagReg(LSM303_WHO_AM_I_M) == 0x3C) ? LSM303DLM_DEVICE : LSM303DLH_DEVICE; } } } // Turns on the LSM303's accelerometer and magnetometers and places them in normal // mode. void LSM303::enableDefault(void) { // Enable Accelerometer // 0x27 = 0b00100111 // Normal power mode, all axes enabled writeAccReg(LSM303_CTRL_REG1_A, 0x27); // Enable Magnetometer // 0x00 = 0b00000000 // Continuous conversion mode writeMagReg(LSM303_MR_REG_M, 0x00); } // Writes an accelerometer register void LSM303::writeAccReg(byte reg, byte value) { char data[2] = { reg, value }; i2c.write( acc_address, data, 2 ); } // Reads an accelerometer register int LSM303::readAccReg(byte reg) { char value[1]; char data[1] = { reg }; i2c.write( acc_address, data, 1 ); i2c.read( acc_address, value, 1 ); return value[0]; } // Writes a magnetometer register void LSM303::writeMagReg(byte reg, byte value) { char data[2] = { reg, value }; i2c.write( MAG_ADDRESS, data, 2 ); } // Reads a magnetometer register int LSM303::readMagReg(int reg) { char value[1]; char data[1]; // if dummy register address (magnetometer Y/Z), use device type to determine actual address if (reg < 0) { switch (reg) { case LSM303_OUT_Y_H_M: reg = (_device == LSM303DLH_DEVICE) ? LSM303DLH_OUT_Y_H_M : LSM303DLM_OUT_Y_H_M; break; case LSM303_OUT_Y_L_M: reg = (_device == LSM303DLH_DEVICE) ? LSM303DLH_OUT_Y_L_M : LSM303DLM_OUT_Y_L_M; break; case LSM303_OUT_Z_H_M: reg = (_device == LSM303DLH_DEVICE) ? LSM303DLH_OUT_Z_H_M : LSM303DLM_OUT_Z_H_M; break; case LSM303_OUT_Z_L_M: reg = (_device == LSM303DLH_DEVICE) ? LSM303DLH_OUT_Z_L_M : LSM303DLM_OUT_Z_L_M; break; } } data[0] = reg; i2c.write( MAG_ADDRESS, data, 1 ); i2c.read( MAG_ADDRESS, value, 1 ); return value[0]; } // Reads the 3 accelerometer channels and stores them in vector a void LSM303::readAcc(void) { char data[1] = { LSM303_OUT_X_L_A | (1<<7) }; char out[6]; i2c.write( acc_address, data, 1 ); i2c.read( acc_address, out, 6 ); a.x = short( ( out[1] << 8 | out[0] ) >> 4 ); a.y = short( ( out[3] << 8 | out[2] ) >> 4 ); a.z = short( ( out[5] << 8 | out[4] ) >> 4 ); } // Reads the 3 magnetometer channels and stores them in vector m void LSM303::readMag(void) { Serial pc(USBTX, USBRX); char data[1] = { LSM303_OUT_X_H_M }; char out[6]; i2c.write( MAG_ADDRESS, data, 1 ); i2c.read( MAG_ADDRESS, out, 6 ); if (_device == LSM303DLH_DEVICE) { // DLH: register address for Y comes before Z m.x = short( out[0] << 8 | out[1] ); m.y = short( out[2] << 8 | out[3] ); m.z = short( out[4] << 8 | out[5] ); } else { // 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] ); } } // Reads all 6 channels of the LSM303 and stores them in the object variables void LSM303::read(void) { readAcc(); readMag(); } // Returns the number of degrees from the -Y axis that it // is pointing. int LSM303::heading(void) { return heading((Plane) { 0,-1,0 }); } // Returns the number of degrees from the From vector projected into // the horizontal plane is away from north. // // Description of heading algorithm: // Shift and scale the magnetic reading based on calibration data to // to find the North vector. Use the acceleration readings to // determine the Down vector. The cross product of North and Down // vectors is East. The vectors East and North form a basis for the // horizontal plane. The From vector is projected into the horizontal // plane and the angle between the projected vector and north is // returned. int LSM303::heading(Plane from) { // shift and 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; Plane temp_a = a; // normalize vector_normalize(&temp_a); //vector_normalize(&m); // compute E and N Plane E; Plane N; vector_cross(&m, &temp_a, &E); vector_normalize(&E); vector_cross(&temp_a, &E, &N); // compute heading int heading = (int)(atan2(vector_dot(&E, &from), vector_dot(&N, &from)) * 180 / M_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; } byte LSM303::detectSA0_A(void) { char out[1]; char data[1] = { LSM303_CTRL_REG1_A }; i2c.write( ACC_ADDRESS_SA0_A_LOW, data, 1 ); if ( 0 == i2c.read( ACC_ADDRESS_SA0_A_LOW, out, 1 ) ) { return LSM303_SA0_A_LOW; } else { return LSM303_SA0_A_HIGH; } }