fork for acc
Dependents: mbed_accelerometer
Fork of LSM303DLHC by
Revision 3:4d9465e7e10e, committed 2013-02-20
- Comitter:
- bclaus
- Date:
- Wed Feb 20 23:52:53 2013 +0000
- Parent:
- 2:aea5caec809c
- Child:
- 4:612f7d5a822d
- Commit message:
- This provides some updates for dealing with LSM303DLHC; biggest changes are to addresses for this chip but I have stripped things I didn't need.
Changed in this revision
--- a/LSM303DLH.cpp Tue Apr 12 18:21:44 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,219 +0,0 @@ -/** 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 |= 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 */ - 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); - - // 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); - - // 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
--- a/LSM303DLH.h Tue Apr 12 18:21:44 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,138 +0,0 @@ -#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/LSM303DLHC.cpp Wed Feb 20 23:52:53 2013 +0000 @@ -0,0 +1,134 @@ +/** LSM303DLHC Interface Library + * + * base on code by Michael Shimniok http://bot-thoughts.com + * + * and on test program by @tosihisa and + * + * and on Pololu sample library for LSM303DLHC 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 "LSM303DLHC.h" + + +const int addr_acc = 0x32; +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 LSM303DLHC::write_reg(int addr_i2c,int addr_reg, char v) +{ + char data[2] = {addr_reg, v}; + return LSM303DLHC::_LSM303.write(addr_i2c, data, 2) == 0; +} + +bool LSM303DLHC::read_reg(int addr_i2c,int addr_reg, char *v) +{ + char data = addr_reg; + bool result = false; + + __disable_irq(); + if ((_LSM303.write(addr_i2c, &data, 1) == 0) && (_LSM303.read(addr_i2c, &data, 1) == 0)){ + *v = data; + result = true; + } + __enable_irq(); + return result; +} + + +LSM303DLHC::LSM303DLHC(PinName sda, PinName scl): + _LSM303(sda, scl) +{ + char reg_v; + _LSM303.frequency(100000); + + reg_v = 0; + + reg_v |= 0x27; /* X/Y/Z axis enable. */ + write_reg(addr_acc,CTRL_REG1_A,reg_v); + + reg_v = 0; + // reg_v |= 0x01 << 6; /* 1: data MSB @ lower address */ + reg_v = 0x01 << 4; /* +/- 4g */ + 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); +} + + +bool LSM303DLHC::read(float *ax, float *ay, float *az, float *mx, float *my, float *mz) { + char acc[6], mag[6]; + + if (recv(addr_acc, OUT_X_A, acc, 6) && recv(addr_mag, OUT_X_M, mag, 6)) { + *ax = float(short(acc[1] << 8 | acc[0]))/8192; //32768/4=8192 + *ay = float(short(acc[3] << 8 | acc[2]))/8192; + *az = float(short(acc[5] << 8 | acc[4]))/8192; + //full scale magnetic readings are from -2048 to 2047 + //gain is x,y =1100; z = 980 LSB/gauss + *mx = float(short(mag[0] << 8 | mag[1]))/1100; + *mz = float(short(mag[2] << 8 | mag[3]))/980; + *my = float(short(mag[4] << 8 | mag[5]))/1100; + + return true; + } + + return false; +} + + +bool LSM303DLHC::recv(char sad, char sub, char *buf, int length) { + if (length > 1) sub |= 0x80; + + return _LSM303.write(sad, &sub, 1, true) == 0 && _LSM303.read(sad, buf, length) == 0; +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LSM303DLHC.h Wed Feb 20 23:52:53 2013 +0000 @@ -0,0 +1,33 @@ +#include "mbed.h" + + + +class LSM303DLHC { + public: + /** Create a new interface for an LSM303DLHC + * + * @param sda is the pin for the I2C SDA line + * @param scl is the pin for the I2C SCL line + */ + LSM303DLHC(PinName sda, PinName scl); + + + /** read the raw accelerometer and compass values + * + * @param ax,ay,az is the accelerometer 3d vector, written by the function + * @param mx,my,mz is the magnetometer 3d vector, written by the function + */ + bool read(float *ax, float *ay, float *az, float *mx, float *my, float *mz); + + + private: + I2C _LSM303; + + + float ax, ay, az; + float mx, my, mz; + + bool write_reg(int addr_i2c,int addr_reg, char v); + bool read_reg(int addr_i2c,int addr_reg, char *v); + bool recv(char sad, char sub, char *buf, int length); +};
--- a/vector.cpp Tue Apr 12 18:21:44 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,22 +0,0 @@ -#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
--- a/vector.h Tue Apr 12 18:21:44 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,11 +0,0 @@ -#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