Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Diff: Barometer.cpp
- Revision:
- 25:abb0f208e6a9
- Parent:
- 19:9e9753b87cfe
- Child:
- 40:8e852115fe55
--- a/Barometer.cpp Mon Apr 13 14:54:35 2015 +0000 +++ b/Barometer.cpp Fri Apr 17 12:08:00 2015 +0000 @@ -3,7 +3,7 @@ #include "Logger.h" #include <cmath> -Barometer::Barometer(I2C &i2c) : I2CPeripheral(i2c, 0xEC /* address */), startingAltitude(0) +Barometer::Barometer(I2C &i2c) : I2CPeripheral(i2c, 0xEC /* address */) { if (powerOn()) { INFO("Bosch Sensortec BMP280 atmospheric pressure sensor found"); @@ -25,8 +25,11 @@ } void Barometer::start() { - startingAltitude = 0; - lastPressure = 102000; + // 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" @@ -45,13 +48,15 @@ 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 pa = bmp280_val_to_pa(adc_P); const float centimeter = pressureToAltitude(pa) * 100.0; - return Vector3(celsius, pa, centimeter); + if (++nsamples < 10) { + sum += centimeter; + avg = sum / nsamples; + } + + return Vector3(celsius, pa, centimeter - avg); } float Barometer::pressureToAltitude(const float pa) const {