Diff: LSM303D.cpp

Revision:

1:27d47f5de82c

Parent:

0:5b930e09bd6e

Child:

2:fcd607760ee8

--- a/LSM303D.cpp	Wed Feb 03 20:57:01 2016 +0000
+++ b/LSM303D.cpp	Thu Dec 01 08:14:40 2016 +0000
@@ -38,7 +38,7 @@
 #include "LSM303D.h"
 
 
-const int addr_acc_mag = 0x3A;
+const int addr_acc_mag = 0x3C;
 
 enum REG_ADDRS {
     /* --- Mag --- */
@@ -50,14 +50,14 @@
     OUT_Y_A     = 0x2A,
     OUT_Z_A     = 0x2C,
     //
-    CTRL0       = 0x1F;
-    CTRL1       = 0x20;
-    CTRL2       = 0x21;
-    CTRL3       = 0x22;
-    CTRL4       = 0x23;
-    CTRL5       = 0x24;
-    CTRL6       = 0x25;
-    CTRL7       = 0x26;
+    CTRL0       = 0x1F,
+    CTRL1       = 0x20,
+    CTRL2       = 0x21,
+    CTRL3       = 0x22,
+    CTRL4       = 0x23,
+    CTRL5       = 0x24,
+    CTRL6       = 0x25,
+    CTRL7       = 0x26
 };
 
 bool LSM303D::write_reg(int addr_i2c,int addr_reg, char v)
@@ -89,19 +89,24 @@
     reg_v = 0;
     
     //acc
-    reg_v |= 0x57;          /* 50 Hz X/Y/Z axis enable. */
+    reg_v |= 0x67;          /* 100 Hz X/Y/Z axis enable. */
     write_reg(addr_acc_mag,CTRL1,reg_v);
-
+    
+    read_reg(addr_acc_mag,CTRL2,&reg_v);
+    reg_v &= ~0x38;
+    reg_v |= 0x00; //+-2g range          
+    write_reg(addr_acc_mag,CTRL2,reg_v);
+    
     /* -- mag --- */
     reg_v = 0;
     reg_v |= 0x78;     //M_RES = 11 (high resolution mode); M_ODR = 100 (50 Hz ODR)
     write_reg(addr_acc_mag,CTRL5,reg_v);
 
- 
+
 
     reg_v = 0;    
     reg_v |= 0x00;   //continuous mag
-    write_reg(addr_mag,CTRL7,reg_v);
+    write_reg(addr_acc_mag,CTRL7,reg_v);
 }
 
 
@@ -109,9 +114,9 @@
     char acc[6], mag[6];
  
     if (recv(addr_acc_mag, OUT_X_A, acc, 6) && recv(addr_acc_mag, OUT_X_M, mag, 6)) {
-        *ax = float(short(acc[1] << 8 | acc[0]))/8192;  //32768/4=8192
-        *ay =  float(short(acc[3] << 8 | acc[2]))/8192;
-        *az =  float(short(acc[5] << 8 | acc[4]))/8192;
+        *ax = float(short(acc[1] << 8 | acc[0]))*0.000061;  //32768/4=8192
+        *ay =  float(short(acc[3] << 8 | acc[2]))*0.000061;
+        *az =  float(short(acc[5] << 8 | acc[4]))*0.000061;
         //full scale magnetic readings are from -2048 to 2047
         //gain is x,y =1100; z = 980 LSB/gauss
         *mx = float(short(mag[0] << 8 | mag[1]))/1100;
@@ -124,7 +129,19 @@
     return false;
 }
 
+bool LSM303D::readA(double *ax, double *ay, double *az) {
+    char acc[6];
+ 
+    if (recv(addr_acc_mag, OUT_X_A, acc, 6)) {
+        *ax = double(short(acc[1] << 8 | acc[0]))*0.000061;  //32768/4=8192
+        *ay =  double(short(acc[3] << 8 | acc[2]))*0.000061;
+        *az =  double(short(acc[5] << 8 | acc[4]))*0.000061;
 
+        return true;
+    }
+ 
+    return false;
+}
 bool LSM303D::recv(char sad, char sub, char *buf, int length) {
     if (length > 1) sub |= 0x80;