Kris Reynolds
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