Gal Yaroslavsky / LSM303DLHC

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.

Fork of LSM303DLHC by brian claus

Files at this revision

API Documentation at this revision

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