LSM9DS1.cpp

00001 #include "mbed.h"
00002 #include "LSM9DS1.h"
00003 #include "LSM9DS1_RegVals.h"
00004 #include "acd52832_bsp.h"
00005 
00006 LSM9DS1::LSM9DS1(I2C *i2c){
00007     _i2c = i2c;
00008 } 
00009 
00010 void LSM9DS1::startMagnetometer(){
00011     char data[2];
00012 
00013     data[0] = (char)CTRL_REG1_M;          // Target register
00014     data[1] = (char)0x7C;                 // Data to write
00015     _i2c->write(TWI_MAG_ADDR, data, 0x02,0);
00016 
00017     data[0] = (char)CTRL_REG2_M;          // Target register
00018     data[1] = (char)0x00;                 // Full scale for the sensor (+-4g)
00019     _i2c->write(TWI_MAG_ADDR, data, 0x02,0); 
00020 
00021     data[0] = (char)CTRL_REG3_M;          // Target register
00022     data[1] = (char)0x00;                 // Data to write
00023     _i2c->write(TWI_MAG_ADDR, data, 0x02,0);
00024 
00025     data[0] = (char)CTRL_REG4_M;          // Target register
00026     data[1] = (char)(3<<3);                 // Data to write
00027     _i2c->write(TWI_MAG_ADDR, data, 0x02,0);
00028 
00029     data[0] = (char)CTRL_REG5_M;          // Target register
00030     data[1] = (char)0x00;                 // Data to write
00031     _i2c->write(TWI_MAG_ADDR, data, 0x02,0);
00032 }
00033 
00034 void LSM9DS1::readMagnetometer(int16_t *results){
00035     char results_[6];
00036     int16_t resultFinal[3];
00037     char out_x_l_m = OUT_X_L_M;
00038     
00039     _i2c->write(TWI_MAG_ADDR, &out_x_l_m, 1, true);
00040     _i2c->read(TWI_MAG_ADDR, results_, 6, 0);
00041     resultFinal[0] = ((results_[1]<<8) | results_[0]);
00042     resultFinal[1] = ((results_[3]<<8) | results_[2]);
00043     resultFinal[2] = ((results_[5]<<8) | results_[4]);
00044     
00045     *(results) = resultFinal[0];
00046     *(results + 1) = resultFinal[1];
00047     *(results + 2) = resultFinal[2];
00048 }
00049 
00050 void LSM9DS1::startAccelerometer(){
00051     char data[2];
00052     
00053     data[0] = (char)CTRL_REG5_XL;          // Target register
00054     data[1] = (char)0x38;                 // Data to write
00055     _i2c->write(TWI_AG_ADDR, data, 0x02,0);
00056      
00057     
00058     data[0] = (char)CTRL_REG6_XL;          // Target register
00059     data[1] = (char)0xC7;                  // +-2g -> See reference manual for more info (page 52) 
00060     _i2c->write(TWI_AG_ADDR, data, 0x02,0);
00061 }
00062 
00063 void LSM9DS1::readAccelerometer(int16_t *results){
00064     char results_[6];
00065     int16_t resultFinal[3];
00066     char out_x_l_xl = OUT_X_L_XL;
00067     
00068     _i2c->write(TWI_AG_ADDR, &out_x_l_xl, 1, true);
00069     _i2c->read(TWI_AG_ADDR, results_, 6, 0);
00070     resultFinal[0] = ((results_[1]<<8) | results_[0]);
00071     resultFinal[1] = ((results_[3]<<8) | results_[2]);
00072     resultFinal[2] = ((results_[5]<<8) | results_[4]);
00073     
00074     *(results) = resultFinal[0];
00075     *(results + 1) = resultFinal[1];
00076     *(results + 2) = resultFinal[2];
00077 }
00078 
00079 void LSM9DS1::startGyroscope(){
00080     char data[2];
00081     
00082     data[0] = (char)CTRL_REG6_XL;                  // Target register
00083     data[1] = (char)0xC7 & (char)0x1F;             // Data to write
00084     _i2c->write(TWI_AG_ADDR, data, 0x02,0);
00085     
00086     data[0] = (char)CTRL_REG1_G;          // Target register
00087     data[1] = (char)0xC0;                 // 245 DPS scale
00088     _i2c->write(TWI_AG_ADDR, data, 0x02,0);
00089      
00090     
00091     data[0] = (char)CTRL_REG2_G;          // Target register
00092     data[1] = (char)0x00;                 // Data to write
00093     _i2c->write(TWI_AG_ADDR, data, 0x02,0);
00094      
00095     
00096     data[0] = (char)CTRL_REG3_G;          // Target register
00097     data[1] = (char)0x00;                 // Data to write
00098     _i2c->write(TWI_AG_ADDR, data, 0x02,0);
00099      
00100     
00101     data[0] = (char)CTRL_REG4;            // Target register
00102     data[1] = (char)0x3A;                 // Data to write
00103     _i2c->write(TWI_AG_ADDR, data, 0x02,0);
00104      
00105     
00106     data[0] = (char)ORIENT_CFG_G;          // Target register
00107     data[1] = (char)0x00;                 // Data to write
00108     _i2c->write(TWI_AG_ADDR, data, 0x02,0);
00109      
00110 }
00111 
00112 void LSM9DS1::readGyroscope(int16_t *results){
00113     char results_[6];
00114     int16_t resultFinal[3];
00115     char out_x_l_g = OUT_X_L_G;
00116     
00117     _i2c->write(TWI_AG_ADDR, &out_x_l_g, 1, true);
00118     _i2c->read(TWI_AG_ADDR, results_, 6, 0);
00119     resultFinal[0] = ((results_[1]<<8) | results_[0]);
00120     resultFinal[1] = ((results_[3]<<8) | results_[2]);
00121     resultFinal[2] = ((results_[5]<<8) | results_[4]);
00122     
00123     *(results) = resultFinal[0];
00124     *(results + 1) = resultFinal[1];
00125     *(results + 2) = resultFinal[2];
00126 }