Diff: main.cpp

Revision:

2:39cdefe3c9f2

Parent:

1:14b97953bd2f

Child:

3:f3796683b4c9

--- a/main.cpp	Sun Nov 28 14:38:44 2010 +0000
+++ b/main.cpp	Tue Nov 30 14:42:07 2010 +0000
@@ -26,6 +26,7 @@
     OUT_Z_M     = 0x07,
     /* --- Acc --- */
     CTRL_REG1_A = 0x20,
+    CTRL_REG4_A = 0x23,
     OUT_X_A     = 0x28,
     OUT_Y_A     = 0x2A,
     OUT_Z_A     = 0x2C,
@@ -47,16 +48,11 @@
     return false;
 }
 
-bool read_reg_short(int addr_i2c,int addr_reg, short *v,int hi)
+bool read_reg_short(int addr_i2c,int addr_reg, short *v)
 {
     char *pv = (char *)v;
-    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);
-    }
+    read_reg(addr_i2c,addr_reg+0,pv+1);
+    return read_reg(addr_i2c,addr_reg+1,pv+0);
 }
 
 int main() {
@@ -70,6 +66,7 @@
     debug.baud(115200);
     debug.printf("LSM303DLH Test\x0d\x0a");
 
+    /* --- acc --- */
     reg_v = 0;
     result = read_reg(addr_acc,CTRL_REG1_A,&reg_v);
     debug.printf("(%s) CTRL_REG1_A=[0x%02X]\x0d\x0a",(result) ? "OK" : "NG",reg_v);
@@ -87,23 +84,30 @@
     debug.printf("(%s) CTRL_REG1_A=[0x%02X]\x0d\x0a",(result) ? "OK" : "NG",reg_v);
 
     reg_v = 0;
+    reg_v |= 0x01 << 6;     /* 1: data MSB @ lower address */
+    reg_v |= 0x01 << 4;     /* +/- 4g */
+    result = write_reg(addr_acc,CTRL_REG4_A,reg_v);
+
+    /* -- mag --- */
+    reg_v = 0;
     reg_v |= 0x04 << 2;     /* Minimum data output rate = 15Hz */
     result = write_reg(addr_mag,CRA_REG_M,reg_v);
 
     reg_v = 0;
-    reg_v |= 0x01 << 5;     /* +-1.3Gauss */
+    //reg_v |= 0x01 << 5;     /* +-1.3Gauss */
+    reg_v |= 0x07 << 5;     /* +-8.1Gauss */
     result = write_reg(addr_mag,CRB_REG_M,reg_v);
 
     reg_v = 0;              /* Continuous-conversion mode */
     result = write_reg(addr_mag,MR_REG_M,reg_v);
 
     while(1) {
-        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);
+        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;
         if(m_deg < 0){
             m_deg = m_deg * -1.0;