LSM303DLH.cpp

00001 /** LSM303DLH Interface Library
00002  *
00003  * Michael Shimniok http://bot-thoughts.com
00004  * Modified by @author Salco <JeSuisSalco@gmail.com>
00005  *
00006  * This file is subject to the terms and conditions of the GNU Lesser
00007  * General Public License v2.1. See the file LICENSE in the top level
00008  * directory for more details.
00009  */
00010 #include "mbed.h"
00011 #include "LSM303DLH.h"
00012 
00013 #ifndef M_PI
00014 #define M_PI 3.14159265358979323846
00015 #endif
00016 
00017 #define FILTER_SHIFT 6      // used in filtering acceleromter readings
00018 
00019 
00020 
00021 
00022 
00023 bool LSM303DLH::write_reg(int addr_i2c,int addr_reg, uint8_t v)
00024 {
00025     return this->write_reg(addr_i2c,addr_reg,(char)v);
00026 }
00027 
00028 bool LSM303DLH::write_reg(int addr_i2c,int addr_reg, char v)
00029 {
00030     bool result=false;
00031     char data[2] = {addr_reg, v};
00032     //__disable_irq();
00033     result = m_ptr_I2C->write(addr_i2c, data, 2) == 0;
00034     if(result == false) debug("Unable to Write \n");
00035     
00036     //__enable_irq(); 
00037     return result;
00038 }
00039 
00040 bool LSM303DLH::read_reg(int addr_i2c,int addr_reg, uint8_t *v)
00041 {
00042     return this->read_reg(addr_i2c,addr_reg,(char*)v);
00043 }
00044 
00045 
00046 bool LSM303DLH::read_reg(int addr_i2c,int addr_reg, char *v)
00047 {
00048     char data = addr_reg; 
00049     bool result = false;
00050     
00051     //__disable_irq();
00052     if(m_ptr_I2C->write(addr_i2c, &data, 1) == 0)
00053     {        
00054         if (m_ptr_I2C->read(addr_i2c, &data, 1) == 0)
00055         {            
00056             *v = data;
00057             result = true;
00058         }
00059         else
00060         {
00061             debug("Unable to Read \n");
00062         }
00063     }
00064     else
00065     {
00066         debug("Unable to Write \n");
00067     }
00068     //__enable_irq();
00069     return result;
00070 }
00071 
00072 bool LSM303DLH::read_reg_short(int addr_i2c,int addr_reg, short *v)
00073 {
00074     
00075     char *pv = (char *)v;
00076     bool result;
00077     
00078     result =  read_reg(addr_i2c,addr_reg+0,pv+1);
00079     result &= read_reg(addr_i2c,addr_reg+1,pv+0);
00080   
00081     
00082     return result;
00083 }
00084 
00085 bool LSM303DLH::read_reg_short(DEV_ADDRS addr_i2c,REG_ADDRS addr_reg, OUT_XYZ_t *dataRead)
00086 {
00087      bool result=true;
00088      
00089      switch(addr_reg)
00090      {
00091          case OUT_X_A:
00092          case OUT_Y_A:
00093          case OUT_Z_A:
00094          
00095          case OUT_X_M:
00096          case OUT_Y_M:
00097          case OUT_Z_M:
00098             result &= read_reg(addr_i2c,addr_reg  ,&(dataRead->UT_L_A));//LSB at lower
00099             result &= read_reg(addr_i2c,addr_reg+1,&(dataRead->UT_H_A));
00100             #if defined(LSM303_LITLE_ENDIAN)
00101                 dataRead.value = (dataRead.byte[1]<<8)+(dataRead.byte[0]); //swap the reading
00102             #endif
00103              break;
00104         default:
00105             result = false;
00106             break;
00107     }
00108     
00109     return result;
00110 }
00111 
00112 LSM303DLH::~LSM303DLH()
00113 {
00114     if(m_have_createdI2C)
00115     {
00116         delete m_ptr_I2C;
00117     }
00118 }
00119 LSM303DLH::LSM303DLH(PinName sda, PinName scl)     
00120 {
00121    m_ptr_I2C = new I2C(sda, scl);
00122    m_have_createdI2C = true;
00123    this->init(); 
00124 }
00125 LSM303DLH::LSM303DLH(I2C* ptrI2C)
00126 {
00127     m_ptr_I2C = ptrI2C;
00128     m_have_createdI2C = false;
00129     this->init();
00130 }
00131 
00132 void LSM303DLH::init(void)
00133 {
00134     char reg_v;
00135  
00136     _offset_x = 0; 
00137     _offset_y = 0;
00138     _offset_z = 0; 
00139     _scale_x  = 0; 
00140     _scale_y  = 0; 
00141     _scale_z  = 0; 
00142     _filt_ax  = 0; 
00143     _filt_ay  = 0; 
00144     _filt_az  = 6000;
00145  
00146  
00147     m_ptr_I2C->frequency(100000);
00148     
00149     ((Ctrl_Reg1_A_t*)&reg_v)->byte = 0;
00150     ((Ctrl_Reg1_A_t*)&reg_v)->ODR |= 0b0010;     /* Normal mode  */
00151     ((Ctrl_Reg1_A_t*)&reg_v)->Xen |= 1;          /* X/Y/Z axis enable. */
00152     ((Ctrl_Reg1_A_t*)&reg_v)->Yen |= 1;
00153     ((Ctrl_Reg1_A_t*)&reg_v)->Zen |= 1;
00154     write_reg(addr_acc,CTRL_REG1_A,reg_v);
00155     //not sure if we need to read the register
00156     //reg_v = 0;
00157     //read_reg(addr_acc,CTRL_REG1_A,&reg_v);
00158 
00159     ((Ctrl_Reg4_A_t*)&reg_v)->byte = 0;
00160     ((Ctrl_Reg4_A_t*)&reg_v)->BDU |= 1;     //full read befor update
00161     //(((Ctrl_Reg4_A_t*)&reg_v)->HR) |= 1;      //hi res
00162     #if defined(LSM303_LITLE_ENDIAN)
00163         ((Ctrl_Reg4_A_t*)&reg_v)->BLE |= 1;     /* 1: data MSB @ lower address */
00164     #endif
00165     ((Ctrl_Reg4_A_t*)&reg_v)->FS |= 0b01; ;     /* +/- 4g */
00166     write_reg(addr_acc,CTRL_REG4_A,reg_v);
00167 
00168     /* -- mag --- */
00169     debug("in MAG \n");
00170     ((CRA_REG_M_t*)&reg_v)->byte = 0;
00171     ((CRA_REG_M_t*)&reg_v)->DO |= 0b100;     /* Minimum data output rate = 15Hz */
00172     write_reg(addr_mag,CRA_REG_M,reg_v);
00173 
00174     reg_v = 0;
00175     //reg_v |= 0x01 << 5;     /* +-1.3Gauss */
00176     ((CRB_REG_M_t*)&reg_v)->GN |= 0b111;     /* +-8.1Gauss */
00177     write_reg(addr_mag,CRB_REG_M,reg_v);
00178 
00179    ((MR_REG_M_t*)&reg_v)->byte = 0;
00180    //((MR_REG_M_t*)&reg_v)->MD  |= 0;              /* Continuous-conversion mode */
00181     write_reg(addr_mag,MR_REG_M,reg_v);
00182     
00183     //put here since we dont change it during the execution
00184     m_FS = get_FullScall_selection();
00185     
00186     read_reg(addr_mag,CRB_REG_M ,&reg_v);
00187     m_GN = (((CRB_REG_M_t*)&reg_v)->GN)-1;
00188 }
00189 
00190 void LSM303DLH::setOffset(float x, float y, float z)
00191 {
00192     _offset_x = x;
00193     _offset_y = y;
00194     _offset_z = z;   
00195 }
00196 
00197 void LSM303DLH::setScale(float x, float y, float z)
00198 {
00199     _scale_x = x;
00200     _scale_y = y;
00201     _scale_z = z;
00202 }
00203 //#define _FS 4
00204 bool LSM303DLH::read(vector &a, vector &m)
00205 {
00206     
00207     bool result = true;
00208     //short a_x, a_y, a_z;
00209     //short m_x, m_y, m_z;
00210     #if defined(CHECK_TIME_SEQUENCE)
00211         Timer t;
00212         int usec1, usec2;
00213         
00214         t.reset();
00215         t.start();
00216     
00217         usec1 = t.read_us();
00218     #endif
00219    
00220     static vector local_a, local_m;
00221     
00222     result &= read_acc_raw(&local_a);
00223     
00224     result &= read_mag_raw(&local_m);
00225     
00226     
00227     #if defined(CHECK_TIME_SEQUENCE)
00228         usec2 = t.read_us();
00229         
00230         debug("%d %d %d\n", usec1, usec2, usec2-usec1);//if (debug) debug->printf("%d %d %d\n", usec1, usec2, usec2-usec1);
00231     #endif
00232     if(result == true)
00233     {
00234         // Perform simple lowpass filtering
00235         // Intended to stabilize heading despite
00236         // device vibration such as on a UGV
00237         
00238        
00239         //float( a[i] ) * pow(2.,(fs+1)) / 32768.
00240         
00241         //x/8 = reading /0xFFFF(655355)
00242         
00243         
00244         
00245        // _filt_ax = _filt_ax + (a_x - (_filt_ax >> FILTER_SHIFT));
00246        /* _filt_ax = _filt_ax + (ax_test - (_filt_ax >> FILTER_SHIFT));
00247         _filt_ay += a_y - (_filt_ay >> FILTER_SHIFT);
00248         _filt_az += a_z - (_filt_az >> FILTER_SHIFT);
00249     
00250         
00251         
00252         a.x = (float) (_filt_ax >> FILTER_SHIFT);
00253         a.y = (float) (_filt_ay >> FILTER_SHIFT);
00254         a.z = (float) (_filt_az >> FILTER_SHIFT);*/
00255         
00256         a = local_a;
00257  
00258         
00259         // offset and scale        
00260         m.x = (/*m_*/local_m.x + _offset_x) * _scale_x;
00261         m.y = (/*m_*/local_m.y + _offset_y) * _scale_y;
00262         m.z = (/*m_*/local_m.z + _offset_z) * _scale_z;
00263     
00264     }
00265     
00266     return result;
00267 }
00268 
00269 
00270 // Returns the number of degrees from the -Y axis that it
00271 // is pointing.
00272 float LSM303DLH::heading()
00273 {
00274     return heading((vector){0,-1,0});
00275 }
00276 
00277 float LSM303DLH::heading(vector from)
00278 {
00279     vector a, m;
00280 
00281     read(a, m);
00282     
00283     ////////////////////////////////////////////////
00284     // compute heading       
00285     ////////////////////////////////////////////////
00286 
00287     vector temp_a = a;
00288     // normalize
00289     vector_normalize(&temp_a);
00290     //vector_normalize(&m);
00291 
00292     // compute E and N
00293     vector E;
00294     vector N;
00295     vector_cross(&m,&temp_a,&E);
00296     vector_normalize(&E);
00297     vector_cross(&temp_a,&E,&N);
00298     
00299     // compute heading
00300     float heading = atan2(vector_dot(&E,&from), vector_dot(&N,&from)) * 180/M_PI;
00301     if (heading < 0) heading += 360;
00302     
00303     return heading;
00304 }
00305 
00306 void LSM303DLH::frequency(int hz)
00307 {
00308     m_ptr_I2C->frequency(hz);
00309 }
00310 
00311 int8_t LSM303DLH::get_FullScall_selection(void)
00312 {
00313     char data_read_acc =0;
00314     read_reg(addr_acc,CTRL_REG4_A,&data_read_acc);
00315         
00316     return 2<<((((Ctrl_Reg4_A_t*)&data_read_acc)->FS));
00317 }
00318 
00319 float LSM303DLH::get_acc_value_in_g(OUT_XYZ_t* dataOut)
00320 {
00321    return (float) (dataOut->value / (float)(32768 /*half of the ADC resolution*/ / m_FS/*+- 4g*/));
00322 }
00323 
00324 bool LSM303DLH::read_acc_raw(vector *a)
00325 {
00326     bool result = true;
00327     char data_read_acc =0;
00328     OUT_XYZ_t dataOut;
00329     
00330     read_reg(addr_acc,STATUS_REG_A,&data_read_acc);
00331    
00332     if(((Status_Reg_A_t*)&data_read_acc)->ZYXDA)//new data
00333     {
00334         result &= read_reg_short(addr_acc,OUT_X_A,&dataOut);
00335         
00336         if(result)
00337         {
00338             a->x = get_acc_value_in_g(&dataOut);
00339         }
00340         else
00341         {
00342             debug("error reading \n");
00343         }
00344         
00345         if(result)
00346         {
00347             result &= read_reg_short(addr_acc,OUT_Y_A,&dataOut);                
00348         }
00349         if(result)
00350         {
00351             a->y = get_acc_value_in_g(&dataOut);
00352         }
00353         else
00354         { 
00355             debug("error reading \n");
00356         }
00357         
00358         if(result)
00359         { 
00360             result &= read_reg_short(addr_acc,OUT_Z_A,&dataOut);
00361         }
00362         if(result)
00363         {
00364             a->z = get_acc_value_in_g(&dataOut);
00365         }
00366         else
00367         { 
00368             debug("error reading \n");
00369         }
00370     }
00371     
00372     return result;
00373 }
00374 bool LSM303DLH::read_mag_raw(vector *m)
00375 {
00376     bool result = true;
00377     char data_read_mag =0;
00378     OUT_XYZ_t dataOut;
00379     
00380     read_reg(addr_mag,SR_REG_M,&data_read_mag);
00381     
00382     
00383     /**@todo not sure if the reading of magnetometer is Litle or big endian, I assume its change like the accelerometer.
00384      * You can try to find the answer if you care.
00385      */
00386      
00387     if(((SR_Reg_M_t*)&data_read_mag)->DRDY)
00388     {    
00389         float gainxy[] = { 1100., 855., 670., 450., 400., 330., 230. };
00390         float gainz[]  = {  980., 760., 600., 400., 355., 295., 205. };
00391         
00392         result &= read_reg_short(addr_mag,OUT_X_M,&dataOut);
00393         if(result)
00394         {
00395             //dataOut.value = (dataOut.byte[0]<<8)+(dataOut.byte[1]);//only a test            
00396             m->x = float(dataOut.value) / gainxy[m_GN];
00397         }
00398         
00399         result &= read_reg_short(addr_mag,OUT_Y_M,&dataOut);
00400         if(result)
00401         {
00402             //dataOut.value = (dataOut.byte[0]<<8)+(dataOut.byte[1]);//only a test            
00403             m->y = float(dataOut.value) / gainxy[m_GN];
00404         }
00405             
00406         result &= read_reg_short(addr_mag,OUT_Z_M,&dataOut);
00407         if(result)
00408         {
00409             //dataOut.value = (dataOut.byte[0]<<8)+(dataOut.byte[1]);//only a test            
00410             m->z = float(dataOut.value) / gainz[m_GN];
00411         }        
00412     }
00413     
00414     return result;
00415 }