Matthias Grob / Mbed 2 deprecated IMU_10DOF

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

Dependencies:   mbed

BMP085/BMP085_old.cpp@4:f62337b907e5, 2013-08-29 (annotated)

Committer:
maetugr
Date:
Thu Aug 29 13:52:30 2013 +0000
Revision:
4:f62337b907e5
The Altitude Sensor is now implemented, it's really 10DOF now ;); TODO: Autocalibration

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maetugr 4:f62337b907e5 1
maetugr 4:f62337b907e5 2 #include "mbed.h"
maetugr 4:f62337b907e5 3 #include "BMP085_old.h"
maetugr 4:f62337b907e5 4
maetugr 4:f62337b907e5 5 //I2C Adresse
maetugr 4:f62337b907e5 6 #define BMP085_ADRESS 0xEE
maetugr 4:f62337b907e5 7
maetugr 4:f62337b907e5 8 #define xpow(x, y) ((long)1 << y)
maetugr 4:f62337b907e5 9
maetugr 4:f62337b907e5 10
maetugr 4:f62337b907e5 11 // Constructor
maetugr 4:f62337b907e5 12 // -----------------------------------------------
maetugr 4:f62337b907e5 13 BMP085_old::BMP085_old(PinName sda, PinName scl) : i2c_(sda, scl)
maetugr 4:f62337b907e5 14 {
maetugr 4:f62337b907e5 15 Init();
maetugr 4:f62337b907e5 16 // MYINIT -------
maetugr 4:f62337b907e5 17 oss = 0; //Oversampling des Barometers setzen
maetugr 4:f62337b907e5 18 // MYINIT -------
maetugr 4:f62337b907e5 19 }
maetugr 4:f62337b907e5 20
maetugr 4:f62337b907e5 21
maetugr 4:f62337b907e5 22 // Temperatur und Druck auslesen und berechnen
maetugr 4:f62337b907e5 23 // -----------------------------------------------
maetugr 4:f62337b907e5 24 void BMP085_old::Update ()
maetugr 4:f62337b907e5 25 {
maetugr 4:f62337b907e5 26 long P, UTemp, UPressure, X1, X2, X3, B3, B5, B6;
maetugr 4:f62337b907e5 27 unsigned long B4, B7;
maetugr 4:f62337b907e5 28
maetugr 4:f62337b907e5 29 twi_writechar(BMP085_ADRESS, 0xf4, 0x2e);
maetugr 4:f62337b907e5 30 // Wait at least 4.5ms
maetugr 4:f62337b907e5 31 wait(0.005);
maetugr 4:f62337b907e5 32 UTemp = twi_readshort(BMP085_ADRESS, 0xf6);
maetugr 4:f62337b907e5 33
maetugr 4:f62337b907e5 34 X1 = ((UTemp - AC6) * AC5) >> 15;
maetugr 4:f62337b907e5 35 X2 = (MC << 11) / (X1 + MD);
maetugr 4:f62337b907e5 36 B5 = X1 + X2;
maetugr 4:f62337b907e5 37 Temperature = (float)((B5 + 8) >> 4)/10.0;
maetugr 4:f62337b907e5 38
maetugr 4:f62337b907e5 39 twi_writechar(BMP085_ADRESS, 0xf4, 0x34 + (oss << 6));
maetugr 4:f62337b907e5 40 // Wait at least 4.5ms
maetugr 4:f62337b907e5 41 wait(0.005);
maetugr 4:f62337b907e5 42 UPressure = twi_readlong(BMP085_ADRESS, 0xf6) >> (8 - oss);
maetugr 4:f62337b907e5 43
maetugr 4:f62337b907e5 44 B6 = B5 - 4000;
maetugr 4:f62337b907e5 45 X1 = (B2 * (B6 * B6) >> 12) >> 11;
maetugr 4:f62337b907e5 46 X2 = (AC2 * B6) >> 11;
maetugr 4:f62337b907e5 47 X3 = X1 + X2;
maetugr 4:f62337b907e5 48 B3 = ((AC1 * 4 + X3) << oss) >> 2;
maetugr 4:f62337b907e5 49
maetugr 4:f62337b907e5 50 X1 = (AC3 * B6) >> 13;
maetugr 4:f62337b907e5 51 X2 = (B1 * (B6 * B6) >> 12) >> 16;
maetugr 4:f62337b907e5 52 X3 = ((X1 + X2) + 2) >> 2;
maetugr 4:f62337b907e5 53 B4 = AC4 * (X3 + 32768) >> 15;
maetugr 4:f62337b907e5 54
maetugr 4:f62337b907e5 55 B7 = (unsigned long)(UPressure - B3) * (50000 >> oss);
maetugr 4:f62337b907e5 56
maetugr 4:f62337b907e5 57 if (B7 < 0x80000000)
maetugr 4:f62337b907e5 58 {
maetugr 4:f62337b907e5 59 P = (2 * B7) / B4;
maetugr 4:f62337b907e5 60 }
maetugr 4:f62337b907e5 61 else
maetugr 4:f62337b907e5 62 {
maetugr 4:f62337b907e5 63 P = 2* (B7 / B4);
maetugr 4:f62337b907e5 64 }
maetugr 4:f62337b907e5 65 X1 = (P >> 8) * (P >> 8);
maetugr 4:f62337b907e5 66 X1 = (X1 * 3038) >> 16;
maetugr 4:f62337b907e5 67 X2 = (-7357 * P) >> 16;
maetugr 4:f62337b907e5 68 P = P + ((X1 + X2 + 3791) >> 4);
maetugr 4:f62337b907e5 69 Pressure = (float)P / 100.0;
maetugr 4:f62337b907e5 70 }
maetugr 4:f62337b907e5 71
maetugr 4:f62337b907e5 72
maetugr 4:f62337b907e5 73 // Hoehe u.M. berechnen (Druck in hPa)
maetugr 4:f62337b907e5 74 // -----------------------------------------------
maetugr 4:f62337b907e5 75 float BMP085_old::CalcAltitude(float Press)
maetugr 4:f62337b907e5 76 {
maetugr 4:f62337b907e5 77 float A = Press/1013.25;
maetugr 4:f62337b907e5 78 float B = 1/5.25588;
maetugr 4:f62337b907e5 79 float C = pow(A,B);
maetugr 4:f62337b907e5 80
maetugr 4:f62337b907e5 81 C = 1 - C;
maetugr 4:f62337b907e5 82 C = C / 22.5577e-6;
maetugr 4:f62337b907e5 83 return C;
maetugr 4:f62337b907e5 84 }
maetugr 4:f62337b907e5 85
maetugr 4:f62337b907e5 86
maetugr 4:f62337b907e5 87 // Drucksensor initialisieren
maetugr 4:f62337b907e5 88 // -----------------------------------------------
maetugr 4:f62337b907e5 89 void BMP085_old::Init ()
maetugr 4:f62337b907e5 90 {
maetugr 4:f62337b907e5 91 AC1 = twi_readshort(BMP085_ADRESS, 0xaa);
maetugr 4:f62337b907e5 92 AC2 = twi_readshort(BMP085_ADRESS, 0xac);
maetugr 4:f62337b907e5 93 AC3 = twi_readshort(BMP085_ADRESS, 0xae);
maetugr 4:f62337b907e5 94 AC4 = twi_readshort(BMP085_ADRESS, 0xb0);
maetugr 4:f62337b907e5 95 AC5 = twi_readshort(BMP085_ADRESS, 0xb2);
maetugr 4:f62337b907e5 96 AC6 = twi_readshort(BMP085_ADRESS, 0xb4);
maetugr 4:f62337b907e5 97 B1 = twi_readshort(BMP085_ADRESS, 0xb6);
maetugr 4:f62337b907e5 98 B2 = twi_readshort(BMP085_ADRESS, 0xb8);
maetugr 4:f62337b907e5 99 MB = twi_readshort(BMP085_ADRESS, 0xba);
maetugr 4:f62337b907e5 100 MC = twi_readshort(BMP085_ADRESS, 0xbc);
maetugr 4:f62337b907e5 101 MD = twi_readshort(BMP085_ADRESS, 0xbe);
maetugr 4:f62337b907e5 102 }
maetugr 4:f62337b907e5 103
maetugr 4:f62337b907e5 104
maetugr 4:f62337b907e5 105 // -----------------------------------------------
maetugr 4:f62337b907e5 106 unsigned short BMP085_old::twi_readshort (int id, int addr) {
maetugr 4:f62337b907e5 107 unsigned short i;
maetugr 4:f62337b907e5 108
maetugr 4:f62337b907e5 109 i2c_.start();
maetugr 4:f62337b907e5 110 i2c_.write(id);
maetugr 4:f62337b907e5 111 i2c_.write(addr);
maetugr 4:f62337b907e5 112
maetugr 4:f62337b907e5 113 i2c_.start();
maetugr 4:f62337b907e5 114 i2c_.write(id | 1);
maetugr 4:f62337b907e5 115 i = i2c_.read(1) << 8;
maetugr 4:f62337b907e5 116 i |= i2c_.read(0);
maetugr 4:f62337b907e5 117 i2c_.stop();
maetugr 4:f62337b907e5 118
maetugr 4:f62337b907e5 119 return i;
maetugr 4:f62337b907e5 120 }
maetugr 4:f62337b907e5 121
maetugr 4:f62337b907e5 122
maetugr 4:f62337b907e5 123 // -----------------------------------------------
maetugr 4:f62337b907e5 124 unsigned long BMP085_old::twi_readlong (int id, int addr) {
maetugr 4:f62337b907e5 125 unsigned long i;
maetugr 4:f62337b907e5 126
maetugr 4:f62337b907e5 127 i2c_.start();
maetugr 4:f62337b907e5 128 i2c_.write(id);
maetugr 4:f62337b907e5 129 i2c_.write(addr);
maetugr 4:f62337b907e5 130
maetugr 4:f62337b907e5 131 i2c_.start();
maetugr 4:f62337b907e5 132 i2c_.write(id | 1);
maetugr 4:f62337b907e5 133 i = i2c_.read(1) << 16;
maetugr 4:f62337b907e5 134 i |= i2c_.read(1) << 8;
maetugr 4:f62337b907e5 135 i |= i2c_.read(0);
maetugr 4:f62337b907e5 136 i2c_.stop();
maetugr 4:f62337b907e5 137
maetugr 4:f62337b907e5 138 return i;
maetugr 4:f62337b907e5 139 }
maetugr 4:f62337b907e5 140
maetugr 4:f62337b907e5 141
maetugr 4:f62337b907e5 142 // -----------------------------------------------
maetugr 4:f62337b907e5 143 void BMP085_old::twi_writechar (int id, int addr, int dat) {
maetugr 4:f62337b907e5 144
maetugr 4:f62337b907e5 145 i2c_.start();
maetugr 4:f62337b907e5 146 i2c_.write(id);
maetugr 4:f62337b907e5 147 i2c_.write(addr);
maetugr 4:f62337b907e5 148 i2c_.write(dat);
maetugr 4:f62337b907e5 149 i2c_.stop();
maetugr 4:f62337b907e5 150 }