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 {