Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependents: minimu-9v2 air_zuma Fish_2014Fall DiscoveryF3-LSM303_Compass ... more
Fork of
LSM303DLH
by 
Michael Shimniok
Diff: LSM303DLH.cpp
- Revision:
- 1:48d83c63d1d9
- Parent:
- 0:de767f4959ef
- Child:
- 2:aea5caec809c
--- 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;