Diff: LSM303DLH.cpp

Revision:

2:aea5caec809c

Parent:

1:48d83c63d1d9

Child:

3:9b4ff901b5c9

--- a/LSM303DLH.cpp	Fri Apr 08 07:29:51 2011 +0000
+++ b/LSM303DLH.cpp	Tue Apr 12 18:21:44 2011 +0000
@@ -69,18 +69,26 @@
 bool LSM303DLH::read_reg(int addr_i2c,int addr_reg, char *v)
 {
     char data = addr_reg; 
-    if ((LSM303DLH::_compass.write(addr_i2c, &data, 1) == 0) && (LSM303DLH::_compass.read(addr_i2c, &data, 1) == 0)){
+    bool result = false;
+    
+    __disable_irq();
+    if ((_compass.write(addr_i2c, &data, 1) == 0) && (_compass.read(addr_i2c, &data, 1) == 0)){
         *v = data;
-        return true;
+        result = true;
     }
-    return false;
+    __enable_irq();
+    return result;
 }
 
 bool LSM303DLH::read_reg_short(int addr_i2c,int addr_reg, short *v)
 {
     char *pv = (char *)v;
-    read_reg(addr_i2c,addr_reg+0,pv+1);
-    return read_reg(addr_i2c,addr_reg+1,pv+0);
+    bool result;
+    
+    result =  read_reg(addr_i2c,addr_reg+0,pv+1);
+    result &= read_reg(addr_i2c,addr_reg+1,pv+0);
+  
+    return result;
 }
 
 LSM303DLH::LSM303DLH(PinName sda, PinName scl):
@@ -134,13 +142,22 @@
 {
     short a_x, a_y, a_z;
     short m_x, m_y, m_z;
+    //Timer t;
+    //int usec1, usec2;
     
+    //t.reset();
+    //t.start();
+
+    //usec1 = t.read_us();
     read_reg_short(addr_acc, OUT_X_A, &a_x);
     read_reg_short(addr_acc, OUT_Y_A, &a_y);
     read_reg_short(addr_acc, OUT_Z_A, &a_z);
     read_reg_short(addr_mag, OUT_X_M, &m_x);
     read_reg_short(addr_mag, OUT_Y_M, &m_y);
     read_reg_short(addr_mag, OUT_Z_M, &m_z);
+    //usec2 = t.read_us();
+    
+    //if (debug) debug->printf("%d %d %d\n", usec1, usec2, usec2-usec1);
 
     // Perform simple lowpass filtering
     // Intended to stabilize heading despite
@@ -195,3 +212,8 @@
     
     return heading;
 }
+
+void LSM303DLH::frequency(int hz)
+{
+    _compass.frequency(hz);
+}
\ No newline at end of file