Diff: main.cpp

Revision:

1:14b97953bd2f

Parent:

0:750784997b84

Child:

2:39cdefe3c9f2

--- a/main.cpp	Sun Nov 28 13:29:02 2010 +0000
+++ b/main.cpp	Sun Nov 28 14:38:44 2010 +0000
@@ -47,11 +47,16 @@
     return false;
 }
 
-bool read_reg_short(int addr_i2c,int addr_reg, short *v)
+bool read_reg_short(int addr_i2c,int addr_reg, short *v,int hi)
 {
     char *pv = (char *)v;
-    read_reg(addr_i2c,addr_reg+0,pv+0);
-    return read_reg(addr_i2c,addr_reg+1,pv+1);
+    if(hi){
+        read_reg(addr_i2c,addr_reg+0,pv+1);
+        return read_reg(addr_i2c,addr_reg+1,pv+0);
+    } else {
+        read_reg(addr_i2c,addr_reg+0,pv+0);
+        return read_reg(addr_i2c,addr_reg+1,pv+1);
+    }
 }
 
 int main() {
@@ -93,15 +98,17 @@
     result = write_reg(addr_mag,MR_REG_M,reg_v);
 
     while(1) {
-        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);
-        m_deg = (atan2((double)m_x,(double)m_y) * 180) / M_PI;
+        read_reg_short(addr_acc,OUT_X_A,&a_x,0);
+        read_reg_short(addr_acc,OUT_Y_A,&a_y,0);
+        read_reg_short(addr_acc,OUT_Z_A,&a_z,0);
+        read_reg_short(addr_mag,OUT_X_M,&m_x,1);
+        read_reg_short(addr_mag,OUT_Y_M,&m_y,1);
+        read_reg_short(addr_mag,OUT_Z_M,&m_z,1);
+        m_deg = (atan2((double)(m_x),(double)(m_y)) * 180) / M_PI;
         if(m_deg < 0){
-            m_deg += 360.0;
+            m_deg = m_deg * -1.0;
+        } else {
+            m_deg = 360.0 - m_deg;
         }
 
         debug.printf("ACC:X=%-7d,Y=%-7d,Z=%-7d ",a_x,a_y,a_z);