An fully working IMU-Filter and Sensor drivers for the 10DOF-Board over I2C. All in one simple class. Include, calibrate sensors, call read, get angles. (3D Visualisation code for Python also included) Sensors: L3G4200D, ADXL345, HMC5883, BMP085
IMU/Sensors/Alt/BMP085.cpp@4:f62337b907e5, 2013-08-29 (annotated)
- Committer:
- maetugr
- Date:
- Thu Aug 29 13:52:30 2013 +0000
- Revision:
- 4:f62337b907e5
- Parent:
- 0:3e7450f1a938
The Altitude Sensor is now implemented, it's really 10DOF now ;); TODO: Autocalibration
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
maetugr | 0:3e7450f1a938 | 1 | #include "BMP085.h" |
maetugr | 0:3e7450f1a938 | 2 | |
maetugr | 4:f62337b907e5 | 3 | BMP085::BMP085(PinName sda, PinName scl) : I2C_Sensor(sda, scl, BMP085_I2C_ADDRESS) { |
maetugr | 4:f62337b907e5 | 4 | // read calibration data from E2PROM, needed for formulas in read function |
maetugr | 4:f62337b907e5 | 5 | char buffer[22]; // 8-Bit pieces of axis data |
maetugr | 4:f62337b907e5 | 6 | readMultiRegister(BMP085_CAL_AC1, buffer, 22); // read all calibration registers in one time 22 Byte = 176 Bit |
maetugr | 0:3e7450f1a938 | 7 | |
maetugr | 4:f62337b907e5 | 8 | AC1 = (short) (buffer[0] << 8 | buffer[1]); // join 8-Bit pieces to 16-bit short integers |
maetugr | 4:f62337b907e5 | 9 | AC2 = (short) (buffer[2] << 8 | buffer[3]); |
maetugr | 4:f62337b907e5 | 10 | AC3 = (short) (buffer[4] << 8 | buffer[5]); |
maetugr | 4:f62337b907e5 | 11 | AC4 = (unsigned short) (buffer[6] << 8 | buffer[7]); // unsigned !! |
maetugr | 4:f62337b907e5 | 12 | AC5 = (unsigned short) (buffer[8] << 8 | buffer[9]); |
maetugr | 4:f62337b907e5 | 13 | AC6 = (unsigned short) (buffer[10] << 8 | buffer[11]); |
maetugr | 4:f62337b907e5 | 14 | B1 = (short) (buffer[12] << 8 | buffer[13]); |
maetugr | 4:f62337b907e5 | 15 | B2 = (short) (buffer[14] << 8 | buffer[15]); |
maetugr | 4:f62337b907e5 | 16 | MB = (short) (buffer[16] << 8 | buffer[17]); |
maetugr | 4:f62337b907e5 | 17 | MC = (short) (buffer[18] << 8 | buffer[19]); |
maetugr | 4:f62337b907e5 | 18 | MD = (short) (buffer[20] << 8 | buffer[21]); |
maetugr | 4:f62337b907e5 | 19 | |
maetugr | 4:f62337b907e5 | 20 | |
maetugr | 4:f62337b907e5 | 21 | oss = 0; // set Oversampling of Sensor |
maetugr | 4:f62337b907e5 | 22 | } |
maetugr | 4:f62337b907e5 | 23 | |
maetugr | 4:f62337b907e5 | 24 | void BMP085::readraw() { |
maetugr | 0:3e7450f1a938 | 25 | } |
maetugr | 0:3e7450f1a938 | 26 | |
maetugr | 4:f62337b907e5 | 27 | void BMP085::read() { |
maetugr | 4:f62337b907e5 | 28 | unsigned short Uncompensated_Temperature; |
maetugr | 4:f62337b907e5 | 29 | long P, Uncompensated_Pressure, X1, X2, X3, B3, B5, B6; |
maetugr | 0:3e7450f1a938 | 30 | unsigned long B4, B7; |
maetugr | 4:f62337b907e5 | 31 | |
maetugr | 4:f62337b907e5 | 32 | // read uncompensated Temperature |
maetugr | 4:f62337b907e5 | 33 | writeRegister(BMP085_CONTROL, READ_TEMPERATURE); // say the sensor we want to read the temperature |
maetugr | 4:f62337b907e5 | 34 | wait(0.005); // Wait at least 4.5ms (written in data sheet) |
maetugr | 4:f62337b907e5 | 35 | char buffer[3]; // TODO: nur 2 wenn unten nicht gebraucht // read 16-Bit Temperature (2 registers) |
maetugr | 4:f62337b907e5 | 36 | readMultiRegister(BMP085_CONTROL_OUTPUT, buffer, 2); |
maetugr | 4:f62337b907e5 | 37 | Uncompensated_Temperature = buffer[0] << 8 | buffer[1]; // join 8-Bit pieces to 16-bit short integer |
maetugr | 0:3e7450f1a938 | 38 | |
maetugr | 4:f62337b907e5 | 39 | // calculate real Temperature |
maetugr | 4:f62337b907e5 | 40 | X1 = ((Uncompensated_Temperature - AC6) * AC5) >> 15; |
maetugr | 0:3e7450f1a938 | 41 | X2 = (MC << 11) / (X1 + MD); |
maetugr | 4:f62337b907e5 | 42 | #warning 33 is a calibration value for 3.3 degree shifft of temperature |
maetugr | 4:f62337b907e5 | 43 | B5 = X1 + X2 - ((33 << 4) - 8); // TODO: I added this term because i think my sensor is about 3.3 degree off which makes a difference in absolute altitude |
maetugr | 4:f62337b907e5 | 44 | Temperature = (float)((B5 + 8) >> 4)/10.0; // we want temperature in degree with digit after comma |
maetugr | 4:f62337b907e5 | 45 | |
maetugr | 4:f62337b907e5 | 46 | // read uncompensated Pressure |
maetugr | 4:f62337b907e5 | 47 | writeRegister(BMP085_CONTROL, READ_PRESSURE + (oss << 6)); // say the sensor we want to read the pressure |
maetugr | 4:f62337b907e5 | 48 | wait(0.005); // Wait at least 4.5ms (written in data sheet) TODO: oss fest, times vary and calculation of B3 |
maetugr | 4:f62337b907e5 | 49 | readMultiRegister(BMP085_CONTROL_OUTPUT, buffer, 3); // read 24-Bit Pressure (3 registers) |
maetugr | 4:f62337b907e5 | 50 | Uncompensated_Pressure = (unsigned int) (buffer[0] << 16 | buffer[1] << 8 | buffer[0]) >> (8 - oss); // join 8-Bit pieces to 24-bit integer |
maetugr | 4:f62337b907e5 | 51 | |
maetugr | 4:f62337b907e5 | 52 | // calculate real Pressure |
maetugr | 4:f62337b907e5 | 53 | B6 = B5 - 4000; // B5 is updated by real temperature above |
maetugr | 4:f62337b907e5 | 54 | X1 = (B2 * ( (B6 * B6) >> 12 )) >> 11; |
maetugr | 0:3e7450f1a938 | 55 | X2 = (AC2 * B6) >> 11; |
maetugr | 0:3e7450f1a938 | 56 | X3 = X1 + X2; |
maetugr | 4:f62337b907e5 | 57 | B3 = (((AC1 * 4 + X3) << oss) + 2) >> 2; |
maetugr | 0:3e7450f1a938 | 58 | |
maetugr | 0:3e7450f1a938 | 59 | X1 = (AC3 * B6) >> 13; |
maetugr | 4:f62337b907e5 | 60 | X2 = (B1 * ( (B6 * B6) >> 12) ) >> 16; |
maetugr | 0:3e7450f1a938 | 61 | X3 = ((X1 + X2) + 2) >> 2; |
maetugr | 4:f62337b907e5 | 62 | B4 = AC4 * ((unsigned long)X3 + 32768) >> 15; |
maetugr | 0:3e7450f1a938 | 63 | |
maetugr | 4:f62337b907e5 | 64 | B7 = ((unsigned long)Uncompensated_Pressure - B3) * (50000 >> oss); |
maetugr | 0:3e7450f1a938 | 65 | |
maetugr | 4:f62337b907e5 | 66 | if (B7 < 0x80000000) |
maetugr | 4:f62337b907e5 | 67 | P = (B7 * 2) / B4; |
maetugr | 0:3e7450f1a938 | 68 | else |
maetugr | 4:f62337b907e5 | 69 | P = (B7 / B4) * 2; |
maetugr | 4:f62337b907e5 | 70 | |
maetugr | 0:3e7450f1a938 | 71 | X1 = (P >> 8) * (P >> 8); |
maetugr | 0:3e7450f1a938 | 72 | X1 = (X1 * 3038) >> 16; |
maetugr | 0:3e7450f1a938 | 73 | X2 = (-7357 * P) >> 16; |
maetugr | 0:3e7450f1a938 | 74 | P = P + ((X1 + X2 + 3791) >> 4); |
maetugr | 0:3e7450f1a938 | 75 | Pressure = (float)P / 100.0; |
maetugr | 4:f62337b907e5 | 76 | |
maetugr | 4:f62337b907e5 | 77 | // calculate height out of the pressure |
maetugr | 4:f62337b907e5 | 78 | Altitude = 44330 * (1.0 - pow((Pressure / 1013.25), 1/5.25588)); |
maetugr | 4:f62337b907e5 | 79 | } |