Diff: LSM303DLH.cpp

Revision:

6:86cf2afe3e52

Parent:

5:48722ae56546

Child:

7:275a0a69cff5

diff -r 48722ae56546 -r 86cf2afe3e52 LSM303DLH.cpp
--- a/LSM303DLH.cpp	Sun Aug 06 19:11:23 2017 +0000
+++ b/LSM303DLH.cpp	Sun Aug 06 22:23:24 2017 +0000
@@ -41,8 +41,7 @@
 
 #define FILTER_SHIFT 6      // used in filtering acceleromter readings
 
-const int addr_acc = 0x32;//0x30;
-const int addr_mag = 0x3c;
+
 
 
 
@@ -108,15 +107,50 @@
     return result;
 }
 
+bool LSM303DLH::read_reg_short(DEV_ADDRS addr_i2c,REG_ADDRS addr_reg, OUT_XYZ_t *dataRead)
+{
+     bool result=true;
+     
+     switch(addr_reg)
+     {
+         case OUT_X_A:
+         case OUT_Y_A:
+         case OUT_Z_A:
+         
+         case OUT_X_M:
+         case OUT_Y_M:
+         case OUT_Z_M:
+            result &= read_reg(addr_i2c,addr_reg  ,&(dataRead->UT_L_A));//LSB at lower
+            result &= read_reg(addr_i2c,addr_reg+1,&(dataRead->UT_H_A));
+            #if defined(LSM303_LITLE_ENDIAN)
+                dataRead.value = (dataRead.byte[1]<<8)+(dataRead.byte[0]); //swap the reading
+            #endif
+             break;
+        default:
+            result = false;
+            break;
+    }
+    
+    return result;
+}
+
+LSM303DLH::~LSM303DLH()
+{
+    if(m_have_createdI2C)
+    {
+        delete m_ptr_I2C;
+    }
+}
 LSM303DLH::LSM303DLH(PinName sda, PinName scl)     
 {
    m_ptr_I2C = new I2C(sda, scl);
-   init(); 
+   m_have_createdI2C = true;
+   this->init(); 
 }
 LSM303DLH::LSM303DLH(I2C* ptrI2C)
 {
     m_ptr_I2C = ptrI2C;
-    //debug( "ptr pass %d, ptr here %d   \n",(int)ptrI2C,(int)m_ptr_I2C);
+    m_have_createdI2C = false;
     this->init();
 }
 
@@ -124,7 +158,6 @@
 {
     char reg_v;
  
-    debug("In INIT \n");
     _offset_x = 0; 
     _offset_y = 0;
     _offset_z = 0; 
@@ -148,29 +181,35 @@
     //reg_v = 0;
     //read_reg(addr_acc,CTRL_REG1_A,&reg_v);
 
-    reg_v = 0;
-    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 */
+    ((Ctrl_Reg4_A_t*)&reg_v)->byte = 0;
+    ((Ctrl_Reg4_A_t*)&reg_v)->BDU |= 1;     //full read befor update
+    //(((Ctrl_Reg4_A_t*)&reg_v)->HR) |= 1;      //hi res
+    #if defined(LSM303_LITLE_ENDIAN)
+        ((Ctrl_Reg4_A_t*)&reg_v)->BLE |= 1;     /* 1: data MSB @ lower address */
+    #endif
+    ((Ctrl_Reg4_A_t*)&reg_v)->FS |= 0b01; ;     /* +/- 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 */
+    ((CRA_REG_M_t*)&reg_v)->byte = 0;
+    ((CRA_REG_M_t*)&reg_v)->DO |= 0b100;     /* Minimum data output rate = 15Hz */
     write_reg(addr_mag,CRA_REG_M,reg_v);
 
     reg_v = 0;
     //reg_v |= 0x01 << 5;     /* +-1.3Gauss */
-    (((CRB_REG_M_t*)&reg_v)->GN) |= 0b111;     /* +-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 */
+   ((MR_REG_M_t*)&reg_v)->byte = 0;
+   //((MR_REG_M_t*)&reg_v)->MD  |= 0;              /* Continuous-conversion mode */
     write_reg(addr_mag,MR_REG_M,reg_v);
     
-    debug("Out INIT \n");
-    clearscreen();
+    //put here since we dont change it during the execution
+    m_FS = get_FullScall_selection();
+    
+    read_reg(addr_mag,CRB_REG_M ,&reg_v);
+    m_GN = (((CRB_REG_M_t*)&reg_v)->GN)-1;
 }
 
 void LSM303DLH::setOffset(float x, float y, float z)
@@ -192,7 +231,7 @@
     
     bool result = true;
     //short a_x, a_y, a_z;
-    short m_x, m_y, m_z;
+    //short m_x, m_y, m_z;
     #if defined(CHECK_TIME_SEQUENCE)
         Timer t;
         int usec1, usec2;
@@ -203,110 +242,14 @@
         usec1 = t.read_us();
     #endif
    
-   /*union{
-        int16_t number;
-        char byte[2];
-        struct{
-            char MSB;
-            char LSB;
-            };
-        }*/
-        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);
-        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.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.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
-            {
-                debug("error reading \n");
-            }
-            
-            if(result)
-            { 
-                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.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
-            { 
-                debug("error reading \n");
-            }
-            
-            if(result)
-            { 
-                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.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");
-            }
-            //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
+    //OUT_XYZ_t my_test,dataOut;
+    static vector a_test, m_test;
     
-    char data_read_mag =0;
-    read_reg(addr_mag,SR_REG_M,&data_read_mag);
+    result &= read_acc_raw(&a_test);
     
-    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);
-    }
+    result &= read_mag_raw(&m_test);
+    
+    
     #if defined(CHECK_TIME_SEQUENCE)
         usec2 = t.read_us();
         
@@ -318,16 +261,7 @@
         // 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*/);
-            
-           
-        }
-        #endif
+       
         //float( a[i] ) * pow(2.,(fs+1)) / 32768.
         
         //x/8 = reading /0xFFFF(655355)
@@ -344,19 +278,15 @@
         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
-        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;
-        }
+        
+        a = a_test;
+ 
+        
+        // offset and scale        
+        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;
@@ -411,3 +341,117 @@
         
     return 2<<((((Ctrl_Reg4_A_t*)&data_read_acc)->FS));
 }
+
+float LSM303DLH::get_acc_value_in_g(OUT_XYZ_t* dataOut)
+{
+   return (float) (dataOut->value / (float)(32768 /*half of the ADC resolution*/ / m_FS/*+- 4g*/));
+}
+
+bool LSM303DLH::read_acc_raw(vector *a)
+{
+    bool result = true;
+    char data_read_acc =0;
+    OUT_XYZ_t dataOut;
+    
+    read_reg(addr_acc,STATUS_REG_A,&data_read_acc);
+    //char _FS = get_FullScall_selection();
+   
+    if(((Status_Reg_A_t*)&data_read_acc)->ZYXDA)//new data
+    {
+        result &= read_reg_short(addr_acc,OUT_X_A,&dataOut);
+        
+        if(result)
+        {
+            a->x = get_acc_value_in_g(&dataOut);
+           
+            //setText(0,0,"read from reg _x:(%d) %.2X %.2X \n",my_test.value/*number*/, my_test.byte[1],my_test.byte[0]);
+        }
+        else
+        {
+            debug("error reading \n");
+        }
+        
+        if(result)
+        {
+            result &= read_reg_short(addr_acc,OUT_Y_A,&dataOut);                
+        }
+        if(result)
+        {
+            a->y = get_acc_value_in_g(&dataOut);
+            
+            //setText(0,1,"read from reg _y:(%d) %.2X %.2X \n",my_test.value/*number*/, my_test.byte[1],my_test.byte[0]);
+        }
+        else
+        { 
+            debug("error reading \n");
+        }
+        
+        if(result)
+        { 
+            result &= read_reg_short(addr_acc,OUT_Z_A,&dataOut);
+        }
+        if(result)
+        {
+            a->z = get_acc_value_in_g(&dataOut);
+            
+            //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");
+        }
+        //setText(0,4,"test 4: x: %.4f y: %.4f z: %.4f \n",a_test.x,a_test.y,a_test.z);
+    }
+    
+    return result;
+}
+bool LSM303DLH::read_mag_raw(vector *m)
+{
+    bool result = true;
+    char data_read_mag =0;
+    OUT_XYZ_t dataOut;
+    
+    read_reg(addr_mag,SR_REG_M,&data_read_mag);
+    
+    
+    /**@todo not sure if the reading of magnetometer is Litle or big endian, I assume its change like the accelerometer.
+     * You can try to find the answer if you care.
+     */
+     
+    if(((SR_Reg_M_t*)&data_read_mag)->DRDY)
+    {    
+        float gainxy[] = { 1100., 855., 670., 450., 400., 330., 230. };
+        float gainz[]  = {  980., 760., 600., 400., 355., 295., 205. };
+        
+        //setText(0,6,"GN: %d \n",_GN);
+        result &= read_reg_short(addr_mag,OUT_X_M,&dataOut);
+        if(result)
+        {
+            //dataOut.value = (dataOut.byte[0]<<8)+(dataOut.byte[1]);//only a test
+            
+            setText(0,0,"read from reg _x:(%d) %.2X %.2X \n",dataOut.value/*number*/, dataOut.byte[1],dataOut.byte[0]);
+            m->x = float(dataOut.value) / gainxy[m_GN];
+        }
+        
+        result &= read_reg_short(addr_mag,OUT_Y_M,&dataOut);
+        if(result)
+        {
+            //dataOut.value = (dataOut.byte[0]<<8)+(dataOut.byte[1]);//only a test
+            
+            setText(0,1,"read from reg _y:(%d) %.2X %.2X \n",dataOut.value/*number*/, dataOut.byte[1],dataOut.byte[0]);
+            m->y = float(dataOut.value) / gainxy[m_GN];
+        }
+            
+        result &= read_reg_short(addr_mag,OUT_Z_M,&dataOut);
+        if(result)
+        {
+            //dataOut.value = (dataOut.byte[0]<<8)+(dataOut.byte[1]);//only a test
+            
+            setText(0,2,"read from reg _z:(%d) %.2X %.2X \n",dataOut.value/*number*/, dataOut.byte[1],dataOut.byte[0]);
+            m->z = float(dataOut.value) / gainz[m_GN];
+        }        
+        setText(0,4,"test 4: x: %.4f y: %.4f z: %.4f \n",m->x,m->y,m->z);
+    }
+    
+    return result;
+}
\ No newline at end of file