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

--- 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);