test_BMX055.cpp

00001 #include "mbed.h"
00002 #include "test_BMX055.h"
00003 
00004 /*
00005 test_BMX055::test_BMX055(PinName sda,PinName scl, char slave_adr):i2c(sda ,scl),address(slave_adr),t_fine(0){
00006     
00007     //I2C initialization
00008     i2c.frequency(100000); //100 kHz
00009     
00010     currentGyroRange = 0;
00011     currentAcceleroRange=0;
00012     gravity=9.80665;
00013     RAD_TO_DEG=57.3;
00014 }
00015 */
00016 //BME280::BME280(I2C &i2c_obj, char slave_adr)
00017 
00018 test_BMX055::test_BMX055(I2C &i2c_obj, char slave_adr):i2c(i2c_obj),address(slave_adr),t_fine(0){
00019     
00020     //I2C initialization
00021     i2c.frequency(100000); //100 kHz
00022     
00023     currentGyroRange = 0;
00024     currentAcceleroRange=0;
00025     gravity=9.80665;
00026     RAD_TO_DEG=57.3;
00027 }
00028 
00029 
00030 test_BMX055::~test_BMX055(){
00031     }
00032 
00033 void test_BMX055::i2c_mem_write(int device_address, int mem_address, int mem_data){
00034     
00035     int device_address_temp = device_address<<1;
00036     device_address_temp = device_address_temp & 0xfe;
00037 
00038     i2c.start();
00039     i2c.write(device_address_temp);
00040     i2c.write(mem_address);
00041     i2c.write(mem_data); 
00042     i2c.stop();   
00043     return;
00044     
00045     }
00046 
00047 //i2c read function
00048 int test_BMX055::i2c_mem_read(int device_address, int mem_address)
00049 {   
00050     int device_address_temp = device_address<<1;
00051     int device_address_temp_w = device_address_temp & 0xfe; 
00052     int device_address_temp_r = device_address_temp | 0x01;
00053 
00054     i2c.start();
00055     i2c.write(device_address_temp_w);
00056     i2c.write(mem_address);  
00057     i2c.start();
00058     i2c.write(device_address_temp_r);    
00059     int data = i2c.read(0);
00060     i2c.stop();   
00061     return data;
00062 }
00063 
00064 void test_BMX055::test_BMX055::set_gyro_range(int range){
00065     
00066     currentGyroRange=range;
00067     
00068     if(range==0){
00069         i2c_mem_write(GYRO_ADDRESS, GYRO_RANGE_SET, 0x00);
00070         gyro_scale_factor=0.061f;
00071     }else if(range==1){
00072         i2c_mem_write(GYRO_ADDRESS, GYRO_RANGE_SET, 0x01);
00073         gyro_scale_factor=0.0305f;
00074     }else if(range==2){
00075         i2c_mem_write(GYRO_ADDRESS, GYRO_RANGE_SET, 0x02);
00076         gyro_scale_factor=0.0153f;
00077     }else if(range==3){
00078         i2c_mem_write(GYRO_ADDRESS, GYRO_RANGE_SET, 0x03);
00079         gyro_scale_factor=0.0076f;
00080     }else if(range==4){
00081         i2c_mem_write(GYRO_ADDRESS, GYRO_RANGE_SET, 0x04);
00082         gyro_scale_factor=0.0038f;
00083     }else{}
00084  
00085     }
00086     
00087 float test_BMX055::get_gyro_x_data()
00088 {
00089     float x_rate;
00090     
00091     //read RATE_X_LSB registor (0x02)
00092     int x_temp_L = i2c_mem_read(GYRO_ADDRESS, GYRO_X_L);
00093     //read RATE_X_MSB registor
00094     int x_temp_H = i2c_mem_read(GYRO_ADDRESS, GYRO_X_H);
00095     
00096     //calculate X angular ratio
00097     int x_data = x_temp_L + 256 * x_temp_H;
00098     if(x_data > 32767)
00099     {
00100         x_data = -1 * (65536 - x_data);    
00101     }
00102     
00103     x_rate=(float(x_data) * gyro_scale_factor)/RAD_TO_DEG; // rad/sec
00104     
00105     return x_rate;
00106 }
00107 
00108     
00109 float test_BMX055::get_gyro_y_data()
00110 {
00111     float y_rate;
00112     
00113     //read RATE_Y_LSB registor (0x04)
00114     int y_temp_L = i2c_mem_read(GYRO_ADDRESS, GYRO_Y_L);
00115     //read RATE_Y_MSB registor
00116     int y_temp_H = i2c_mem_read(GYRO_ADDRESS, GYRO_Y_H);
00117     
00118     //calculate Y angular ratio
00119     int y_data = y_temp_L + 256 * y_temp_H;
00120     if(y_data > 32767)
00121     {
00122         y_data = -1 * (65536 - y_data);    
00123     }
00124 
00125     y_rate=(float(y_data) * gyro_scale_factor)/RAD_TO_DEG; //  rad/sec
00126     
00127     return y_rate;
00128 }
00129 
00130 float test_BMX055::get_gyro_z_data()
00131 {
00132     float z_rate;
00133     
00134     //read RATE_Z_LSB registor (0x04)
00135     int z_temp_L = i2c_mem_read(GYRO_ADDRESS, GYRO_Z_L);
00136     //read RATE_Z_MSB registor
00137     int z_temp_H = i2c_mem_read(GYRO_ADDRESS, GYRO_Z_H);
00138     
00139     //calculate Z angular ratio
00140     int z_data = z_temp_L + 256 * z_temp_H;
00141     if(z_data > 32767)
00142     {
00143         z_data = -1 * (65536 - z_data);    
00144     }
00145 
00146     z_data = -1 * z_data;
00147 
00148     z_rate=(float(z_data) * gyro_scale_factor)/RAD_TO_DEG; //  rad/sec
00149     
00150     return z_rate;
00151 }
00152 
00153 void test_BMX055::test_BMX055::set_acc_range(int range){
00154     
00155     currentAcceleroRange=range;
00156     
00157     if(range==0){// +-2G
00158         i2c_mem_write(ACC_ADDRESS, ACC_RANGE_SET, 0x03);
00159         acc_scale_factor=1024;
00160     }else if(range==1){//+-4G
00161         i2c_mem_write(ACC_ADDRESS, ACC_RANGE_SET, 0x05);
00162         acc_scale_factor=512;
00163     }else if(range==2){
00164         i2c_mem_write(ACC_ADDRESS, ACC_RANGE_SET, 0x08);
00165         acc_scale_factor=256;
00166     }else if(range==3){
00167         i2c_mem_write(ACC_ADDRESS, ACC_RANGE_SET, 0x0c);
00168         acc_scale_factor=128;
00169     }else{}
00170  
00171 }
00172 
00173 float test_BMX055::get_acc_x_data()
00174 {
00175     float x_acc;
00176     
00177     //read ACC_X_LSB registor (0x02)
00178     int x_temp_L = i2c_mem_read(ACC_ADDRESS, ACC_X_L);
00179     x_temp_L = x_temp_L >> 4;
00180     x_temp_L = x_temp_L & 0x0f;
00181     
00182     //read ACC_X_MSB registor (0x03)
00183     int x_temp_H = i2c_mem_read(ACC_ADDRESS, ACC_X_H);
00184     
00185     //calculate Z acceleration
00186     int x_data = x_temp_L + 16 * x_temp_H;
00187     if(x_data > 2047)
00188     {
00189         x_data = -1 * (4096 - x_data);    
00190     }
00191     
00192     x_acc=float(x_data)*gravity/acc_scale_factor;//m/s^2
00193     
00194     return x_acc;
00195 }
00196 
00197 float test_BMX055::get_acc_y_data()
00198 {
00199     float y_acc;
00200     
00201     //read ACC_X_LSB registor (0x04)
00202     int y_temp_L = i2c_mem_read(ACC_ADDRESS, ACC_Y_L);
00203     y_temp_L = y_temp_L >> 4;
00204     y_temp_L = y_temp_L & 0x0f;
00205     
00206     //read ACC_X_MSB registor (0x05)
00207     int y_temp_H = i2c_mem_read(ACC_ADDRESS, ACC_Y_H);
00208     
00209     //calculate Z acceleration
00210     int y_data = y_temp_L + 16 * y_temp_H;
00211     if(y_data > 2047)
00212     {
00213         y_data = -1 * (4096 - y_data);    
00214     }
00215     
00216     y_acc=float(y_data)*gravity/acc_scale_factor;//m/s^2
00217     
00218     return y_acc;
00219 }
00220     
00221 float test_BMX055::get_acc_z_data()
00222 {
00223     float z_acc;
00224     
00225     //read ACC_Z_LSB registor (0x06)
00226     int z_temp_L = i2c_mem_read(ACC_ADDRESS, ACC_Z_L);
00227     z_temp_L = z_temp_L >> 4;
00228     z_temp_L = z_temp_L & 0x0f;
00229     
00230     //read ACC_Z_MSB registor (0x07)
00231     int z_temp_H = i2c_mem_read(ACC_ADDRESS, ACC_Z_H);
00232     
00233     //calculate Z acceleration
00234     int z_data = z_temp_L + 16 * z_temp_H;
00235     if(z_data > 2047)
00236     {
00237         z_data = -1*(4096 - z_data);    
00238     }
00239     
00240     z_acc=float(-z_data)*gravity/acc_scale_factor;//m/s^2
00241     
00242     return z_acc;
00243 }
00244 
00245 void test_BMX055::initiate_mag()
00246 {
00247     i2c_mem_write(MAG_ADDRESS, 0x4b, 0x83);
00248     wait_us(10000);
00249     i2c_mem_write(MAG_ADDRESS, 0x4b, 0x01);
00250     wait_us(10000);
00251     i2c_mem_write(MAG_ADDRESS, 0x4c, 0x00);
00252     wait_us(10000);
00253     i2c_mem_write(MAG_ADDRESS, 0x4c, 0x00);
00254     wait_us(10000);
00255     i2c_mem_write(MAG_ADDRESS, 0x4e, 0x84);
00256     wait_us(10000); 
00257     i2c_mem_write(MAG_ADDRESS, 0x51, 0x04);
00258     wait_us(10000);
00259     i2c_mem_write(MAG_ADDRESS, 0x52, 0x16);
00260     wait_us(10000);
00261     
00262         
00263     
00264 }
00265 
00266 float test_BMX055::get_mag_x_data()
00267 {
00268     float x_mag;
00269     
00270     //read MAG_X_LSB registor (0x42)
00271     int x_temp_L = i2c_mem_read(MAG_ADDRESS, MAG_X_L);
00272     x_temp_L = x_temp_L >> 3;
00273     x_temp_L = x_temp_L & 0x0f;
00274     
00275     //read MAG_X_MSB registor (0x43)
00276     int x_temp_H = i2c_mem_read(MAG_ADDRESS, MAG_X_H);
00277     
00278     //calculate Z acceleration
00279     int x_data = x_temp_L +  32* x_temp_H;
00280     if(x_data > 4095)
00281     {
00282         x_data = -1 * (8192 - x_data);    
00283     }
00284     
00285     x_mag=float(x_data);
00286     
00287     return x_mag;
00288     
00289 }
00290 
00291 float test_BMX055::get_mag_y_data()
00292 {
00293     float y_mag;
00294     
00295     //read MAG_Y_LSB registor (0x44)
00296     int y_temp_L = i2c_mem_read(MAG_ADDRESS, MAG_Y_L);
00297     y_temp_L = y_temp_L >> 3;
00298     y_temp_L = y_temp_L & 0x0f;
00299     
00300     //read MAG_Y_MSB registor (0x45)
00301     int y_temp_H = i2c_mem_read(MAG_ADDRESS, MAG_Y_H);
00302     
00303     //calculate Z acceleration
00304     int y_data = y_temp_L +  32* y_temp_H;
00305     if(y_data > 4095)
00306     {
00307         y_data = -1 * (8192 - y_data);    
00308     }
00309     
00310     y_mag= - 1 * float(y_data);
00311     
00312     return y_mag;
00313     
00314 }
00315 
00316 float test_BMX055::get_mag_z_data()
00317 {
00318     float z_mag;
00319     
00320     //read MAG_Z_LSB registor (0x46)
00321     int z_temp_L = i2c_mem_read(MAG_ADDRESS, MAG_Z_L);
00322     z_temp_L = z_temp_L >> 1;
00323     z_temp_L = z_temp_L & 0x0f;
00324     
00325     //read MAG_Z_MSB registor (0x47)
00326     int z_temp_H = i2c_mem_read(MAG_ADDRESS, MAG_Z_H);
00327     
00328     //calculate Z acceleration
00329     int z_data = z_temp_L +  128* z_temp_H;
00330     if(z_data > 16383)
00331     {
00332         z_data = -1 * (32768 - z_data);    
00333     }
00334     
00335     z_mag= - 1* float(z_data);
00336     
00337     return z_mag;
00338     
00339 }
00340 
00341 //Pressure sensor BME280
00342 
00343 void test_BMX055::initialize()
00344 {
00345     //I2C initialization
00346     //i2c.frequency(400000); //400 kHz
00347     //i2c.frequency(100000); //100 kHz
00348     
00349     char cmd[18];
00350  
00351     cmd[0] = 0xf2; // ctrl_hum
00352     cmd[1] = 0x01; // Humidity oversampling x1
00353     i2c.write(address, cmd, 2);
00354  
00355     cmd[0] = 0xf4; // ctrl_meas
00356     cmd[1] = 0x27; // Temparature oversampling x1, Pressure oversampling x1, Normal mode
00357     i2c.write(address, cmd, 2);
00358  
00359     cmd[0] = 0xf5; // config
00360     cmd[1] = 0xa0; // Standby 1000ms, Filter off
00361     i2c.write(address, cmd, 2);
00362  
00363     cmd[0] = 0x88; // read dig_T regs
00364     i2c.write(address, cmd, 1);
00365     i2c.read(address, cmd, 6);
00366  
00367     dig_T1 = (cmd[1] << 8) | cmd[0];
00368     dig_T2 = (cmd[3] << 8) | cmd[2];
00369     dig_T3 = (cmd[5] << 8) | cmd[4];
00370  
00371     //DEBUG_PRINT("dig_T = 0x%x, 0x%x, 0x%x\n", dig_T1, dig_T2, dig_T3);
00372  
00373     cmd[0] = 0x8E; // read dig_P regs
00374     i2c.write(address, cmd, 1);
00375     i2c.read(address, cmd, 18);
00376  
00377     dig_P1 = (cmd[ 1] << 8) | cmd[ 0];
00378     dig_P2 = (cmd[ 3] << 8) | cmd[ 2];
00379     dig_P3 = (cmd[ 5] << 8) | cmd[ 4];
00380     dig_P4 = (cmd[ 7] << 8) | cmd[ 6];
00381     dig_P5 = (cmd[ 9] << 8) | cmd[ 8];
00382     dig_P6 = (cmd[11] << 8) | cmd[10];
00383     dig_P7 = (cmd[13] << 8) | cmd[12];
00384     dig_P8 = (cmd[15] << 8) | cmd[14];
00385     dig_P9 = (cmd[17] << 8) | cmd[16];
00386  
00387     //DEBUG_PRINT("dig_P = 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x\n", dig_P1, dig_P2, dig_P3, dig_P4, dig_P5, dig_P6, dig_P7, dig_P8, dig_P9);
00388  
00389     cmd[0] = 0xA1; // read dig_H regs
00390     i2c.write(address, cmd, 1);
00391     i2c.read(address, cmd, 1);
00392      cmd[1] = 0xE1; // read dig_H regs
00393     i2c.write(address, &cmd[1], 1);
00394     i2c.read(address, &cmd[1], 7);
00395 
00396     dig_H1 = cmd[0];
00397     dig_H2 = (cmd[2] << 8) | cmd[1];
00398     dig_H3 = cmd[3];
00399     dig_H4 = (cmd[4] << 4) | (cmd[5] & 0x0f);
00400     dig_H5 = (cmd[6] << 4) | ((cmd[5]>>4) & 0x0f);
00401     dig_H6 = cmd[7];
00402  
00403     //DEBUG_PRINT("dig_H = 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x\n", dig_H1, dig_H2, dig_H3, dig_H4, dig_H5, dig_H6);
00404 }
00405 
00406 float test_BMX055::getTemperature()
00407 {
00408     uint32_t temp_raw;
00409     float tempf;
00410     char cmd[4];
00411  
00412     cmd[0] = 0xfa; // temp_msb
00413     i2c.write(address, cmd, 1);
00414     i2c.read(address, &cmd[1], 3);
00415  
00416     temp_raw = (cmd[1] << 12) | (cmd[2] << 4) | (cmd[3] >> 4);
00417  
00418     int32_t temp;
00419  
00420     temp =
00421         (((((temp_raw >> 3) - (dig_T1 << 1))) * dig_T2) >> 11) +
00422         ((((((temp_raw >> 4) - dig_T1) * ((temp_raw >> 4) - dig_T1)) >> 12) * dig_T3) >> 14);
00423  
00424     t_fine = temp;
00425     temp = (temp * 5 + 128) >> 8;
00426     tempf = (float)temp;
00427  
00428     return (tempf/100.0f);
00429 }
00430  
00431 float test_BMX055::getPressure()
00432 {
00433     uint32_t press_raw;
00434     float pressf;
00435     char cmd[4];
00436  
00437     cmd[0] = 0xf7; // press_msb
00438     i2c.write(address, cmd, 1);
00439     i2c.read(address, &cmd[1], 3);
00440  
00441     press_raw = (cmd[1] << 12) | (cmd[2] << 4) | (cmd[3] >> 4);
00442  
00443     int32_t var1, var2;
00444     uint32_t press;
00445  
00446     var1 = (t_fine >> 1) - 64000;
00447     var2 = (((var1 >> 2) * (var1 >> 2)) >> 11) * dig_P6;
00448     var2 = var2 + ((var1 * dig_P5) << 1);
00449     var2 = (var2 >> 2) + (dig_P4 << 16);
00450     var1 = (((dig_P3 * (((var1 >> 2)*(var1 >> 2)) >> 13)) >> 3) + ((dig_P2 * var1) >> 1)) >> 18;
00451     var1 = ((32768 + var1) * dig_P1) >> 15;
00452     if (var1 == 0) {
00453         return 0;
00454     }
00455     press = (((1048576 - press_raw) - (var2 >> 12))) * 3125;
00456     if(press < 0x80000000) {
00457         press = (press << 1) / var1;
00458     } else {
00459         press = (press / var1) * 2;
00460     }
00461     var1 = ((int32_t)dig_P9 * ((int32_t)(((press >> 3) * (press >> 3)) >> 13))) >> 12;
00462     var2 = (((int32_t)(press >> 2)) * (int32_t)dig_P8) >> 13;
00463     press = (press + ((var1 + var2 + dig_P7) >> 4));
00464  
00465     pressf = (float)press;
00466     return (pressf/100.0f);
00467 }
00468  
00469 float test_BMX055::getHumidity()
00470 {
00471     uint32_t hum_raw;
00472     float humf;
00473     char cmd[4];
00474  
00475     cmd[0] = 0xfd; // hum_msb
00476     i2c.write(address, cmd, 1);
00477     i2c.read(address, &cmd[1], 2);
00478  
00479     hum_raw = (cmd[1] << 8) | cmd[2];
00480  
00481     int32_t v_x1;
00482  
00483     v_x1 = t_fine - 76800;
00484     v_x1 =  (((((hum_raw << 14) -(((int32_t)dig_H4) << 20) - (((int32_t)dig_H5) * v_x1)) +
00485                ((int32_t)16384)) >> 15) * (((((((v_x1 * (int32_t)dig_H6) >> 10) *
00486                                             (((v_x1 * ((int32_t)dig_H3)) >> 11) + 32768)) >> 10) + 2097152) *
00487                                             (int32_t)dig_H2 + 8192) >> 14));
00488     v_x1 = (v_x1 - (((((v_x1 >> 15) * (v_x1 >> 15)) >> 7) * (int32_t)dig_H1) >> 4));
00489     v_x1 = (v_x1 < 0 ? 0 : v_x1);
00490     v_x1 = (v_x1 > 419430400 ? 419430400 : v_x1);
00491  
00492     humf = (float)(v_x1 >> 12);
00493  
00494     return (humf/1024.0f);
00495 }
00496 /*
00497 void test_BMX055::initiate_pres()
00498 {
00499  
00500     i2c_mem_write(PRES_ADDRESS, CTRL_HUM, 0x01);
00501     i2c_mem_write(PRES_ADDRESS, CONFIG, 0xa0);
00502  
00503     //dig_T1
00504     int dig_t1_Low = i2c_mem_read(PRES_ADDRESS,DIG_T1_LOW_REGS);
00505     int dig_t1_High = i2c_mem_read(PRES_ADDRESS,DIG_T1_HIGH_REGS);
00506     dig_T1 = uint16_t(dig_t1_Low + 256 * dig_t1_High);
00507 
00508     //dig_T2
00509     int dig_t2_Low = i2c_mem_read(PRES_ADDRESS, DIG_T2_LOW_REGS);
00510     int dig_t2_High = i2c_mem_read(PRES_ADDRESS, DIG_T2_HIGH_REGS);
00511     dig_T2 = int16_t(dig_t2_Low + 256 * dig_t2_High);
00512     
00513     //dig_T3
00514     int dig_t3_Low = i2c_mem_read(PRES_ADDRESS, DIG_T3_LOW_REGS);
00515     int dig_t3_High = i2c_mem_read(PRES_ADDRESS, DIG_T3_HIGH_REGS);
00516     dig_T3 = int16_t(dig_t3_Low + 256 * dig_t3_High);
00517  
00518  
00519     //Pressure
00520 
00521     //dig_P1
00522     int dig_p1_Low = i2c_mem_read(PRES_ADDRESS, DIG_P1_LOW_REGS);
00523     int dig_p1_High = i2c_mem_read(PRES_ADDRESS, DIG_P1_HIGH_REGS);
00524     dig_P1 = uint16_t(dig_p1_Low + 256 * dig_p1_High);
00525 
00526     //dig_P2
00527     int dig_p2_Low = i2c_mem_read(PRES_ADDRESS, DIG_P2_LOW_REGS);
00528     int dig_p2_High = i2c_mem_read(PRES_ADDRESS, DIG_P2_HIGH_REGS);
00529     dig_P2 = int16_t(dig_p2_Low + 256 * dig_p2_High);
00530     
00531     //dig_P3
00532     int dig_p3_Low = i2c_mem_read(PRES_ADDRESS, DIG_P3_LOW_REGS);
00533     int dig_p3_High = i2c_mem_read(PRES_ADDRESS, DIG_P3_HIGH_REGS);
00534     dig_P3 = int16_t(dig_p3_Low + 256 * dig_p3_High);
00535 
00536     //dig_P4
00537     int dig_p4_Low = i2c_mem_read(PRES_ADDRESS, DIG_P4_LOW_REGS);
00538     int dig_p4_High = i2c_mem_read(PRES_ADDRESS, DIG_P4_HIGH_REGS);
00539     dig_P4 = int16_t(dig_p4_Low + 256 * dig_p4_High);
00540     
00541     //dig_P5
00542     int dig_p5_Low = i2c_mem_read(PRES_ADDRESS, DIG_P5_LOW_REGS);
00543     int dig_p5_High = i2c_mem_read(PRES_ADDRESS, DIG_P5_HIGH_REGS);
00544     dig_P5 = int16_t(dig_p5_Low + 256 * dig_p5_High);
00545     
00546     //dig_P6
00547     int dig_p6_Low = i2c_mem_read(PRES_ADDRESS, DIG_P6_LOW_REGS);
00548     int dig_p6_High = i2c_mem_read(PRES_ADDRESS, DIG_P6_HIGH_REGS);
00549     dig_P6 = int16_t(dig_p6_Low + 256 * dig_p6_High);
00550     
00551     //dig_P7
00552     int dig_p7_Low = i2c_mem_read(PRES_ADDRESS, DIG_P7_LOW_REGS);
00553     int dig_p7_High = i2c_mem_read(PRES_ADDRESS, DIG_P7_HIGH_REGS);
00554     dig_P7 = int16_t(dig_p7_Low + 256 * dig_p7_High);
00555     
00556     //dig_P8
00557     int dig_p8_Low = i2c_mem_read(PRES_ADDRESS, DIG_P8_LOW_REGS);
00558     int dig_p8_High = i2c_mem_read(PRES_ADDRESS, DIG_P8_HIGH_REGS);
00559     dig_P8 = int16_t(dig_p8_Low + 256 * dig_p8_High);
00560     
00561     //dig_P9
00562     int dig_p9_Low = i2c_mem_read(PRES_ADDRESS, DIG_P9_LOW_REGS);
00563     int dig_p9_High = i2c_mem_read(PRES_ADDRESS, DIG_P9_HIGH_REGS);
00564     dig_P9 = int16_t(dig_p2_Low + 256 * dig_p9_High);
00565     
00566     
00567     //Humid
00568     //dig_H1
00569     dig_H1 = uint16_t(i2c_mem_read(PRES_ADDRESS, HUD_H1_REGS));
00570     
00571     //dig_H2
00572     int dig_h2_Low = i2c_mem_read(PRES_ADDRESS, HUD_H2_LOW_REGS);
00573     int dig_h2_High = i2c_mem_read(PRES_ADDRESS, HUD_H2_HIGH_REGS);
00574     dig_H2 = int16_t(dig_h2_Low + 256 * dig_h2_High);
00575     
00576     //dig_H3
00577     dig_H3 = uint16_t(i2c_mem_read(PRES_ADDRESS, HUD_H3_REGS));    
00578     
00579     //dig_H4
00580     int dig_h4_Low = i2c_mem_read(PRES_ADDRESS, HUD_H4_LOW_REGS);
00581     int dig_h4_High = i2c_mem_read(PRES_ADDRESS, HUD_H4_HIGH_REGS);
00582     dig_H4 = int16_t((dig_h4_High << 4) | (dig_h4_Low & 0x0f));
00583     
00584     //HUD_H5_HIGH_REGS
00585     int dig_h5_High = i2c_mem_read(PRES_ADDRESS, HUD_H5_HIGH_REGS);
00586     dig_H5 = int16_t((dig_h5_High << 4) | ((dig_h4_Low>>4) & 0x0f));
00587     
00588     //HUD H6
00589     dig_H6 = int16_t(i2c_mem_read(PRES_ADDRESS, HUD_H6_REGS));
00590  
00591 }
00592 
00593 float test_BMX055::getTemperature()
00594 {
00595     uint32_t temp_raw;
00596     float tempf;
00597     
00598     //Tempreture raw data
00599     int temp_XLow = i2c_mem_read(PRES_ADDRESS, TEMP_XLSB);
00600     int temp_Low = i2c_mem_read(PRES_ADDRESS, TEMP_LSB);
00601     int temp_High = i2c_mem_read(PRES_ADDRESS, TEMP_MSB);
00602 
00603     temp_raw = (temp_High << 12) | (temp_Low << 4) | (temp_XLow >> 4);    
00604  
00605     int32_t temp;
00606  
00607     temp =
00608         (((((temp_raw >> 3) - (dig_T1 << 1))) * dig_T2) >> 11) +
00609         ((((((temp_raw >> 4) - dig_T1) * ((temp_raw >> 4) - dig_T1)) >> 12) * dig_T3) >> 14);
00610  
00611     t_fine = temp;
00612     temp = (temp * 5 + 128) >> 8;
00613     tempf = (float)temp;
00614  
00615     return (tempf/100.0f);
00616 }
00617 
00618 float test_BMX055::getPressure()
00619 {
00620     uint32_t press_raw;
00621     float pressf;
00622  
00623     int press_XLow = i2c_mem_read(PRES_ADDRESS, PRES_XLSB);
00624     int press_Low = i2c_mem_read(PRES_ADDRESS, PRES_LSB);
00625     int press_High = i2c_mem_read(PRES_ADDRESS, PRES_MSB);
00626  
00627     press_raw = (press_High << 12) | (press_Low << 4) | (press_XLow >> 4);
00628  
00629     int32_t var1, var2;
00630     uint32_t press;
00631  
00632     var1 = (t_fine >> 1) - 64000;
00633     var2 = (((var1 >> 2) * (var1 >> 2)) >> 11) * dig_P6;
00634     var2 = var2 + ((var1 * dig_P5) << 1);
00635     var2 = (var2 >> 2) + (dig_P4 << 16);
00636     var1 = (((dig_P3 * (((var1 >> 2)*(var1 >> 2)) >> 13)) >> 3) + ((dig_P2 * var1) >> 1)) >> 18;
00637     var1 = ((32768 + var1) * dig_P1) >> 15;
00638     if (var1 == 0) {
00639         return 0;
00640     }
00641     press = (((1048576 - press_raw) - (var2 >> 12))) * 3125;
00642     if(press < 0x80000000) {
00643         press = (press << 1) / var1;
00644     } else {
00645         press = (press / var1) * 2;
00646     }
00647     var1 = ((int32_t)dig_P9 * ((int32_t)(((press >> 3) * (press >> 3)) >> 13))) >> 12;
00648     var2 = (((int32_t)(press >> 2)) * (int32_t)dig_P8) >> 13;
00649     press = (press + ((var1 + var2 + dig_P7) >> 4));
00650  
00651     pressf = (float)press;
00652     return (pressf/100.0f);
00653 }
00654 
00655 float test_BMX055::getHumidity()
00656 {
00657     uint32_t hum_raw;
00658     float humf;
00659  
00660     int hud_Low = i2c_mem_read(PRES_ADDRESS, HUM_LSB);
00661     int hud_High = i2c_mem_read(PRES_ADDRESS, HUM_MSB);
00662  
00663     hum_raw = (hud_High << 8) | hud_Low;
00664  
00665     int32_t v_x1;
00666  
00667     v_x1 = t_fine - 76800;
00668     v_x1 =  (((((hum_raw << 14) -(((int32_t)dig_H4) << 20) - (((int32_t)dig_H5) * v_x1)) +
00669                ((int32_t)16384)) >> 15) * (((((((v_x1 * (int32_t)dig_H6) >> 10) *
00670                                             (((v_x1 * ((int32_t)dig_H3)) >> 11) + 32768)) >> 10) + 2097152) *
00671                                             (int32_t)dig_H2 + 8192) >> 14));
00672     v_x1 = (v_x1 - (((((v_x1 >> 15) * (v_x1 >> 15)) >> 7) * (int32_t)dig_H1) >> 4));
00673     v_x1 = (v_x1 < 0 ? 0 : v_x1);
00674     v_x1 = (v_x1 > 419430400 ? 419430400 : v_x1);
00675  
00676     humf = (float)(v_x1 >> 12);
00677  
00678     return (humf/1024.0f);
00679 }
00680 */