Diff: LSM303DLH.cpp

Revision:

3:9b4ff901b5c9

Parent:

2:aea5caec809c

Child:

4:4f2ed3f8726c

diff -r aea5caec809c -r 9b4ff901b5c9 LSM303DLH.cpp
--- a/LSM303DLH.cpp	Tue Apr 12 18:21:44 2011 +0000
+++ b/LSM303DLH.cpp	Sun Aug 06 00:43:57 2017 +0000
@@ -41,7 +41,7 @@
 
 #define FILTER_SHIFT 6      // used in filtering acceleromter readings
 
-const int addr_acc = 0x30;
+const int addr_acc = 0x32;//0x30;
 const int addr_mag = 0x3c;
 
 enum REG_ADDRS {
@@ -55,6 +55,7 @@
     /* --- Acc --- */
     CTRL_REG1_A = 0x20,
     CTRL_REG4_A = 0x23,
+    STATUS_REG_A= 0x27,
     OUT_X_A     = 0x28,
     OUT_Y_A     = 0x2A,
     OUT_Z_A     = 0x2C,
@@ -62,8 +63,14 @@
 
 bool LSM303DLH::write_reg(int addr_i2c,int addr_reg, char v)
 {
-    char data[2] = {addr_reg, v}; 
-    return LSM303DLH::_compass.write(addr_i2c, data, 2) == 0;
+    bool result=false;
+    char data[2] = {addr_reg, v};
+    //__disable_irq();
+    result = m_ptr_I2C->write(addr_i2c, data, 2) == 0;
+    if(result == false) debug("Unable to Write \n");
+    
+    //__enable_irq(); 
+    return result;
 }
 
 bool LSM303DLH::read_reg(int addr_i2c,int addr_reg, char *v)
@@ -71,32 +78,72 @@
     char data = addr_reg; 
     bool result = false;
     
-    __disable_irq();
-    if ((_compass.write(addr_i2c, &data, 1) == 0) && (_compass.read(addr_i2c, &data, 1) == 0)){
-        *v = data;
-        result = true;
+    //__disable_irq();
+    if(m_ptr_I2C->write(addr_i2c, &data, 1) == 0)
+    {        
+        if (m_ptr_I2C->read(addr_i2c, &data, 1) == 0)
+        {            
+            *v = data;
+            result = true;
+        }
+        else
+        {
+            debug("Unable to Read \n");
+        }
     }
-    __enable_irq();
+    else
+    {
+        debug("Unable to Write \n");
+    }
+    //__enable_irq();
     return result;
 }
 
 bool LSM303DLH::read_reg_short(int addr_i2c,int addr_reg, short *v)
 {
+    
     char *pv = (char *)v;
     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):
-    _compass(sda, scl), _offset_x(0), _offset_y(0), _offset_z(0), _scale_x(0), _scale_y(0), _scale_z(0), _filt_ax(0), _filt_ay(0), _filt_az(6000)
+LSM303DLH::LSM303DLH(PinName sda, PinName scl)     
+{
+   m_ptr_I2C = new I2C(sda, scl);
+   init(); 
+}
+LSM303DLH::LSM303DLH(I2C* ptrI2C)
+{
+    m_ptr_I2C = ptrI2C;
+    //debug( "ptr pass %d, ptr here %d   \n",(int)ptrI2C,(int)m_ptr_I2C);
+    this->init();
+}
+
+void LSM303DLH::init(void)
 {
     char reg_v;
-    _compass.frequency(100000);
-        
+ 
+    debug("In INIT \n");
+    _offset_x = 0; 
+    _offset_y = 0;
+    _offset_z = 0; 
+    _scale_x  = 0; 
+    _scale_y  = 0; 
+    _scale_z  = 0; 
+    _filt_ax  = 0; 
+    _filt_ay  = 0; 
+    _filt_az  = 6000;
+ 
+ 
+    m_ptr_I2C->frequency(100000);
+    
+       
+    debug("in ACC \n");
     reg_v = 0;
     reg_v |= 0x01 << 5;     /* Normal mode  */
     reg_v |= 0x07;          /* X/Y/Z axis enable. */
@@ -105,11 +152,14 @@
     read_reg(addr_acc,CTRL_REG1_A,&reg_v);
 
     reg_v = 0;
-    reg_v |= 0x01 << 6;     /* 1: data MSB @ lower address */
+    reg_v |= 0x01 << 7;     //full read
+    reg_v |= 0x01 << 3;     //hi res
+    //reg_v |= 0x01 << 6;     /* 1: data MSB @ lower address */
     reg_v |= 0x01 << 4;     /* +/- 4g */
     write_reg(addr_acc,CTRL_REG4_A,reg_v);
 
     /* -- mag --- */
+    debug("in MAG \n");
     reg_v = 0;
     reg_v |= 0x04 << 2;     /* Minimum data output rate = 15Hz */
     write_reg(addr_mag,CRA_REG_M,reg_v);
@@ -121,9 +171,11 @@
 
     reg_v = 0;              /* Continuous-conversion mode */
     write_reg(addr_mag,MR_REG_M,reg_v);
+    
+    debug("Out INIT \n");
+    clearscreen();
 }
 
-
 void LSM303DLH::setOffset(float x, float y, float z)
 {
     _offset_x = x;
@@ -137,43 +189,149 @@
     _scale_y = y;
     _scale_z = z;
 }
-
-void LSM303DLH::read(vector &a, vector &m)
+#define _FS 4
+bool LSM303DLH::read(vector &a, vector &m)
 {
+    
+    bool result = true;
     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 defined(CHECK_TIME_SEQUENCE)
+        Timer t;
+        int usec1, usec2;
+        
+        t.reset();
+        t.start();
     
-    //if (debug) debug->printf("%d %d %d\n", usec1, usec2, usec2-usec1);
-
-    // Perform simple lowpass filtering
-    // Intended to stabilize heading despite
-    // device vibration such as on a UGV
-    _filt_ax += a_x - (_filt_ax >> FILTER_SHIFT);
-    _filt_ay += a_y - (_filt_ay >> FILTER_SHIFT);
-    _filt_az += a_z - (_filt_az >> FILTER_SHIFT);
-
-    a.x = (float) (_filt_ax >> FILTER_SHIFT);
-    a.y = (float) (_filt_ay >> FILTER_SHIFT);
-    a.z = (float) (_filt_az >> FILTER_SHIFT);
+        usec1 = t.read_us();
+    #endif
+   
+   union{
+        int16_t number;
+        char byte[2];
+        struct{
+            char MSB;
+            char LSB;
+            };
+        }my_test;
+        vector a_test;
+        
+        char data_read_acc =0;
+        read_reg(addr_acc,STATUS_REG_A,&data_read_acc);
+        if(data_read_acc & (1<<3))//new data
+        {
+            result &= read_reg(addr_acc,OUT_X_A  ,&(my_test.MSB));//MSB at lower
+            if(result)
+            {
+                result &= read_reg(addr_acc,OUT_X_A+1,&(my_test.LSB));
+            }
+            if(result)
+            {
+                //a_test.x = my_test.number;
+                a_test.x = (my_test.number / (float)(32768 /*half of the ADC resolution*/ / _FS/*+- 4g*/));
+                //debug("read from reg _x:(%d) %.2X %.2X \n",my_test.number, my_test.byte[1],my_test.byte[0]);
+                setText(0,0,"read from reg _x:(%d) %.2X %.2X \n",my_test.number, my_test.byte[1],my_test.byte[0]);
+            }
+            else
+            {
+                debug("error reading \n");
+            }
+            
+            if(result)
+            { 
+                result &= read_reg(addr_acc,OUT_Y_A  ,&(my_test.MSB));//MSB at lower
+            }
+            if(result)
+            {
+                result &= read_reg(addr_acc,OUT_Y_A+1,&(my_test.LSB));
+            }
+            if(result)
+            {
+                //a_test.y = my_test.number;
+                a_test.y = (my_test.number / (float)(32768 /*half of the ADC resolution*/ / _FS/*+- 4g*/));
+                //debug("read from reg _x:(%d) %.2X %.2X \n",my_test.number, my_test.byte[1],my_test.byte[0]);
+                setText(0,1,"read from reg _y:(%d) %.2X %.2X \n",my_test.number, my_test.byte[1],my_test.byte[0]);
+            }
+            else
+            { 
+                debug("error reading \n");
+            }
+            
+            if(result)
+            { 
+                result &= read_reg(addr_acc,OUT_Z_A  ,&(my_test.MSB));//MSB at lower
+            }
+            if(result)
+            {
+                result &= read_reg(addr_acc,OUT_Z_A+1,&(my_test.LSB));
+            }
+            if(result)
+            {
+                //a_test.z = my_test.number;
+                a_test.z = (my_test.number / (float)(32768 /*half of the ADC resolution*/ / _FS/*+- 4g*/));
+                //debug("read from reg _x:(%d) %.2X %.2X \n",my_test.number, my_test.byte[1],my_test.byte[0]);
+                setText(0,2,"read from reg _z:(%d) %.2X %.2X \n",my_test.number, my_test.byte[1],my_test.byte[0]);
+            }
+            else
+            { 
+                debug("error reading \n");
+            }
+            //debug("test 4: x: %.4f y: %.4f z: %.4f \n",a_test.x,a_test.y,a_test.z);
+            setText(0,4,"test 4: x: %.4f y: %.4f z: %.4f \n",a_test.x,a_test.y,a_test.z);
+        }
+    /*result &= read_reg_short(addr_acc, OUT_X_A, &a_x);
+    result &= read_reg_short(addr_acc, OUT_Y_A, &a_y);
+    result &= read_reg_short(addr_acc, OUT_Z_A, &a_z);*/
+     //This test pass so its ok
     
-    // offset and scale
-    m.x = (m_x + _offset_x) * _scale_x;
-    m.y = (m_y + _offset_y) * _scale_y;
-    m.z = (m_z + _offset_z) * _scale_z;
+    result &= read_reg_short(addr_mag, OUT_X_M, &m_x);
+    result &= read_reg_short(addr_mag, OUT_Y_M, &m_y);
+    result &= read_reg_short(addr_mag, OUT_Z_M, &m_z);
+    #if defined(CHECK_TIME_SEQUENCE)
+        usec2 = t.read_us();
+        
+        debug("%d %d %d\n", usec1, usec2, usec2-usec1);//if (debug) debug->printf("%d %d %d\n", usec1, usec2, usec2-usec1);
+    #endif
+    if(result == true)
+    {
+        // Perform simple lowpass filtering
+        // Intended to stabilize heading despite
+        // device vibration such as on a UGV
+        
+        #if 0
+        if(data_read_acc & (1<<3))
+        {
+            a_test.x= a_test.x / (32768 /*half of the ADC resolution*/ / _FS/*+- 4g*/);
+            a_test.y= a_test.y / (32768 /*half of the ADC resolution*/ / _FS/*+- 4g*/);
+            a_test.z= a_test.z / (32768 /*half of the ADC resolution*/ / _FS/*+- 4g*/);
+            
+            debug("test 4: x: %.4f y: %.4f z: %.4f \n",a_test.x,a_test.y,a_test.z);
+        }
+        #endif
+        //float( a[i] ) * pow(2.,(fs+1)) / 32768.
+        
+        //x/8 = reading /0xFFFF(655355)
+        
+        
+        
+       // _filt_ax = _filt_ax + (a_x - (_filt_ax >> FILTER_SHIFT));
+        /*_filt_ax = _filt_ax + (ax_test - (_filt_ax >> FILTER_SHIFT));
+        _filt_ay += a_y - (_filt_ay >> FILTER_SHIFT);
+        _filt_az += a_z - (_filt_az >> FILTER_SHIFT);
+    
+        
+        
+        a.x = (float) (_filt_ax >> FILTER_SHIFT);
+        a.y = (float) (_filt_ay >> FILTER_SHIFT);
+        a.z = (float) (_filt_az >> FILTER_SHIFT);*/
+        
+        // offset and scale
+        m.x = (m_x + _offset_x) * _scale_x;
+        m.y = (m_y + _offset_y) * _scale_y;
+        m.z = (m_z + _offset_z) * _scale_z;
+    }
+    
+    return result;
 }
 
 
@@ -188,7 +346,7 @@
 {
     vector a, m;
 
-    this->read(a, m);
+    read(a, m);
     
     ////////////////////////////////////////////////
     // compute heading       
@@ -215,5 +373,5 @@
 
 void LSM303DLH::frequency(int hz)
 {
-    _compass.frequency(hz);
-}
\ No newline at end of file
+    m_ptr_I2C->frequency(hz);
+}