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.
Diff: BMP085.cpp
- Revision:
- 0:9a1682a09c50
- Child:
- 3:450acaa4f52d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BMP085.cpp Wed Jul 17 18:50:28 2013 +0000 @@ -0,0 +1,171 @@ +/* + * BMP085.cpp + * + * Created on: 13 juil. 2013 + * Author: Vincent + */ + +#include "BMP085.h" +#include <new> + +BMP085::BMP085(PinName sda, PinName scl) : i2c_(*reinterpret_cast<I2C*>(i2cRaw)) +{ + // Placement new to avoid additional heap memory allocation. + new(i2cRaw) I2C(sda, scl); + + pressure = 101300; + temperature = 298; +} + +BMP085::~BMP085() +{ + // If the I2C object is initialized in the buffer in this object, call destructor of it. + if(&i2c_ == reinterpret_cast<I2C*>(&i2cRaw)) + reinterpret_cast<I2C*>(&i2cRaw)->~I2C(); +} + +// oversampling setting +// 0 = ultra low power +// 1 = standard +// 2 = high +// 3 = ultra high resolution +const unsigned char oversampling_setting = 3; //oversampling for measurement +const unsigned char pressure_conversiontime[4] = {4.5, 7.5, 13.5, 25.5 }; // delays for oversampling settings 0, 1, 2 and 3 + +// sensor registers from the BOSCH BMP085 datasheet +short ac1; +short ac2; +short ac3; +unsigned short ac4; +unsigned short ac5; +unsigned short ac6; +short b1; +short b2; +short mb; +short mc; +short md; + +void BMP085::writeRegister(unsigned char r, unsigned char v) +{ + char cmd1[2]; + cmd1[0] = r; + cmd1[1] = v; + i2c_.write(I2C_ADDRESS,cmd1, 2); +} + +// read a 16 bit register +int BMP085::readIntRegister(unsigned char r) +{ + char cmd1[2]; + char cmd2[1]; + //unsigned char msb, lsb; + cmd2[0] = r; + i2c_.write(I2C_ADDRESS,cmd2, 1); + i2c_.read(I2C_ADDRESS, cmd1, 2); + return (((int)cmd1[0]<<8) | ((int)cmd1[1])); +} + +// read uncompensated temperature value +unsigned int BMP085::readUT() { + writeRegister(0xf4,0x2e); + wait(0.0045); // the datasheet suggests 4.5 ms + return readIntRegister(0xf6); + +} + +// read uncompensated pressure value +long BMP085::readUP() { + writeRegister(0xf4,0x34+(oversampling_setting<<6)); + wait(pressure_conversiontime[oversampling_setting]*0.001); + + //unsigned char msb, lsb, xlsb; + char cmd1[3]; + char cmd0[1]; + cmd0[0] = 0xf6; + i2c_.write(I2C_ADDRESS,cmd0, 1); + i2c_.read(I2C_ADDRESS, cmd1, 3); + return (((long)cmd1[0]<<16) | ((long)cmd1[1]<<8) | ((long)cmd1[2])) >>(8-oversampling_setting); + +} + +// Below there are the utility functions to get data from the sensor. + +// read temperature and pressure from sensor +void BMP085::readSensor() { + + long ut= readUT(); + long up = readUP(); + + + int x1, x2, x3, b3, b5, b6, p; + unsigned int b4, b7; + //calculate true temperature + x1 = ((long)ut - ac6) * ac5 >> 15; + x2 = ((long) mc << 11) / (x1 + md); + b5 = x1 + x2; + temperature = (b5 + 8) >> 4; + + //calculate true pressure + b6 = b5 - 4000; + x1 = (b2 * (b6 * b6 >> 12)) >> 11; + x2 = ac2 * b6 >> 11; + x3 = x1 + x2; + + if (oversampling_setting == 3) b3 = ((int32_t) ac1 * 4 + x3 + 2) << 1; + if (oversampling_setting == 2) b3 = ((int32_t) ac1 * 4 + x3 + 2); + if (oversampling_setting == 1) b3 = ((int32_t) ac1 * 4 + x3 + 2) >> 1; + if (oversampling_setting == 0) b3 = ((int32_t) ac1 * 4 + x3 + 2) >> 2; + + x1 = ac3 * b6 >> 13; + x2 = (b1 * (b6 * b6 >> 12)) >> 16; + x3 = ((x1 + x2) + 2) >> 2; + b4 = (ac4 * (unsigned long) (x3 + 32768)) >> 15; + b7 = ((unsigned long) up - b3) * (50000 >> oversampling_setting); + p = b7 < 0x80000000 ? (b7 * 2) / b4 : (b7 / b4) * 2; + + x1 = (p >> 8) * (p >> 8); + x1 = (x1 * 3038) >> 16; + x2 = (-7357 * p) >> 16; + pressure = p + ((x1 + x2 + 3791) >> 4); +} + +void BMP085::getCalibrationData() { + ac1 = readIntRegister(0xAA); + ac2 = readIntRegister(0xAC); + ac3 = readIntRegister(0xAE); + ac4 = readIntRegister(0xB0); + ac5 = readIntRegister(0xB2); + ac6 = readIntRegister(0xB4); + b1 = readIntRegister(0xB6); + b2 = readIntRegister(0xB8); + mb = readIntRegister(0xBA); + mc = readIntRegister(0xBC); + md = readIntRegister(0xBE); +} + +float BMP085::press(void) +{ + return(pressure); +} + +float BMP085::temp(void) +{ + return(temperature); +} + +float BMP085::altitud(void) +{ + //press(); + // temp(); + float To=298; + float ho=0; + float Po=101325; + //ecuacion + float c=(To-0.0065*ho); + float e=(pressure/Po); + float d=exp(0.19082*log(e)); + float b=c*d; + float alt=153.84615*(To-b); + return(alt); +} +