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
BMP085_old.cpp
00001 00002 #include "mbed.h" 00003 #include "BMP085_old.h" 00004 00005 //I2C Adresse 00006 #define BMP085_ADRESS 0xEE 00007 00008 #define xpow(x, y) ((long)1 << y) 00009 00010 00011 // Constructor 00012 // ----------------------------------------------- 00013 BMP085_old::BMP085_old(PinName sda, PinName scl) : i2c_(sda, scl) 00014 { 00015 Init(); 00016 // MYINIT ------- 00017 oss = 0; //Oversampling des Barometers setzen 00018 // MYINIT ------- 00019 } 00020 00021 00022 // Temperatur und Druck auslesen und berechnen 00023 // ----------------------------------------------- 00024 void BMP085_old::Update () 00025 { 00026 long P, UTemp, UPressure, X1, X2, X3, B3, B5, B6; 00027 unsigned long B4, B7; 00028 00029 twi_writechar(BMP085_ADRESS, 0xf4, 0x2e); 00030 // Wait at least 4.5ms 00031 wait(0.005); 00032 UTemp = twi_readshort(BMP085_ADRESS, 0xf6); 00033 00034 X1 = ((UTemp - AC6) * AC5) >> 15; 00035 X2 = (MC << 11) / (X1 + MD); 00036 B5 = X1 + X2; 00037 Temperature = (float)((B5 + 8) >> 4)/10.0; 00038 00039 twi_writechar(BMP085_ADRESS, 0xf4, 0x34 + (oss << 6)); 00040 // Wait at least 4.5ms 00041 wait(0.005); 00042 UPressure = twi_readlong(BMP085_ADRESS, 0xf6) >> (8 - oss); 00043 00044 B6 = B5 - 4000; 00045 X1 = (B2 * (B6 * B6) >> 12) >> 11; 00046 X2 = (AC2 * B6) >> 11; 00047 X3 = X1 + X2; 00048 B3 = ((AC1 * 4 + X3) << oss) >> 2; 00049 00050 X1 = (AC3 * B6) >> 13; 00051 X2 = (B1 * (B6 * B6) >> 12) >> 16; 00052 X3 = ((X1 + X2) + 2) >> 2; 00053 B4 = AC4 * (X3 + 32768) >> 15; 00054 00055 B7 = (unsigned long)(UPressure - B3) * (50000 >> oss); 00056 00057 if (B7 < 0x80000000) 00058 { 00059 P = (2 * B7) / B4; 00060 } 00061 else 00062 { 00063 P = 2* (B7 / B4); 00064 } 00065 X1 = (P >> 8) * (P >> 8); 00066 X1 = (X1 * 3038) >> 16; 00067 X2 = (-7357 * P) >> 16; 00068 P = P + ((X1 + X2 + 3791) >> 4); 00069 Pressure = (float)P / 100.0; 00070 } 00071 00072 00073 // Hoehe u.M. berechnen (Druck in hPa) 00074 // ----------------------------------------------- 00075 float BMP085_old::CalcAltitude(float Press) 00076 { 00077 float A = Press/1013.25; 00078 float B = 1/5.25588; 00079 float C = pow(A,B); 00080 00081 C = 1 - C; 00082 C = C / 22.5577e-6; 00083 return C; 00084 } 00085 00086 00087 // Drucksensor initialisieren 00088 // ----------------------------------------------- 00089 void BMP085_old::Init () 00090 { 00091 AC1 = twi_readshort(BMP085_ADRESS, 0xaa); 00092 AC2 = twi_readshort(BMP085_ADRESS, 0xac); 00093 AC3 = twi_readshort(BMP085_ADRESS, 0xae); 00094 AC4 = twi_readshort(BMP085_ADRESS, 0xb0); 00095 AC5 = twi_readshort(BMP085_ADRESS, 0xb2); 00096 AC6 = twi_readshort(BMP085_ADRESS, 0xb4); 00097 B1 = twi_readshort(BMP085_ADRESS, 0xb6); 00098 B2 = twi_readshort(BMP085_ADRESS, 0xb8); 00099 MB = twi_readshort(BMP085_ADRESS, 0xba); 00100 MC = twi_readshort(BMP085_ADRESS, 0xbc); 00101 MD = twi_readshort(BMP085_ADRESS, 0xbe); 00102 } 00103 00104 00105 // ----------------------------------------------- 00106 unsigned short BMP085_old::twi_readshort (int id, int addr) { 00107 unsigned short i; 00108 00109 i2c_.start(); 00110 i2c_.write(id); 00111 i2c_.write(addr); 00112 00113 i2c_.start(); 00114 i2c_.write(id | 1); 00115 i = i2c_.read(1) << 8; 00116 i |= i2c_.read(0); 00117 i2c_.stop(); 00118 00119 return i; 00120 } 00121 00122 00123 // ----------------------------------------------- 00124 unsigned long BMP085_old::twi_readlong (int id, int addr) { 00125 unsigned long i; 00126 00127 i2c_.start(); 00128 i2c_.write(id); 00129 i2c_.write(addr); 00130 00131 i2c_.start(); 00132 i2c_.write(id | 1); 00133 i = i2c_.read(1) << 16; 00134 i |= i2c_.read(1) << 8; 00135 i |= i2c_.read(0); 00136 i2c_.stop(); 00137 00138 return i; 00139 } 00140 00141 00142 // ----------------------------------------------- 00143 void BMP085_old::twi_writechar (int id, int addr, int dat) { 00144 00145 i2c_.start(); 00146 i2c_.write(id); 00147 i2c_.write(addr); 00148 i2c_.write(dat); 00149 i2c_.stop(); 00150 }
Generated on Wed Jul 13 2022 22:03:48 by 1.7.2