Toshihisa T
/
LSM303DLHTest
LSM303DLH Test Program for get angle.
Revision 3:f3796683b4c9, committed 2012-05-06
- Comitter:
- tosihisa
- Date:
- Sun May 06 05:14:30 2012 +0000
- Parent:
- 2:39cdefe3c9f2
- Child:
- 4:3c677edffb13
- Commit message:
- An angle of inclination is obtained using an accelerometer.
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LSM303DLH/LSM303DLH.cpp Sun May 06 05:14:30 2012 +0000 @@ -0,0 +1,227 @@ +/** LSM303DLH Interface Library + * + * Michael Shimniok http://bot-thoughts.com + * + * Based on test program by @tosihisa and + * + * Pololu sample library for LSM303DLH breakout by ryantm: + * + * Copyright (c) 2011 Pololu Corporation. For more information, see + * + * http://www.pololu.com/ + * http://forum.pololu.com/ + * + * 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 "mbed.h" +#include "LSM303DLH.h" + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +#define FILTER_SHIFT 6 // used in filtering acceleromter readings + +const int addr_acc = 0x30; +const int addr_mag = 0x3c; + +enum REG_ADDRS { + /* --- Mag --- */ + CRA_REG_M = 0x00, + CRB_REG_M = 0x01, + MR_REG_M = 0x02, + OUT_X_M = 0x03, + OUT_Y_M = 0x05, + OUT_Z_M = 0x07, + /* --- Acc --- */ + CTRL_REG1_A = 0x20, + CTRL_REG4_A = 0x23, + OUT_X_A = 0x28, + OUT_Y_A = 0x2A, + OUT_Z_A = 0x2C, +}; + +bool LSM303DLH::write_reg(int addr_i2c,int addr_reg, char v) +{ + char data[2] = {addr_reg, v}; + return LSM303DLH::_compass.write(addr_i2c, data, 2) == 0; +} + +bool LSM303DLH::read_reg(int addr_i2c,int addr_reg, char *v) +{ + char data = addr_reg; + bool result = false; + + __disable_irq(); + if ((_compass.write(addr_i2c, &data, 1) == 0) && (_compass.read(addr_i2c, &data, 1) == 0)){ + *v = data; + result = true; + } + __enable_irq(); + return result; +} + +bool LSM303DLH::read_reg_short(int addr_i2c,int addr_reg, short *v) +{ + char *pv = (char *)v; + bool result; + + result = read_reg(addr_i2c,addr_reg+0,pv+1); + result &= read_reg(addr_i2c,addr_reg+1,pv+0); + + return result; +} + +LSM303DLH::LSM303DLH(PinName sda, PinName scl): + _compass(sda, scl), _offset_x(0), _offset_y(0), _offset_z(0), _scale_x(0), _scale_y(0), _scale_z(0), _filt_ax(0), _filt_ay(0), _filt_az(6000) +{ + char reg_v; + _compass.frequency(100000); + + reg_v = 0; + reg_v |= 0x01 << 5; /* Normal mode */ + reg_v |= 0x03 << 3; /* Output data rate 1000 [Hz] */ + reg_v |= 0x07; /* X/Y/Z axis enable. */ + write_reg(addr_acc,CTRL_REG1_A,reg_v); + reg_v = 0; + read_reg(addr_acc,CTRL_REG1_A,®_v); + + reg_v = 0; + reg_v |= 0x01 << 6; /* 1: data MSB @ lower address */ + reg_v |= 0x01 << 4; /* +/- 4g */ + //reg_v |= 0x00 << 4; /* +/- 2g */ + write_reg(addr_acc,CTRL_REG4_A,reg_v); + + /* -- mag --- */ + reg_v = 0; + reg_v |= 0x04 << 2; /* Minimum data output rate = 15Hz */ + write_reg(addr_mag,CRA_REG_M,reg_v); + + reg_v = 0; + //reg_v |= 0x01 << 5; /* +-1.3Gauss */ + reg_v |= 0x07 << 5; /* +-8.1Gauss */ + write_reg(addr_mag,CRB_REG_M,reg_v); + + reg_v = 0; /* Continuous-conversion mode */ + write_reg(addr_mag,MR_REG_M,reg_v); +} + + +void LSM303DLH::setOffset(float x, float y, float z) +{ + _offset_x = x; + _offset_y = y; + _offset_z = z; +} + +void LSM303DLH::setScale(float x, float y, float z) +{ + _scale_x = x; + _scale_y = y; + _scale_z = z; +} + +void LSM303DLH::read(vector &a, vector &m) +{ + short a_x, a_y, a_z; + short m_x, m_y, m_z; + //Timer t; + //int usec1, usec2; + + //t.reset(); + //t.start(); + + //usec1 = t.read_us(); + read_reg_short(addr_acc, OUT_X_A, &a_x); + read_reg_short(addr_acc, OUT_Y_A, &a_y); + read_reg_short(addr_acc, OUT_Z_A, &a_z); + read_reg_short(addr_mag, OUT_X_M, &m_x); + read_reg_short(addr_mag, OUT_Y_M, &m_y); + read_reg_short(addr_mag, OUT_Z_M, &m_z); + //usec2 = t.read_us(); + + //if (debug) debug->printf("%d %d %d\n", usec1, usec2, usec2-usec1); + +#if 0 + // Perform simple lowpass filtering + // Intended to stabilize heading despite + // device vibration such as on a UGV + _filt_ax += a_x - (_filt_ax >> FILTER_SHIFT); + _filt_ay += a_y - (_filt_ay >> FILTER_SHIFT); + _filt_az += a_z - (_filt_az >> FILTER_SHIFT); + + a.x = (float) (_filt_ax >> FILTER_SHIFT); + a.y = (float) (_filt_ay >> FILTER_SHIFT); + a.z = (float) (_filt_az >> FILTER_SHIFT); +#else + a.x = (float)a_x; + a.y = (float)a_y; + a.z = (float)a_z; +#endif + + // offset and scale + m.x = (m_x + _offset_x) * _scale_x; + m.y = (m_y + _offset_y) * _scale_y; + m.z = (m_z + _offset_z) * _scale_z; +} + + +// Returns the number of degrees from the -Y axis that it +// is pointing. +float LSM303DLH::heading() +{ + return heading((vector){0,-1,0}); +} + +float LSM303DLH::heading(vector from) +{ + vector a, m; + + this->read(a, m); + + //////////////////////////////////////////////// + // compute heading + //////////////////////////////////////////////// + + vector temp_a = a; + // normalize + vector_normalize(&temp_a); + //vector_normalize(&m); + + // compute E and N + vector E; + vector N; + vector_cross(&m,&temp_a,&E); + vector_normalize(&E); + vector_cross(&temp_a,&E,&N); + + // compute heading + float heading = atan2(vector_dot(&E,&from), vector_dot(&N,&from)) * 180/M_PI; + if (heading < 0) heading += 360; + + return heading; +} + +void LSM303DLH::frequency(int hz) +{ + _compass.frequency(hz); +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LSM303DLH/LSM303DLH.h Sun May 06 05:14:30 2012 +0000 @@ -0,0 +1,138 @@ +#include "mbed.h" +#include "vector.h" + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +/** Tilt-compensated compass interface Library for the STMicro LSM303DLH 3-axis magnetometer, 3-axis acceleromter + * + * Michael Shimniok http://bot-thoughts.com + * + * Based on test program by @tosihisa and + * + * Pololu sample library for LSM303DLH breakout by ryantm: + * + * Copyright (c) 2011 Pololu Corporation. For more information, see + * + * http://www.pololu.com/ + * http://forum.pololu.com/ + * + * 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. + * + * @code + * #include "mbed.h" + * #include "LSM303DLH.h" + * + * Serial debug(USBTX,USBRX); + * LSM303DLH compass(p28, p27); + * + * int main() { + * float hdg; + * debug.format(8,Serial::None,1); + * debug.baud(115200); + * debug.printf("LSM303DLH Test\x0d\x0a"); + * compass.setOffset(29.50, -0.50, 4.00); // example calibration + * compass.setScale(1.00, 1.03, 1.21); // example calibration + * while(1) { + * hdg = compass.heading(); + * debug.printf("Heading: %.2f\n", hdg); + * wait(0.1); + * } + * } + * @endcode + */ +class LSM303DLH { + public: + /** Create a new interface for an LSM303DLH + * + * @param sda is the pin for the I2C SDA line + * @param scl is the pin for the I2C SCL line + */ + LSM303DLH(PinName sda, PinName scl); + + /** sets the x, y, and z offset corrections for hard iron calibration + * + * Calibration details here: + * http://mbed.org/users/shimniok/notebook/quick-and-dirty-3d-compass-calibration/ + * + * If you gather raw magnetometer data and find, for example, x is offset + * by hard iron by -20 then pass +20 to this member function to correct + * for hard iron. + * + * @param x is the offset correction for the x axis + * @param y is the offset correction for the y axis + * @param z is the offset correction for the z axis + */ + void setOffset(float x, float y, float z); + + /** sets the scale factor for the x, y, and z axes + * + * Calibratio details here: + * http://mbed.org/users/shimniok/notebook/quick-and-dirty-3d-compass-calibration/ + * + * Sensitivity of the three axes is never perfectly identical and this + * function can help to correct differences in sensitivity. You're + * supplying a multipler such that x, y and z will be normalized to the + * same max/min values + */ + void setScale(float x, float y, float z); + + /** read the raw accelerometer and compass values + * + * @param a is the accelerometer 3d vector, written by the function + * @param m is the magnetometer 3d vector, written by the function + */ + void read(vector &a, vector &m); + + /** returns the magnetic heading with respect to the y axis + * + */ + float heading(void); + + /** returns the heading with respect to the specified vector + * + */ + float heading(vector from); + + /** sets the I2C bus frequency + * + * @param frequency is the I2C bus/clock frequency, either standard (100000) or fast (400000) + */ + void frequency(int hz); + + private: + I2C _compass; + float _offset_x; + float _offset_y; + float _offset_z; + float _scale_x; + float _scale_y; + float _scale_z; + long _filt_ax; + long _filt_ay; + long _filt_az; + + bool write_reg(int addr_i2c,int addr_reg, char v); + bool read_reg(int addr_i2c,int addr_reg, char *v); + bool read_reg_short(int addr_i2c,int addr_reg, short *v); +};
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LSM303DLH/vector.cpp Sun May 06 05:14:30 2012 +0000 @@ -0,0 +1,22 @@ +#include <vector.h> +#include <math.h> + +void vector_cross(const vector *a,const vector *b, vector *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 vector_dot(const vector *a,const vector *b) +{ + return a->x*b->x+a->y*b->y+a->z*b->z; +} + +void vector_normalize(vector *a) +{ + float mag = sqrt(vector_dot(a,a)); + a->x /= mag; + a->y /= mag; + a->z /= mag; +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LSM303DLH/vector.h Sun May 06 05:14:30 2012 +0000 @@ -0,0 +1,11 @@ +#ifndef vector_h +#define vector_h +typedef struct vector +{ + float x, y, z; +} vector; + +extern void vector_cross(const vector *a, const vector *b, vector *out); +extern float vector_dot(const vector *a,const vector *b); +extern void vector_normalize(vector *a); +#endif \ No newline at end of file
--- a/main.cpp Tue Nov 30 14:42:07 2010 +0000 +++ b/main.cpp Sun May 06 05:14:30 2012 +0000 @@ -1,122 +1,26 @@ - -/* - * LSM303DLH Test program - * @tosihisa - */ #include "mbed.h" -#ifndef M_PI -#define M_PI 3.14159265358979323846 -#endif - -Serial debug(USBTX,USBRX); - -I2C LSM303DLH(p28, p27); - -const int addr_acc = 0x30; -const int addr_mag = 0x3c; - -enum REG_ADDRS { - /* --- Mag --- */ - CRA_REG_M = 0x00, - CRB_REG_M = 0x01, - MR_REG_M = 0x02, - OUT_X_M = 0x03, - OUT_Y_M = 0x05, - OUT_Z_M = 0x07, - /* --- Acc --- */ - CTRL_REG1_A = 0x20, - CTRL_REG4_A = 0x23, - OUT_X_A = 0x28, - OUT_Y_A = 0x2A, - OUT_Z_A = 0x2C, -}; + #include "LSM303DLH.h" -bool write_reg(int addr_i2c,int addr_reg, char v) -{ - char data[2] = {addr_reg, v}; - return LSM303DLH.write(addr_i2c, data, 2) == 0; -} - -bool read_reg(int addr_i2c,int addr_reg, char *v) -{ - char data = addr_reg; - if ((LSM303DLH.write(addr_i2c, &data, 1) == 0) && (LSM303DLH.read(addr_i2c, &data, 1) == 0)){ - *v = data; - return true; - } - return false; -} - -bool read_reg_short(int addr_i2c,int addr_reg, short *v) -{ - char *pv = (char *)v; - read_reg(addr_i2c,addr_reg+0,pv+1); - return read_reg(addr_i2c,addr_reg+1,pv+0); -} - -int main() { - char reg_v; - bool result; - short a_x,a_y,a_z; - short m_x,m_y,m_z; - double m_deg; + Serial debug(USBTX,USBRX); + LSM303DLH compass(p28, p27); - debug.format(8,Serial::None,1); - debug.baud(115200); - debug.printf("LSM303DLH Test\x0d\x0a"); - - /* --- acc --- */ - reg_v = 0; - result = read_reg(addr_acc,CTRL_REG1_A,®_v); - debug.printf("(%s) CTRL_REG1_A=[0x%02X]\x0d\x0a",(result) ? "OK" : "NG",reg_v); - - reg_v = 0; - result = read_reg(addr_mag,CRA_REG_M,®_v); - debug.printf("(%s) CRA_REG_M=[0x%02X]\x0d\x0a",(result) ? "OK" : "NG",reg_v); - - reg_v = 0; - reg_v |= 0x01 << 5; /* Normal mode */ - reg_v |= 0x07; /* X/Y/Z axis enable. */ - result = write_reg(addr_acc,CTRL_REG1_A,reg_v); - reg_v = 0; - result = read_reg(addr_acc,CTRL_REG1_A,®_v); - debug.printf("(%s) CTRL_REG1_A=[0x%02X]\x0d\x0a",(result) ? "OK" : "NG",reg_v); - - reg_v = 0; - reg_v |= 0x01 << 6; /* 1: data MSB @ lower address */ - reg_v |= 0x01 << 4; /* +/- 4g */ - result = write_reg(addr_acc,CTRL_REG4_A,reg_v); - - /* -- mag --- */ - reg_v = 0; - reg_v |= 0x04 << 2; /* Minimum data output rate = 15Hz */ - result = write_reg(addr_mag,CRA_REG_M,reg_v); - - reg_v = 0; - //reg_v |= 0x01 << 5; /* +-1.3Gauss */ - reg_v |= 0x07 << 5; /* +-8.1Gauss */ - result = write_reg(addr_mag,CRB_REG_M,reg_v); - - reg_v = 0; /* Continuous-conversion mode */ - result = write_reg(addr_mag,MR_REG_M,reg_v); - - while(1) { - read_reg_short(addr_acc,OUT_X_A,&a_x); - read_reg_short(addr_acc,OUT_Y_A,&a_y); - read_reg_short(addr_acc,OUT_Z_A,&a_z); - read_reg_short(addr_mag,OUT_X_M,&m_x); - read_reg_short(addr_mag,OUT_Y_M,&m_y); - read_reg_short(addr_mag,OUT_Z_M,&m_z); - m_deg = (atan2((double)(m_x),(double)(m_y)) * 180) / M_PI; - if(m_deg < 0){ - m_deg = m_deg * -1.0; - } else { - m_deg = 360.0 - m_deg; - } - - debug.printf("ACC:X=%-7d,Y=%-7d,Z=%-7d ",a_x,a_y,a_z); - debug.printf("MAG:X=%-7d,Y=%-7d,Z=%-7d DEG=%f\x0d\x0a",m_x,m_y,m_z,m_deg); - wait(0.1); - } -} + int main() { + float hdg; + float hdgV; + vector acc; + vector mag; + debug.format(8,Serial::None,1); + debug.baud(115200); + debug.printf("LSM303DLH Test\x0d\x0a"); + compass.setOffset(0.00,0.00,0.00); // example calibration + compass.setScale(1.00,1.00,1.00); // example calibration + while(1) { + compass.read(acc,mag); + hdg = compass.heading(); + hdgV = atan2(acc.y,acc.z) * 180/M_PI; + debug.printf("ACC: %6.2f %6.2f %6.2f Heading: %6.2f %6.2f\n",acc.x,acc.y,acc.z,hdgV,hdg); + wait(0.1); + } + } + \ No newline at end of file
--- a/mbed.bld Tue Nov 30 14:42:07 2010 +0000 +++ b/mbed.bld Sun May 06 05:14:30 2012 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/users/mbed_official/code/mbed/builds/e2ac27c8e93e +http://mbed.org/users/mbed_official/code/mbed/builds/737756e0b479