Diff: LSM303DLH.cpp

Revision:

5:48722ae56546

Parent:

4:4f2ed3f8726c

Child:

6:86cf2afe3e52

--- a/LSM303DLH.cpp	Sun Aug 06 03:19:17 2017 +0000
+++ b/LSM303DLH.cpp	Sun Aug 06 19:11:23 2017 +0000
@@ -44,22 +44,12 @@
 const int addr_acc = 0x32;//0x30;
 const int addr_mag = 0x3c;
 
-enum REG_ADDRS {
-    /* --- Mag --- */
-    CRA_REG_M   = 0x00,
-    CRB_REG_M   = 0x01,
-    MR_REG_M    = 0x02,
-    OUT_X_M     = 0x03,
-    OUT_Y_M     = 0x05,
-    OUT_Z_M     = 0x07,
-    /* --- 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,
-};
+
+
+bool LSM303DLH::write_reg(int addr_i2c,int addr_reg, uint8_t v)
+{
+    return this->write_reg(addr_i2c,addr_reg,(char)v);
+}
 
 bool LSM303DLH::write_reg(int addr_i2c,int addr_reg, char v)
 {
@@ -73,6 +63,12 @@
     return result;
 }
 
+bool LSM303DLH::read_reg(int addr_i2c,int addr_reg, uint8_t *v)
+{
+    return this->read_reg(addr_i2c,addr_reg,(char*)v);
+}
+
+
 bool LSM303DLH::read_reg(int addr_i2c,int addr_reg, char *v)
 {
     char data = addr_reg; 
@@ -167,7 +163,7 @@
 
     reg_v = 0;
     //reg_v |= 0x01 << 5;     /* +-1.3Gauss */
-    reg_v |= 0x07 << 5;     /* +-8.1Gauss */
+    (((CRB_REG_M_t*)&reg_v)->GN) |= 0b111;     /* +-8.1Gauss */
     write_reg(addr_mag,CRB_REG_M,reg_v);
 
     reg_v = 0;              /* Continuous-conversion mode */
@@ -190,12 +186,12 @@
     _scale_y = y;
     _scale_z = z;
 }
-#define _FS 4
+//#define _FS 4
 bool LSM303DLH::read(vector &a, vector &m)
 {
     
     bool result = true;
-    short a_x, a_y, a_z;
+    //short a_x, a_y, a_z;
     short m_x, m_y, m_z;
     #if defined(CHECK_TIME_SEQUENCE)
         Timer t;
@@ -207,31 +203,32 @@
         usec1 = t.read_us();
     #endif
    
-   union{
+   /*union{
         int16_t number;
         char byte[2];
         struct{
             char MSB;
             char LSB;
             };
-        }my_test;
-        vector a_test;
+        }*/
+        OUT_XYZ_t my_test;
+        vector a_test, m_test;
         
         char data_read_acc =0;
         read_reg(addr_acc,STATUS_REG_A,&data_read_acc);
-        if(data_read_acc & (1<<3))//new data
+        char _FS = get_FullScall_selection();
+       
+        if(((Status_Reg_A_t*)&data_read_acc)->ZYXDA)//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));
-            }
+            result &= read_reg(addr_acc,OUT_X_A  ,&(my_test.UT_L_A/*MSB*/));//MSB at lower
+            result &= read_reg(addr_acc,OUT_X_A+1,&(my_test.UT_H_A/*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]);
+                a_test.x = (my_test.value/*number*/ / (float)(32768 /*half of the ADC resolution*/ / _FS/*+- 4g*/));
+                
+                //setText(0,0,"read from reg _x:(%d) %.2X %.2X \n",my_test.value/*number*/, my_test.byte[1],my_test.byte[0]);
             }
             else
             {
@@ -240,18 +237,15 @@
             
             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));
+                result &= read_reg(addr_acc,OUT_Y_A  ,&(my_test.UT_L_A/*MSB*/));//MSB at lower
+                result &= read_reg(addr_acc,OUT_Y_A+1,&(my_test.UT_H_A/*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]);
+                a_test.y = (my_test.value/*number*/ / (float)(32768 /*half of the ADC resolution*/ / _FS/*+- 4g*/));
+                
+                //setText(0,1,"read from reg _y:(%d) %.2X %.2X \n",my_test.value/*number*/, my_test.byte[1],my_test.byte[0]);
             }
             else
             { 
@@ -260,34 +254,59 @@
             
             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));
+                result &= read_reg(addr_acc,OUT_Z_A  ,&(my_test.UT_L_A/*MSB*/));//MSB at lower
+                result &= read_reg(addr_acc,OUT_Z_A+1,&(my_test.UT_H_A/*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]);
+                a_test.z = (my_test.value/*number*/ / (float)(32768 /*half of the ADC resolution*/ / _FS/*+- 4g*/));
+                
+                //setText(0,2,"read from reg _z:(%d) %.2X %.2X \n",my_test.value/*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);
+            //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
     
-    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);
+    char data_read_mag =0;
+    read_reg(addr_mag,SR_REG_M,&data_read_mag);
+    
+    if(((SR_Reg_M_t*)&data_read_acc)->DRDY)
+    {    
+        float gainxy[] = { 1100., 855., 670., 450., 400., 330., 230. };
+        float gainz[]  = {  980., 760., 600., 400., 355., 295., 205. };
+        char _GN;
+        read_reg(addr_mag,CRB_REG_M ,&_GN);
+        _GN = (((CRB_REG_M_t*)&_GN)->GN)-1;
+        setText(0,6,"GN: %d \n",_GN);
+        //result &= read_reg_short(addr_mag, OUT_X_M, &m_x);
+        result &= read_reg(addr_mag,OUT_X_M  ,&(my_test.UT_L_A/*MSB*/));//MSB at lower
+        result &= read_reg(addr_mag,OUT_X_M+1,&(my_test.UT_H_A/*LSB*/));
+        //a_test.x = (my_test.value/*number*/ / (float)(32768 /*half of the ADC resolution*/ / _FS/*+- 4g*/));
+        setText(0,0,"read from reg _x:(%d) %.2X %.2X \n",my_test.value/*number*/, my_test.byte[1],my_test.byte[0]);
+        m_test.x = float(my_test.value) / gainxy[_GN];
+        
+        //result &= read_reg_short(addr_mag, OUT_Y_M, &m_y);
+         result &= read_reg(addr_mag,OUT_Y_M  ,&(my_test.UT_L_A/*MSB*/));//MSB at lower
+        result &= read_reg(addr_mag,OUT_Y_M+1,&(my_test.UT_H_A/*LSB*/));
+        setText(0,1,"read from reg _y:(%d) %.2X %.2X \n",my_test.value/*number*/, my_test.byte[1],my_test.byte[0]);
+        m_test.y = float(my_test.value) / gainxy[_GN];
+        
+        //result &= read_reg_short(addr_mag, OUT_Z_M, &m_z);
+         result &= read_reg(addr_mag,OUT_Z_M  ,&(my_test.UT_L_A/*MSB*/));//MSB at lower
+        result &= read_reg(addr_mag,OUT_Z_M+1,&(my_test.UT_H_A/*LSB*/));
+        setText(0,2,"read from reg _z:(%d) %.2X %.2X \n",my_test.value/*number*/, my_test.byte[1],my_test.byte[0]);
+        m_test.z = float(my_test.value) / gainz[_GN];
+        
+        setText(0,4,"test 4: x: %.4f y: %.4f z: %.4f \n",m_test.x,m_test.y,m_test.z);
+    }
     #if defined(CHECK_TIME_SEQUENCE)
         usec2 = t.read_us();
         
@@ -306,7 +325,7 @@
             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.
@@ -316,7 +335,7 @@
         
         
        // _filt_ax = _filt_ax + (a_x - (_filt_ax >> FILTER_SHIFT));
-        /*_filt_ax = _filt_ax + (ax_test - (_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);
     
@@ -325,11 +344,19 @@
         a.x = (float) (_filt_ax >> FILTER_SHIFT);
         a.y = (float) (_filt_ay >> FILTER_SHIFT);
         a.z = (float) (_filt_az >> FILTER_SHIFT);*/
-        
+        if(((Status_Reg_A_t*)&data_read_acc)->ZYXDA)
+        {
+            a.x = a_test.x;
+            a.y = a_test.y;
+            a.z = a_test.z;
+        }
         // 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;
+        if(((SR_Reg_M_t*)&data_read_acc)->DRDY)
+        {
+            m.x = (/*m_*/m_test.x + _offset_x) * _scale_x;
+            m.y = (/*m_*/m_test.y + _offset_y) * _scale_y;
+            m.z = (/*m_*/m_test.z + _offset_z) * _scale_z;
+        }
     }
     
     return result;
@@ -376,3 +403,11 @@
 {
     m_ptr_I2C->frequency(hz);
 }
+
+int8_t LSM303DLH::get_FullScall_selection(void)
+{
+    char data_read_acc =0;
+    read_reg(addr_acc,CTRL_REG4_A,&data_read_acc);
+        
+    return 2<<((((Ctrl_Reg4_A_t*)&data_read_acc)->FS));
+}