Diff: LSM303D.cpp

Revision:

2:fcd607760ee8

Parent:

1:27d47f5de82c

--- a/LSM303D.cpp	Thu Dec 01 08:14:40 2016 +0000
+++ b/LSM303D.cpp	Thu Jan 18 07:52:30 2018 +0000
@@ -38,13 +38,14 @@
 #include "LSM303D.h"
 
 
-const int addr_acc_mag = 0x3C;
 
 enum REG_ADDRS {
+    
     /* --- Mag --- */
     OUT_X_M     = 0x08,
     OUT_Y_M     = 0x0A,
     OUT_Z_M     = 0x0C,
+    INT_CTRL_M  = 0x12,
     /* --- Acc --- */
     OUT_X_A     = 0x28,
     OUT_Y_A     = 0x2A,
@@ -63,7 +64,7 @@
 bool LSM303D::write_reg(int addr_i2c,int addr_reg, char v)
 {
     char data[2] = {addr_reg, v}; 
-    return LSM303D::_LSM303.write(addr_i2c, data, 2) == 0;
+    return LSM303D::_LSM303->write(addr_i2c, data, 2) == 0;
 }
 
 bool LSM303D::read_reg(int addr_i2c,int addr_reg, char *v)
@@ -71,57 +72,54 @@
     char data = addr_reg; 
     bool result = false;
     
-    __disable_irq();
-    if ((_LSM303.write(addr_i2c, &data, 1) == 0) && (_LSM303.read(addr_i2c, &data, 1) == 0)){
+    //__disable_irq();
+    if ((_LSM303->write(addr_i2c, &data, 1) == 0) && (_LSM303->read(addr_i2c, &data, 1) == 0)){
         *v = data;
         result = true;
     }
-    __enable_irq();
+    //__enable_irq();
     return result;
 }
 
-LSM303D::LSM303D(PinName sda, PinName scl):
-    _LSM303(sda, scl)
+LSM303D::LSM303D(I2C * puntero):
+    _LSM303(puntero)
 {
     char reg_v;
-    _LSM303.frequency(200000);
-        
-    reg_v = 0;
+    
+    reg_v = 0x0;          /* 400 Hz X/Y/Z axis enable. */
+    write_reg(addr_acc_mag,INT_CTRL_M,reg_v);
     
     //acc
-    reg_v |= 0x67;          /* 100 Hz X/Y/Z axis enable. */
+    reg_v = 0x87;          /* 400 Hz X/Y/Z axis enable. */
     write_reg(addr_acc_mag,CTRL1,reg_v);
+    reg_v = 0x04;          /* Interrupt Data Redy */
+    write_reg(addr_acc_mag,CTRL3,reg_v);
     
-    read_reg(addr_acc_mag,CTRL2,&reg_v);
-    reg_v &= ~0x38;
-    reg_v |= 0x00; //+-2g range          
+    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)
+    reg_v = 0x80;     //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
+    reg_v = 0x04;   //T_only Power Down Magnetometer
     write_reg(addr_acc_mag,CTRL7,reg_v);
+    
 }
 
 
-bool LSM303D::read(float *ax, float *ay, float *az, float *mx, float *my, float *mz) {
+bool LSM303D::read(double *ax, double *ay, double *az, double *mx, double *my, double *mz) {
     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]))*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;
+        *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;
         //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;
-        *mz = float(short(mag[2] << 8 | mag[3]))/980;
-        *my = float(short(mag[4] << 8 | mag[5]))/1100;
+        *mx = double(short(mag[0] << 8 | mag[1]))/1100;
+        *mz = double(short(mag[2] << 8 | mag[3]))/980;
+        *my = double(short(mag[4] << 8 | mag[5]))/1100;
  
         return true;
     }
@@ -135,7 +133,7 @@
     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;
+        *az =  double(short(acc[5] << 8 | acc[4]))*0.000061; 
 
         return true;
     }
@@ -145,5 +143,5 @@
 bool LSM303D::recv(char sad, char sub, char *buf, int length) {
     if (length > 1) sub |= 0x80;
  
-    return _LSM303.write(sad, &sub, 1, true) == 0 && _LSM303.read(sad, buf, length) == 0;
+    return _LSM303->write(sad, &sub, 1, true) == 0 && _LSM303->read(sad, buf, length) == 0;
 }