IMU 10dof MEMS from DR Robot adapted from HK10DOF Changed gyro to ITG3200
Fork of HK10DOF by
BMP085.cpp
00001 /* 00002 * BMP085.cpp 00003 * 00004 * Created on: 13 juil. 2013 00005 * Author: Vincent 00006 */ 00007 00008 #include "BMP085.h" 00009 #include <new> 00010 00011 BMP085::BMP085(PinName sda, PinName scl) : i2c_(*reinterpret_cast<I2C*>(i2cRaw)) 00012 { 00013 // Placement new to avoid additional heap memory allocation. 00014 new(i2cRaw) I2C(sda, scl); 00015 i2c_.frequency(50000); 00016 pressure = 101300; 00017 temperature = 298; 00018 } 00019 00020 BMP085::~BMP085() 00021 { 00022 // If the I2C object is initialized in the buffer in this object, call destructor of it. 00023 if(&i2c_ == reinterpret_cast<I2C*>(&i2cRaw)) 00024 reinterpret_cast<I2C*>(&i2cRaw)->~I2C(); 00025 } 00026 00027 // oversampling setting 00028 // 0 = ultra low power 00029 // 1 = standard 00030 // 2 = high 00031 // 3 = ultra high resolution 00032 const unsigned char oversampling_setting = 3; //oversampling for measurement 00033 const unsigned char pressure_conversiontime[4] = {4.5, 7.5, 13.5, 25.5 }; // delays for oversampling settings 0, 1, 2 and 3 00034 00035 // sensor registers from the BOSCH BMP085 datasheet 00036 short ac1; 00037 short ac2; 00038 short ac3; 00039 unsigned short ac4; 00040 unsigned short ac5; 00041 unsigned short ac6; 00042 short b1; 00043 short b2; 00044 short mb; 00045 short mc; 00046 short md; 00047 00048 void BMP085::writeRegister(unsigned char r, unsigned char v) 00049 { 00050 char cmd1[2]; 00051 cmd1[0] = r; 00052 cmd1[1] = v; 00053 i2c_.write(I2C_ADDRESS,cmd1, 2); 00054 } 00055 00056 // read a 16 bit register 00057 int BMP085::readIntRegister(unsigned char r) 00058 { 00059 char cmd1[2]; 00060 char cmd2[1]; 00061 //unsigned char msb, lsb; 00062 cmd2[0] = r; 00063 i2c_.write(I2C_ADDRESS,cmd2, 1); 00064 i2c_.read(I2C_ADDRESS, cmd1, 2); 00065 return (((int)cmd1[0]<<8) | ((int)cmd1[1])); 00066 } 00067 00068 // read uncompensated temperature value 00069 unsigned int BMP085::readUT() { 00070 writeRegister(0xf4,0x2e); 00071 wait(0.0045); // the datasheet suggests 4.5 ms 00072 return readIntRegister(0xf6); 00073 00074 } 00075 00076 // read uncompensated pressure value 00077 long BMP085::readUP() { 00078 writeRegister(0xf4,0x34+(oversampling_setting<<6)); 00079 wait(pressure_conversiontime[oversampling_setting]*0.001); 00080 00081 //unsigned char msb, lsb, xlsb; 00082 char cmd1[3]; 00083 char cmd0[1]; 00084 cmd0[0] = 0xf6; 00085 i2c_.write(I2C_ADDRESS,cmd0, 1); 00086 i2c_.read(I2C_ADDRESS, cmd1, 3); 00087 return (((long)cmd1[0]<<16) | ((long)cmd1[1]<<8) | ((long)cmd1[2])) >>(8-oversampling_setting); 00088 00089 } 00090 00091 // Below there are the utility functions to get data from the sensor. 00092 00093 // read temperature and pressure from sensor 00094 void BMP085::readSensor() { 00095 00096 long ut= readUT(); 00097 long up = readUP(); 00098 00099 00100 int x1, x2, x3, b3, b5, b6, p; 00101 unsigned int b4, b7; 00102 //calculate true temperature 00103 x1 = ((long)ut - ac6) * ac5 >> 15; 00104 x2 = ((long) mc << 11) / (x1 + md); 00105 b5 = x1 + x2; 00106 temperature = (b5 + 8) >> 4; 00107 00108 //calculate true pressure 00109 b6 = b5 - 4000; 00110 x1 = (b2 * (b6 * b6 >> 12)) >> 11; 00111 x2 = ac2 * b6 >> 11; 00112 x3 = x1 + x2; 00113 00114 if (oversampling_setting == 3) b3 = ((int32_t) ac1 * 4 + x3 + 2) << 1; 00115 if (oversampling_setting == 2) b3 = ((int32_t) ac1 * 4 + x3 + 2); 00116 if (oversampling_setting == 1) b3 = ((int32_t) ac1 * 4 + x3 + 2) >> 1; 00117 if (oversampling_setting == 0) b3 = ((int32_t) ac1 * 4 + x3 + 2) >> 2; 00118 00119 x1 = ac3 * b6 >> 13; 00120 x2 = (b1 * (b6 * b6 >> 12)) >> 16; 00121 x3 = ((x1 + x2) + 2) >> 2; 00122 b4 = (ac4 * (unsigned long) (x3 + 32768)) >> 15; 00123 b7 = ((unsigned long) up - b3) * (50000 >> oversampling_setting); 00124 p = b7 < 0x80000000 ? (b7 * 2) / b4 : (b7 / b4) * 2; 00125 00126 x1 = (p >> 8) * (p >> 8); 00127 x1 = (x1 * 3038) >> 16; 00128 x2 = (-7357 * p) >> 16; 00129 pressure = p + ((x1 + x2 + 3791) >> 4); 00130 } 00131 00132 void BMP085::getCalibrationData() { 00133 ac1 = readIntRegister(0xAA); 00134 ac2 = readIntRegister(0xAC); 00135 ac3 = readIntRegister(0xAE); 00136 ac4 = readIntRegister(0xB0); 00137 ac5 = readIntRegister(0xB2); 00138 ac6 = readIntRegister(0xB4); 00139 b1 = readIntRegister(0xB6); 00140 b2 = readIntRegister(0xB8); 00141 mb = readIntRegister(0xBA); 00142 mc = readIntRegister(0xBC); 00143 md = readIntRegister(0xBE); 00144 } 00145 00146 float BMP085::press(void) 00147 { 00148 return(pressure); 00149 } 00150 00151 float BMP085::temp(void) 00152 { 00153 return(temperature); 00154 } 00155 00156 float BMP085::altitud(void) 00157 { 00158 //press(); 00159 // temp(); 00160 float To=298; 00161 float ho=0; 00162 float Po=101325; 00163 //ecuacion 00164 float c=(To-0.0065*ho); 00165 float e=(pressure/Po); 00166 float d=exp(0.19082*log(e)); 00167 float b=c*d; 00168 float alt=153.84615*(To-b); 00169 return(alt); 00170 } 00171
Generated on Wed Jul 20 2022 02:40:41 by 1.7.2