Diff: LSM303DLH.cpp

Revision:

1:48d83c63d1d9

Parent:

0:de767f4959ef

Child:

2:aea5caec809c

diff -r de767f4959ef -r 48d83c63d1d9 LSM303DLH.cpp
--- 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;