Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Barometer.cpp
- Committer:
- pvaibhav
- Date:
- 2015-04-17
- Revision:
- 25:abb0f208e6a9
- Parent:
- 19:9e9753b87cfe
- Child:
- 40:8e852115fe55
File content as of revision 25:abb0f208e6a9:
#include "Barometer.h" #define DEBUG "BMP280" #include "Logger.h" #include <cmath> Barometer::Barometer(I2C &i2c) : I2CPeripheral(i2c, 0xEC /* address */) { if (powerOn()) { INFO("Bosch Sensortec BMP280 atmospheric pressure sensor found"); bmp280_read_calibration(); powerOff(); } else { WARN("Bosch Sensortec BMP280 atmospheric pressure sensor not found"); } } 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() { // reset our initial calibration nsamples = 0; sum = 0; avg = 0; // 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" } 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 pa = bmp280_val_to_pa(adc_P); const float centimeter = pressureToAltitude(pa) * 100.0; if (++nsamples < 10) { sum += centimeter; avg = sum / nsamples; } return Vector3(celsius, pa, centimeter - avg); } float Barometer::pressureToAltitude(const float pa) const { return -44330.7692 * (pow(pa * 0.0000098692, 0.1902632365) - 1); } void Barometer::bmp280_read_calibration() { 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); } // Returns temperature in DegC, resolution is 0.01 DegC. Output value of “5123” equals 51.23 DegC. // 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; var2 = (((((adc_T>>4) - ((BMP280_S32_t)dig_T1)) * ((adc_T>>4) - ((BMP280_S32_t)dig_T1))) >> 12) * ((BMP280_S32_t)dig_T3)) >> 14; t_fine = var1 + var2; T =(t_fine*5+128)>>8; 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 // 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; var2 = var1 * var1 * (BMP280_S64_t)dig_P6; var2 = var2 + ((var1*(BMP280_S64_t)dig_P5)<<17); var2 = var2 + (((BMP280_S64_t)dig_P4)<<35); var1 = ((var1 * var1 * (BMP280_S64_t)dig_P3)>>8) + ((var1 * (BMP280_S64_t)dig_P2)<<12); var1 = (((((BMP280_S64_t)1)<<47)+var1))*((BMP280_S64_t)dig_P1)>>33; if (var1 == 0) { return 0; // avoid exception caused by division by zero } p = 1048576-adc_P; p = (((p<<31)-var2)*3125)/var1; 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.0f; }