Implemented first Hangar-Service

Dependencies:   CalibrateMagneto QuaternionMath

Fork of SML2 by TobyRich GmbH

Revision:
2:3898208e02da
Parent:
1:c279bc3af90c
Child:
3:ee90a9ada112
--- a/Barometer.cpp	Wed Jan 14 15:44:34 2015 +0000
+++ b/Barometer.cpp	Wed Jan 14 17:57:11 2015 +0000
@@ -2,17 +2,144 @@
 #define DEBUG "BMP280"
 #include "Logger.h"
 
-Barometer::Barometer(I2C &i2c) : I2CPeripheral(i2c, 0xEC /* address */) {
+// 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;
+
+void Barometer::bmp280_read_cal_reg(const uint8_t reg, char* val)
+{
+    *val = read_reg(reg);
+    *(val + 1) = read_reg(reg + 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);
+    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
+    43.2, // 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?
+}
+
+// Returns pressure in Pa as double. Output value of “96386.2” equals 96386.2 Pa = 963.862 hPa
+typedef int32_t BMP280_S32_t;
+static BMP280_S32_t t_fine;
+static double bmp280_val_to_Pa(BMP280_S32_t adc_P)
+{
+    double var1, var2, p;
+    var1 = ((double)t_fine/2.0) - 64000.0;
+    var2 = var1 * var1 * ((double)dig_P6) / 32768.0;
+    var2 = var2 + var1 * ((double)dig_P5) * 2.0;
+    var2 = (var2/4.0)+(((double)dig_P4) * 65536.0);
+    var1 = (((double)dig_P3) * var1 * var1 / 524288.0 + ((double)dig_P2) * var1) / 524288.0;
+    var1 = (1.0 + var1 / 32768.0)*((double)dig_P1);
+    if (var1 == 0.0) {
+        return 0; // avoid exception caused by division by zero
+    }
+    p = 1048576.0 - (double)adc_P;
+    p = (p - (var2 / 4096.0)) * 6250.0 / var1;
+    var1 = ((double)dig_P9) * p * p / 2147483648.0;
+    var2 = p * ((double)dig_P8) / 32768.0;
+    p = p + (var1 + var2 + ((double)dig_P7)) / 16.0;
+    return p;
+}
+
+Barometer::Barometer(I2C &i2c) : I2CPeripheral(i2c, 0xEC /* address */)
+{
     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) {
+        bmp280_read_calibration();
+        //setFilterCoefficient(kFilter_16x);
         INFO("Bosch Sensortec BMP280 ready");
     } else {
         WARN("Bosch Sensortec BMP280 not found (chip ID=0x%02x, expected=0x58)", chip_id);
     }
 }
 
-float Barometer::getPressure() {
-    
+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;
+    return bmp280_val_to_Pa(val) / 100.0;
+}
+
+double Barometer::getHeightFromPressure(const double p) {
+    const double R = 287.05; //general gas constant
+    const double g = 9.80665; // acceleration due to gravity
+    const double T = 24.0; // temperature
+    const double p0 = 1000.0; // hPa sea level
+    const double h = (R / g) * T * log(p0 / p);
+    return h;
 }
\ No newline at end of file