Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Diff: Barometer.cpp
- Revision:
- 16:3e2468d4f4c1
- Parent:
- 4:e759b8c756da
- Child:
- 19:9e9753b87cfe
--- a/Barometer.cpp Wed Mar 18 15:34:51 2015 +0000 +++ b/Barometer.cpp Fri Mar 20 10:30:01 2015 +0000 @@ -1,117 +1,105 @@ #include "Barometer.h" #define DEBUG "BMP280" #include "Logger.h" +#include <cmath> -Barometer::Barometer(I2C &i2c) : I2CPeripheral(i2c, 0xEC /* address */) +Barometer::Barometer(I2C &i2c) : I2CPeripheral(i2c, 0xEC /* address */), startingAltitude(0) { - write_reg(0xE0, 0xB6); // reset - wait_ms(2); // cf. datasheet page 8, t_startup - const uint8_t chip_id = read_reg(0xD0); - if (chip_id == 0x58) { + if (powerOn()) { + INFO("Bosch Sensortec BMP280 atmospheric pressure sensor found"); bmp280_read_calibration(); - //setFilterCoefficient(kFilter_16x); - INFO("Bosch Sensortec BMP280 ready"); + powerOff(); } else { - WARN("Bosch Sensortec BMP280 not found (chip ID=0x%02x, expected=0x58)", chip_id); + WARN("Bosch Sensortec BMP280 atmospheric pressure sensor not found"); } } -// Calibration parameters stored on chip -static uint16_t dig_T1; -static int16_t dig_T2; -static int16_t dig_T3; -static uint16_t dig_P1; -static int16_t dig_P2; -static int16_t dig_P3; -static int16_t dig_P4; -static int16_t dig_P5; -static int16_t dig_P6; -static int16_t dig_P7; -static int16_t dig_P8; -static int16_t dig_P9; +bool Barometer::powerOn() { + write_reg(0xE0, 0xB6); // reset + wait_ms(2); // cf. datasheet page 8, t_startup + return read_reg(0xD0) == 0x58; // verify chip ID +} + +void Barometer::powerOff() { + // nothing to do +} + +void Barometer::start() { + startingAltitude = 0; + lastPressure = 102000; + // set parameters for Bosch-recommended "Indoor navigation" preset + write_reg(0xF5, 0x10); // 0.5ms t_standby, IIR coefficient=16 + write_reg(0xF4, 0x57); // 2x oversampling for temperature, 16x for pressure and power mode "normal" +} + +void Barometer::stop() { + write_reg(0xF4, 0x54); // keep the oversampling settings but set power mode to "sleep" +} -void Barometer::bmp280_read_cal_reg(const uint8_t reg, char* val) -{ - *val = read_reg(reg); - *(val + 1) = read_reg(reg + 1); +Vector3 Barometer::read() { + uint8_t buffer[6]; + for (int i = 0; i < 6; i++) + buffer[i] = read_reg(0xF7 + i); + + const uint32_t adc_P = ((buffer[0] << 16) | (buffer[1] << 8) | buffer[2]) >> 4; + const uint32_t adc_T = ((buffer[3] << 16) | (buffer[4] << 8) | buffer[5]) >> 4; + + const float celsius = bmp280_val_to_temp(adc_T) - 20; // 20 degree offset (?) + const float A = 0.5; + const float pa = A * bmp280_val_to_pa(adc_P) + (1-A)*lastPressure; + lastPressure = pa; + + const float centimeter = (pressureToAltitude(pa) * 100.0) - startingAltitude; + if (startingAltitude == 0) + startingAltitude = centimeter; + + return Vector3(celsius, pa, centimeter); +} + +float Barometer::pressureToAltitude(const float pa) const { + return -44330.7692 * (pow(pa * 0.0000098692, 0.1902632365) - 1); } void Barometer::bmp280_read_calibration() { - bmp280_read_cal_reg(0x88, (char*)&dig_T1); - bmp280_read_cal_reg(0x8A, (char*)&dig_T2); - bmp280_read_cal_reg(0x8C, (char*)&dig_T3); - bmp280_read_cal_reg(0x8E, (char*)&dig_P1); - bmp280_read_cal_reg(0x90, (char*)&dig_P2); - bmp280_read_cal_reg(0x92, (char*)&dig_P3); - bmp280_read_cal_reg(0x94, (char*)&dig_P4); - bmp280_read_cal_reg(0x96, (char*)&dig_P5); - bmp280_read_cal_reg(0x98, (char*)&dig_P6); - bmp280_read_cal_reg(0x9A, (char*)&dig_P7); - bmp280_read_cal_reg(0x9C, (char*)&dig_P8); - bmp280_read_cal_reg(0x9E, (char*)&dig_P9); + struct { + uint16_t dig_T1; + int16_t dig_T2; + int16_t dig_T3; + uint16_t dig_P1; + int16_t dig_P2; + int16_t dig_P3; + int16_t dig_P4; + int16_t dig_P5; + int16_t dig_P6; + int16_t dig_P7; + int16_t dig_P8; + int16_t dig_P9; + } cal_data; + + read_reg(0x88, (uint8_t*)&cal_data, sizeof cal_data); + + dig_T1 = cal_data.dig_T1; + dig_T2 = cal_data.dig_T2; + dig_T3 = cal_data.dig_T3; + dig_P1 = cal_data.dig_P1; + dig_P2 = cal_data.dig_P2; + dig_P3 = cal_data.dig_P3; + dig_P4 = cal_data.dig_P4; + dig_P5 = cal_data.dig_P5; + dig_P6 = cal_data.dig_P6; + dig_P7 = cal_data.dig_P7; + dig_P8 = cal_data.dig_P8; + dig_P9 = cal_data.dig_P9; + LOG("Calibration parameters: T=[%u, %d, %d] P=[%u, %d, %d, %d, %d, %d, %d, %d, %d]", dig_T1, dig_T2, dig_T3, dig_P1, dig_P2, dig_P3, dig_P4, dig_P5, dig_P6, dig_P7, dig_P8, dig_P9); } -enum Oversampling { - kSkip = 0, - kOversample_1x = 1, - kOversample_2x = 2, - kOversample_4x = 3, - kOversample_8x = 4, - kOversample_16x = 5, -}; - -// Time taken to read the pressure at a particular oversampling -// cf. page 18 -static float waitTime_ms[] = { - 0, // skip - 6.4, // 1x - 8.7, // 2x - 13.3, // 4x - 22.5, // 8x - 50, // 16x -}; - -enum Filtering { - kFilter_None = 0, - kFilter_2x = 1, - kFilter_4x = 2, - kFilter_8x = 3, - kFilter_16x = 4 -}; - -void Barometer::setFilterCoefficient(const uint8_t iir) -{ - write_reg(0xF5, (iir & 0x07) << 1); - INFO("Filter coefficient => %dx", 1 << iir); -} - -void Barometer::takeMeasurement(const uint8_t tmpovr, - const uint8_t psrovr) -{ - // Start a forced measurement - write_reg(0xF4, - ((tmpovr & 0x07) << 5) | - ((psrovr & 0x07) << 2) | - 0x01 /* force reading mode */); - - // wait until it's done - //wait_ms(waitTime_ms[psrovr]); // XXX: what does this mean for BLE? -} - -// These typedefs are for Bosch's conversion algorithms below -typedef uint32_t BMP280_U32_t; -typedef int32_t BMP280_S32_t; -typedef int64_t BMP280_S64_t; - // Returns temperature in DegC, resolution is 0.01 DegC. Output value of “5123” equals 51.23 DegC. -// t_fine carries fine temperature as global value -static BMP280_S32_t t_fine; - -double bmp280_val_to_temp(BMP280_S32_t adc_T) +// XXX: converted to return float result directly +float Barometer::bmp280_val_to_temp(BMP280_S32_t adc_T) { BMP280_S32_t var1, var2, T; var1 = ((((adc_T>>3) - ((BMP280_S32_t)dig_T1<<1))) * ((BMP280_S32_t)dig_T2)) >> 11; @@ -119,12 +107,14 @@ ((BMP280_S32_t)dig_T3)) >> 14; t_fine = var1 + var2; T =(t_fine*5+128)>>8; - return T / 100.0; + return T / 100.0f; } // Returns pressure in Pa as unsigned 32 bit integer in Q24.8 format (24 integer bits and 8 fractional bits). // Output value of “24674867” represents 24674867/256 = 96386.2 Pa = 963.862 hPa -double bmp280_val_to_pa(BMP280_S32_t adc_P) +// XXX: converted it to return a float directly. +// XXX: uses t_fine, so call temperature conversion BEFORE calling this. +float Barometer::bmp280_val_to_pa(BMP280_S32_t adc_P) { BMP280_S64_t var1, var2, p; var1 = ((BMP280_S64_t)t_fine) - 128000; @@ -141,36 +131,5 @@ var1 = (((BMP280_S64_t)dig_P9) * (p>>13) * (p>>13)) >> 25; var2 = (((BMP280_S64_t)dig_P8) * p) >> 19; p = ((p + var1 + var2) >> 8) + (((BMP280_S64_t)dig_P7)<<4); - return ((BMP280_U32_t)p) / 256.0; -} - -double Barometer::getPressure() -{ - takeMeasurement(kSkip, kOversample_16x); - const uint8_t msb = read_reg(0xF7); - const uint8_t lsb = read_reg(0xF8); - const uint8_t xlsb = read_reg(0xF9); - const uint32_t val = (msb << 12) | (lsb << 4) | ((xlsb & 0xF0) >> 4); - return bmp280_val_to_pa(val) / 100.0; + return ((BMP280_U32_t)p) / 256.0f; } - -double Barometer::getTemperature() -{ - takeMeasurement(kOversample_1x, kSkip); - const uint8_t msb = read_reg(0xFA); - const uint8_t lsb = read_reg(0xFB); - const uint8_t xlsb = read_reg(0xFC); - const uint32_t val = (msb << 12) | (lsb << 4) | ((xlsb & 0xF0) >> 4); - return bmp280_val_to_temp(val); -} - -double Barometer::getAltitude() -{ - const double R = 287.05; // general gas constant - const double g = 9.80665; // acceleration due to gravity - const double T = 297.6; // supposed to be average temperature between p and p0 - const double p0 = 1000.0; // hPa sea level - const double p = getPressure(); - const double h = (R / g) * T * log(p0 / p); - return h; -} \ No newline at end of file