IMU 10dof MEMS from DR Robot adapted from HK10DOF Changed gyro to ITG3200
Fork of HK10DOF by
WARNING: This project is not complete, but this library seems ok so far.
I have the module DFRobotics.com 10DOF MEMS IMU. I wanted a concise module for resolving direction and movement.
I found HK10DOF library (http://developer.mbed.org/users/pommzorz/code/HK10DOF/) with quaternions. But it used a different gyro. So I modified that code to use the same higher level calls but use the ITG3200 low level calls.
BMP085.cpp@0:9a1682a09c50, 2013-07-17 (annotated)
- Committer:
- pommzorz
- Date:
- Wed Jul 17 18:50:28 2013 +0000
- Revision:
- 0:9a1682a09c50
- Child:
- 3:450acaa4f52d
mbed implementation of the FreeIMU imu for the HobbyKing 10DOF board
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
pommzorz | 0:9a1682a09c50 | 1 | /* |
pommzorz | 0:9a1682a09c50 | 2 | * BMP085.cpp |
pommzorz | 0:9a1682a09c50 | 3 | * |
pommzorz | 0:9a1682a09c50 | 4 | * Created on: 13 juil. 2013 |
pommzorz | 0:9a1682a09c50 | 5 | * Author: Vincent |
pommzorz | 0:9a1682a09c50 | 6 | */ |
pommzorz | 0:9a1682a09c50 | 7 | |
pommzorz | 0:9a1682a09c50 | 8 | #include "BMP085.h" |
pommzorz | 0:9a1682a09c50 | 9 | #include <new> |
pommzorz | 0:9a1682a09c50 | 10 | |
pommzorz | 0:9a1682a09c50 | 11 | BMP085::BMP085(PinName sda, PinName scl) : i2c_(*reinterpret_cast<I2C*>(i2cRaw)) |
pommzorz | 0:9a1682a09c50 | 12 | { |
pommzorz | 0:9a1682a09c50 | 13 | // Placement new to avoid additional heap memory allocation. |
pommzorz | 0:9a1682a09c50 | 14 | new(i2cRaw) I2C(sda, scl); |
pommzorz | 0:9a1682a09c50 | 15 | |
pommzorz | 0:9a1682a09c50 | 16 | pressure = 101300; |
pommzorz | 0:9a1682a09c50 | 17 | temperature = 298; |
pommzorz | 0:9a1682a09c50 | 18 | } |
pommzorz | 0:9a1682a09c50 | 19 | |
pommzorz | 0:9a1682a09c50 | 20 | BMP085::~BMP085() |
pommzorz | 0:9a1682a09c50 | 21 | { |
pommzorz | 0:9a1682a09c50 | 22 | // If the I2C object is initialized in the buffer in this object, call destructor of it. |
pommzorz | 0:9a1682a09c50 | 23 | if(&i2c_ == reinterpret_cast<I2C*>(&i2cRaw)) |
pommzorz | 0:9a1682a09c50 | 24 | reinterpret_cast<I2C*>(&i2cRaw)->~I2C(); |
pommzorz | 0:9a1682a09c50 | 25 | } |
pommzorz | 0:9a1682a09c50 | 26 | |
pommzorz | 0:9a1682a09c50 | 27 | // oversampling setting |
pommzorz | 0:9a1682a09c50 | 28 | // 0 = ultra low power |
pommzorz | 0:9a1682a09c50 | 29 | // 1 = standard |
pommzorz | 0:9a1682a09c50 | 30 | // 2 = high |
pommzorz | 0:9a1682a09c50 | 31 | // 3 = ultra high resolution |
pommzorz | 0:9a1682a09c50 | 32 | const unsigned char oversampling_setting = 3; //oversampling for measurement |
pommzorz | 0:9a1682a09c50 | 33 | const unsigned char pressure_conversiontime[4] = {4.5, 7.5, 13.5, 25.5 }; // delays for oversampling settings 0, 1, 2 and 3 |
pommzorz | 0:9a1682a09c50 | 34 | |
pommzorz | 0:9a1682a09c50 | 35 | // sensor registers from the BOSCH BMP085 datasheet |
pommzorz | 0:9a1682a09c50 | 36 | short ac1; |
pommzorz | 0:9a1682a09c50 | 37 | short ac2; |
pommzorz | 0:9a1682a09c50 | 38 | short ac3; |
pommzorz | 0:9a1682a09c50 | 39 | unsigned short ac4; |
pommzorz | 0:9a1682a09c50 | 40 | unsigned short ac5; |
pommzorz | 0:9a1682a09c50 | 41 | unsigned short ac6; |
pommzorz | 0:9a1682a09c50 | 42 | short b1; |
pommzorz | 0:9a1682a09c50 | 43 | short b2; |
pommzorz | 0:9a1682a09c50 | 44 | short mb; |
pommzorz | 0:9a1682a09c50 | 45 | short mc; |
pommzorz | 0:9a1682a09c50 | 46 | short md; |
pommzorz | 0:9a1682a09c50 | 47 | |
pommzorz | 0:9a1682a09c50 | 48 | void BMP085::writeRegister(unsigned char r, unsigned char v) |
pommzorz | 0:9a1682a09c50 | 49 | { |
pommzorz | 0:9a1682a09c50 | 50 | char cmd1[2]; |
pommzorz | 0:9a1682a09c50 | 51 | cmd1[0] = r; |
pommzorz | 0:9a1682a09c50 | 52 | cmd1[1] = v; |
pommzorz | 0:9a1682a09c50 | 53 | i2c_.write(I2C_ADDRESS,cmd1, 2); |
pommzorz | 0:9a1682a09c50 | 54 | } |
pommzorz | 0:9a1682a09c50 | 55 | |
pommzorz | 0:9a1682a09c50 | 56 | // read a 16 bit register |
pommzorz | 0:9a1682a09c50 | 57 | int BMP085::readIntRegister(unsigned char r) |
pommzorz | 0:9a1682a09c50 | 58 | { |
pommzorz | 0:9a1682a09c50 | 59 | char cmd1[2]; |
pommzorz | 0:9a1682a09c50 | 60 | char cmd2[1]; |
pommzorz | 0:9a1682a09c50 | 61 | //unsigned char msb, lsb; |
pommzorz | 0:9a1682a09c50 | 62 | cmd2[0] = r; |
pommzorz | 0:9a1682a09c50 | 63 | i2c_.write(I2C_ADDRESS,cmd2, 1); |
pommzorz | 0:9a1682a09c50 | 64 | i2c_.read(I2C_ADDRESS, cmd1, 2); |
pommzorz | 0:9a1682a09c50 | 65 | return (((int)cmd1[0]<<8) | ((int)cmd1[1])); |
pommzorz | 0:9a1682a09c50 | 66 | } |
pommzorz | 0:9a1682a09c50 | 67 | |
pommzorz | 0:9a1682a09c50 | 68 | // read uncompensated temperature value |
pommzorz | 0:9a1682a09c50 | 69 | unsigned int BMP085::readUT() { |
pommzorz | 0:9a1682a09c50 | 70 | writeRegister(0xf4,0x2e); |
pommzorz | 0:9a1682a09c50 | 71 | wait(0.0045); // the datasheet suggests 4.5 ms |
pommzorz | 0:9a1682a09c50 | 72 | return readIntRegister(0xf6); |
pommzorz | 0:9a1682a09c50 | 73 | |
pommzorz | 0:9a1682a09c50 | 74 | } |
pommzorz | 0:9a1682a09c50 | 75 | |
pommzorz | 0:9a1682a09c50 | 76 | // read uncompensated pressure value |
pommzorz | 0:9a1682a09c50 | 77 | long BMP085::readUP() { |
pommzorz | 0:9a1682a09c50 | 78 | writeRegister(0xf4,0x34+(oversampling_setting<<6)); |
pommzorz | 0:9a1682a09c50 | 79 | wait(pressure_conversiontime[oversampling_setting]*0.001); |
pommzorz | 0:9a1682a09c50 | 80 | |
pommzorz | 0:9a1682a09c50 | 81 | //unsigned char msb, lsb, xlsb; |
pommzorz | 0:9a1682a09c50 | 82 | char cmd1[3]; |
pommzorz | 0:9a1682a09c50 | 83 | char cmd0[1]; |
pommzorz | 0:9a1682a09c50 | 84 | cmd0[0] = 0xf6; |
pommzorz | 0:9a1682a09c50 | 85 | i2c_.write(I2C_ADDRESS,cmd0, 1); |
pommzorz | 0:9a1682a09c50 | 86 | i2c_.read(I2C_ADDRESS, cmd1, 3); |
pommzorz | 0:9a1682a09c50 | 87 | return (((long)cmd1[0]<<16) | ((long)cmd1[1]<<8) | ((long)cmd1[2])) >>(8-oversampling_setting); |
pommzorz | 0:9a1682a09c50 | 88 | |
pommzorz | 0:9a1682a09c50 | 89 | } |
pommzorz | 0:9a1682a09c50 | 90 | |
pommzorz | 0:9a1682a09c50 | 91 | // Below there are the utility functions to get data from the sensor. |
pommzorz | 0:9a1682a09c50 | 92 | |
pommzorz | 0:9a1682a09c50 | 93 | // read temperature and pressure from sensor |
pommzorz | 0:9a1682a09c50 | 94 | void BMP085::readSensor() { |
pommzorz | 0:9a1682a09c50 | 95 | |
pommzorz | 0:9a1682a09c50 | 96 | long ut= readUT(); |
pommzorz | 0:9a1682a09c50 | 97 | long up = readUP(); |
pommzorz | 0:9a1682a09c50 | 98 | |
pommzorz | 0:9a1682a09c50 | 99 | |
pommzorz | 0:9a1682a09c50 | 100 | int x1, x2, x3, b3, b5, b6, p; |
pommzorz | 0:9a1682a09c50 | 101 | unsigned int b4, b7; |
pommzorz | 0:9a1682a09c50 | 102 | //calculate true temperature |
pommzorz | 0:9a1682a09c50 | 103 | x1 = ((long)ut - ac6) * ac5 >> 15; |
pommzorz | 0:9a1682a09c50 | 104 | x2 = ((long) mc << 11) / (x1 + md); |
pommzorz | 0:9a1682a09c50 | 105 | b5 = x1 + x2; |
pommzorz | 0:9a1682a09c50 | 106 | temperature = (b5 + 8) >> 4; |
pommzorz | 0:9a1682a09c50 | 107 | |
pommzorz | 0:9a1682a09c50 | 108 | //calculate true pressure |
pommzorz | 0:9a1682a09c50 | 109 | b6 = b5 - 4000; |
pommzorz | 0:9a1682a09c50 | 110 | x1 = (b2 * (b6 * b6 >> 12)) >> 11; |
pommzorz | 0:9a1682a09c50 | 111 | x2 = ac2 * b6 >> 11; |
pommzorz | 0:9a1682a09c50 | 112 | x3 = x1 + x2; |
pommzorz | 0:9a1682a09c50 | 113 | |
pommzorz | 0:9a1682a09c50 | 114 | if (oversampling_setting == 3) b3 = ((int32_t) ac1 * 4 + x3 + 2) << 1; |
pommzorz | 0:9a1682a09c50 | 115 | if (oversampling_setting == 2) b3 = ((int32_t) ac1 * 4 + x3 + 2); |
pommzorz | 0:9a1682a09c50 | 116 | if (oversampling_setting == 1) b3 = ((int32_t) ac1 * 4 + x3 + 2) >> 1; |
pommzorz | 0:9a1682a09c50 | 117 | if (oversampling_setting == 0) b3 = ((int32_t) ac1 * 4 + x3 + 2) >> 2; |
pommzorz | 0:9a1682a09c50 | 118 | |
pommzorz | 0:9a1682a09c50 | 119 | x1 = ac3 * b6 >> 13; |
pommzorz | 0:9a1682a09c50 | 120 | x2 = (b1 * (b6 * b6 >> 12)) >> 16; |
pommzorz | 0:9a1682a09c50 | 121 | x3 = ((x1 + x2) + 2) >> 2; |
pommzorz | 0:9a1682a09c50 | 122 | b4 = (ac4 * (unsigned long) (x3 + 32768)) >> 15; |
pommzorz | 0:9a1682a09c50 | 123 | b7 = ((unsigned long) up - b3) * (50000 >> oversampling_setting); |
pommzorz | 0:9a1682a09c50 | 124 | p = b7 < 0x80000000 ? (b7 * 2) / b4 : (b7 / b4) * 2; |
pommzorz | 0:9a1682a09c50 | 125 | |
pommzorz | 0:9a1682a09c50 | 126 | x1 = (p >> 8) * (p >> 8); |
pommzorz | 0:9a1682a09c50 | 127 | x1 = (x1 * 3038) >> 16; |
pommzorz | 0:9a1682a09c50 | 128 | x2 = (-7357 * p) >> 16; |
pommzorz | 0:9a1682a09c50 | 129 | pressure = p + ((x1 + x2 + 3791) >> 4); |
pommzorz | 0:9a1682a09c50 | 130 | } |
pommzorz | 0:9a1682a09c50 | 131 | |
pommzorz | 0:9a1682a09c50 | 132 | void BMP085::getCalibrationData() { |
pommzorz | 0:9a1682a09c50 | 133 | ac1 = readIntRegister(0xAA); |
pommzorz | 0:9a1682a09c50 | 134 | ac2 = readIntRegister(0xAC); |
pommzorz | 0:9a1682a09c50 | 135 | ac3 = readIntRegister(0xAE); |
pommzorz | 0:9a1682a09c50 | 136 | ac4 = readIntRegister(0xB0); |
pommzorz | 0:9a1682a09c50 | 137 | ac5 = readIntRegister(0xB2); |
pommzorz | 0:9a1682a09c50 | 138 | ac6 = readIntRegister(0xB4); |
pommzorz | 0:9a1682a09c50 | 139 | b1 = readIntRegister(0xB6); |
pommzorz | 0:9a1682a09c50 | 140 | b2 = readIntRegister(0xB8); |
pommzorz | 0:9a1682a09c50 | 141 | mb = readIntRegister(0xBA); |
pommzorz | 0:9a1682a09c50 | 142 | mc = readIntRegister(0xBC); |
pommzorz | 0:9a1682a09c50 | 143 | md = readIntRegister(0xBE); |
pommzorz | 0:9a1682a09c50 | 144 | } |
pommzorz | 0:9a1682a09c50 | 145 | |
pommzorz | 0:9a1682a09c50 | 146 | float BMP085::press(void) |
pommzorz | 0:9a1682a09c50 | 147 | { |
pommzorz | 0:9a1682a09c50 | 148 | return(pressure); |
pommzorz | 0:9a1682a09c50 | 149 | } |
pommzorz | 0:9a1682a09c50 | 150 | |
pommzorz | 0:9a1682a09c50 | 151 | float BMP085::temp(void) |
pommzorz | 0:9a1682a09c50 | 152 | { |
pommzorz | 0:9a1682a09c50 | 153 | return(temperature); |
pommzorz | 0:9a1682a09c50 | 154 | } |
pommzorz | 0:9a1682a09c50 | 155 | |
pommzorz | 0:9a1682a09c50 | 156 | float BMP085::altitud(void) |
pommzorz | 0:9a1682a09c50 | 157 | { |
pommzorz | 0:9a1682a09c50 | 158 | //press(); |
pommzorz | 0:9a1682a09c50 | 159 | // temp(); |
pommzorz | 0:9a1682a09c50 | 160 | float To=298; |
pommzorz | 0:9a1682a09c50 | 161 | float ho=0; |
pommzorz | 0:9a1682a09c50 | 162 | float Po=101325; |
pommzorz | 0:9a1682a09c50 | 163 | //ecuacion |
pommzorz | 0:9a1682a09c50 | 164 | float c=(To-0.0065*ho); |
pommzorz | 0:9a1682a09c50 | 165 | float e=(pressure/Po); |
pommzorz | 0:9a1682a09c50 | 166 | float d=exp(0.19082*log(e)); |
pommzorz | 0:9a1682a09c50 | 167 | float b=c*d; |
pommzorz | 0:9a1682a09c50 | 168 | float alt=153.84615*(To-b); |
pommzorz | 0:9a1682a09c50 | 169 | return(alt); |
pommzorz | 0:9a1682a09c50 | 170 | } |
pommzorz | 0:9a1682a09c50 | 171 |