AHRS library for the Polulu minIMU-9 Ability to interface with the Polulu Python minIMU-9 monitor
Diff: LSM303.cpp
- Revision:
- 1:3272ece36ce1
- Parent:
- 0:dc35364e2291
--- a/LSM303.cpp Thu Apr 12 13:47:23 2012 +0000 +++ b/LSM303.cpp Mon Apr 23 14:31:08 2012 +0000 @@ -1,209 +1,228 @@ -/* mbed LSM303 Library version 0beta1 - * Copyright (c) 2012 bengo - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - #include <LSM303.h> -#include <cmath> +#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; -// LSM303 I2C addresses -const int LSM303::ACC_ADDRESS = 0x30; -const int LSM303::MAG_ADDRESS = 0x3c; -// LSM303 register addresses -const int LSM303::ACC_CTRL_REG1 = 0x20; -const int LSM303::ACC_CTRL_REG2 = 0x21; -const int LSM303::ACC_CTRL_REC3 = 0x22; -const int LSM303::ACC_CTRL_REG4 = 0x23; -const int LSM303::ACC_CTRL_REG5 = 0x24; -const int LSM303::ACC_HP_FILTER_RESET = 0x25; -const int LSM303::ACC_REFERENCE = 0x26; -const int LSM303::ACC_STATUS_REG = 0x27; -const int LSM303::ACC_OUT_X_L = 0x28; -const int LSM303::ACC_OUT_X_H = 0x29; -const int LSM303::ACC_OUT_Y_L = 0x2a; -const int LSM303::ACC_OUT_Y_H = 0x2b; -const int LSM303::ACC_OUT_Z_L = 0x2c; -const int LSM303::ACC_OUT_Z_H = 0x2d; -const int LSM303::ACC_INT1_CFG = 0x30; -const int LSM303::ACC_INT1_SOURCE = 0x31; -const int LSM303::ACC_INT1_THS = 0x32; -const int LSM303::ACC_INT1_DURATION = 0x33; -const int LSM303::ACC_INT2_CFG = 0x34; -const int LSM303::ACC_INT2_SOURCE = 0x35; -const int LSM303::ACC_INT2_THS = 0x36; -const int LSM303::ACC_INT2_DURATION = 0x37; -const int LSM303::MAG_CRA_REG = 0x00; -const int LSM303::MAG_CRB_REG = 0x01; -const int LSM303::MAG_MR_REG = 0x02; -const int LSM303::MAG_OUT_X_H = 0x03; -const int LSM303::MAG_OUT_X_L = 0x04; -const int LSM303::MAG_OUT_Y_H = 0x07; -const int LSM303::MAG_OUT_Y_L = 0x08; -const int LSM303::MAG_OUT_Z_H = 0x05; -const int LSM303::MAG_OUT_Z_L = 0x6; -const int LSM303::MAG_SR_REG = 0x9; -const int LSM303::MAG_IRA_REG = 0x0a; -const int LSM303::MAG_IRB_REG = 0x0b; -const int LSM303::MAG_IRC_REG = 0x0c; -const int LSM303::MAG_WHO_AM_I = 0x0f; -// + 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); -// ------------------------------------------- -LSM303::LSM303( PinName sda, PinName scl ) : _i2c( sda, scl ) { - // Get SA0 pin status - _bytes[0] = ACC_CTRL_REG1; - _i2c.write( ACC_ADDRESS, _bytes, 1 ); - int sa0low = _i2c.read( ACC_ADDRESS+1, _bytes, 1 ); - _bytes[0] = ACC_CTRL_REG1; - _i2c.write( ACC_ADDRESS+2, _bytes, 1 ); - int sa0hig = _i2c.read( ACC_ADDRESS+2+1, _bytes, 1 ); - if( sa0low == 0 && sa0hig != 0 ) { - _SA0Pad = 0x0; - } - else if( sa0low != 0 && sa0hig == 0 ) { - _SA0Pad = 0x2; - } - else { - _status = 1; - return; - } - // Check that you're talking with an LM303DLM device - _bytes[0] = MAG_WHO_AM_I; - _i2c.write( MAG_ADDRESS, _bytes, 1 ); - _status = _i2c.read( MAG_ADDRESS+1, _bytes, 1 ); - if( _bytes[0] == 0x3c ) { - _status = 0; - } - else { - _status = 1; - return; - } - // Enable normal mode... - // ... On accelerometer - this->accRegisterWrite( ACC_CTRL_REG1, 0x27 ); - if( _status != 0 ) { - return; - } - // ... And on magnetometer - this->magRegisterWrite( MAG_MR_REG, 0x00 ); + // 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 ); } -LSM303::LSM303( void ) : _i2c( p9, p10 ) {} -// ------------------------------------------- -int LSM303::accRegisterRead( int reg ) { - _bytes[0] = reg & 0xff; - _status = _i2c.write( ACC_ADDRESS + _SA0Pad, _bytes, 1 ); - if( _status == 0 ) { - _status = _i2c.read( ACC_ADDRESS + _SA0Pad + 1, _bytes, 1 ); - return( _bytes[0] ); - } - return( 0 ); + +// 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 ); } -// ------------------------------------------- -void LSM303::accRegisterWrite( int reg, char data ) { - _bytes[0] = reg & 0xff; - _bytes[1] = data & 0xff; - _status = _i2c.write( ACC_ADDRESS + _SA0Pad, _bytes, 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] ); + } } -// ------------------------------------------- -int LSM303::magRegisterRead( int reg ) { - _bytes[0] = reg & 0xff; - _status = _i2c.write( MAG_ADDRESS, _bytes, 1 ); - if( _status == 0 ) { - _status = _i2c.read( MAG_ADDRESS + 1, _bytes, 1 ); - return( _bytes[0] ); - } - return( 0 ); +// 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 + }); } -// ------------------------------------------- -void LSM303::magRegisterWrite( int reg, char data ) { - _bytes[0] = reg & 0xff; - _bytes[1] = data & 0xff; - _status = _i2c.write( MAG_ADDRESS, _bytes, 2 ); +// 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; } -// ------------------------------------------- -std::vector<short> LSM303::accRead( void ) { - std::vector<short> acc( 3, 0 ); - _bytes[0] = ACC_OUT_X_L | (1<<7); - _status = _i2c.write( ACC_ADDRESS + _SA0Pad, _bytes, 1 ); - if( _status == 0 ) { - _status = _i2c.read( ACC_ADDRESS + _SA0Pad + 1, _bytes, 6 ); - if( _status == 0 ) { - for( int i=0; i<3; i++ ) { - acc[i] = ( short( _bytes[2*i] ) | short(_bytes[2*i+1]) << 8 ); - } - } - } - return( acc ); -} - -// ------------------------------------------- -std::vector<float> LSM303::acceleration( void ) { - - const float cal[3][2] = { { 16291.5, -16245.4 }, { 16819.0, -16253.0 }, { 16994.8, -15525.6 } }; - - std::vector<float> acc( 3, 0 ); - int fs = ( this->accRegisterRead( ACC_CTRL_REG4 ) >> 4 ) & 0x3; - std::vector<short> a = this->accRead(); - if( _status == 0 ) { - for( int i=0; i<3; i++ ) { - acc[i] = acc[i] * ( (cal[i][0] - cal[i][1]) / 32768. ) + (cal[i][0]+cal[i][1])/2.; - acc[i] = float( a[i] ) * pow(2.,(fs+1)) / 32768.; - } - } - return( acc ); -} - -// ------------------------------------------- -std::vector<short> LSM303::magRead( void ) { - std::vector<short> mag( 3, 0 ); - _bytes[0] = MAG_OUT_X_H; - _status = _i2c.write( MAG_ADDRESS, _bytes, 1 ); - if( _status == 0 ) { - _status = _i2c.read( MAG_ADDRESS + 1, _bytes, 6 ); - if( _status == 0 ) { - mag[0] = (short)( _bytes[0] << 8 ) | (short)( _bytes[1] ); - mag[1] = (short)( _bytes[4] << 8 ) | (short)( _bytes[5] ); - mag[2] = (short)( _bytes[2] << 8 ) | (short)( _bytes[3] ); - } - } - return( mag ); -} - -// ------------------------------------------- -std::vector<float> LSM303::magneticField( void ) { - - float gainxy[] = { 1100., 855., 670., 450., 400., 330., 230. }; - float gainz[] = { 980., 760., 600., 400., 355., 295., 205. }; - - std::vector<float> mag( 3, 0 ); - int gn = ( this->magRegisterRead( MAG_CRB_REG ) >> 5 ) & 0x7; - std::vector<short> m = this->magRead(); - if( _status == 0 ) { - mag[0] = float( m[0] ) / gainxy[gn-1]; - mag[1] = float( m[1] ) / gainxy[gn-1]; - mag[2] = float( m[2] ) / gainz[gn-1]; - } - return( 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; + } } \ No newline at end of file