Interface library for STMicro LSM303DLH 3-axis magnetometer w/ 3-axis acceleromter. Computes magnetic heading.
Revision 1:48d83c63d1d9, committed 2011-04-08
- Comitter:
- shimniok
- Date:
- Fri Apr 08 07:29:51 2011 +0000
- Parent:
- 0:de767f4959ef
- Child:
- 2:aea5caec809c
- Commit message:
- Added accelerometer filtering in an attempt to reduce vibration effects on heading.
Changed in this revision
LSM303DLH.cpp | Show annotated file Show diff for this revision Revisions of this file |
LSM303DLH.h | Show annotated file Show diff for this revision Revisions of this file |
--- a/LSM303DLH.cpp Wed Apr 06 05:05:10 2011 +0000 +++ b/LSM303DLH.cpp Fri Apr 08 07:29:51 2011 +0000 @@ -39,6 +39,8 @@ #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; @@ -82,7 +84,7 @@ } 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) + _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); @@ -140,9 +142,16 @@ read_reg_short(addr_mag, OUT_Y_M, &m_y); read_reg_short(addr_mag, OUT_Z_M, &m_z); - a.x = (float) a_x; - a.y = (float) a_y; - a.z = (float) a_z; + // 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;
--- a/LSM303DLH.h Wed Apr 06 05:05:10 2011 +0000 +++ b/LSM303DLH.h Fri Apr 08 07:29:51 2011 +0000 @@ -1,4 +1,11 @@ -/** LSM303DLH Interface Library +#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 * @@ -52,14 +59,8 @@ * wait(0.1); * } * } + * @endcode */ -#include "mbed.h" -#include "vector.h" - -#ifndef M_PI -#define M_PI 3.14159265358979323846 -#endif - class LSM303DLH { public: @@ -122,7 +123,10 @@ 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);