Bremen Team - Hangar / SML2

Implemented first Hangar-Service

Dependencies:   CalibrateMagneto QuaternionMath

Fork of SML2 by TobyRich GmbH

Barometer.cpp@4:e759b8c756da, 2015-01-23 (annotated)

Committer:
pvaibhav
Date:
Fri Jan 23 13:00:46 2015 +0000
Revision:
4:e759b8c756da
Parent:
3:ee90a9ada112
Child:
16:3e2468d4f4c1
Added stub drivers for magneto and gyro

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pvaibhav 1:c279bc3af90c 1 #include "Barometer.h"
pvaibhav 1:c279bc3af90c 2 #define DEBUG "BMP280"
pvaibhav 1:c279bc3af90c 3 #include "Logger.h"
pvaibhav 1:c279bc3af90c 4
pvaibhav 4:e759b8c756da 5 Barometer::Barometer(I2C &i2c) : I2CPeripheral(i2c, 0xEC /* address */)
pvaibhav 4:e759b8c756da 6 {
pvaibhav 4:e759b8c756da 7 write_reg(0xE0, 0xB6); // reset
pvaibhav 4:e759b8c756da 8 wait_ms(2); // cf. datasheet page 8, t_startup
pvaibhav 4:e759b8c756da 9 const uint8_t chip_id = read_reg(0xD0);
pvaibhav 4:e759b8c756da 10 if (chip_id == 0x58) {
pvaibhav 4:e759b8c756da 11 bmp280_read_calibration();
pvaibhav 4:e759b8c756da 12 //setFilterCoefficient(kFilter_16x);
pvaibhav 4:e759b8c756da 13 INFO("Bosch Sensortec BMP280 ready");
pvaibhav 4:e759b8c756da 14 } else {
pvaibhav 4:e759b8c756da 15 WARN("Bosch Sensortec BMP280 not found (chip ID=0x%02x, expected=0x58)", chip_id);
pvaibhav 4:e759b8c756da 16 }
pvaibhav 4:e759b8c756da 17 }
pvaibhav 4:e759b8c756da 18
pvaibhav 2:3898208e02da 19 // Calibration parameters stored on chip
pvaibhav 2:3898208e02da 20 static uint16_t dig_T1;
pvaibhav 2:3898208e02da 21 static int16_t dig_T2;
pvaibhav 2:3898208e02da 22 static int16_t dig_T3;
pvaibhav 2:3898208e02da 23 static uint16_t dig_P1;
pvaibhav 2:3898208e02da 24 static int16_t dig_P2;
pvaibhav 2:3898208e02da 25 static int16_t dig_P3;
pvaibhav 2:3898208e02da 26 static int16_t dig_P4;
pvaibhav 2:3898208e02da 27 static int16_t dig_P5;
pvaibhav 2:3898208e02da 28 static int16_t dig_P6;
pvaibhav 2:3898208e02da 29 static int16_t dig_P7;
pvaibhav 2:3898208e02da 30 static int16_t dig_P8;
pvaibhav 2:3898208e02da 31 static int16_t dig_P9;
pvaibhav 2:3898208e02da 32
pvaibhav 2:3898208e02da 33 void Barometer::bmp280_read_cal_reg(const uint8_t reg, char* val)
pvaibhav 2:3898208e02da 34 {
pvaibhav 2:3898208e02da 35 *val = read_reg(reg);
pvaibhav 2:3898208e02da 36 *(val + 1) = read_reg(reg + 1);
pvaibhav 2:3898208e02da 37 }
pvaibhav 2:3898208e02da 38
pvaibhav 2:3898208e02da 39 void Barometer::bmp280_read_calibration()
pvaibhav 2:3898208e02da 40 {
pvaibhav 2:3898208e02da 41 bmp280_read_cal_reg(0x88, (char*)&dig_T1);
pvaibhav 2:3898208e02da 42 bmp280_read_cal_reg(0x8A, (char*)&dig_T2);
pvaibhav 2:3898208e02da 43 bmp280_read_cal_reg(0x8C, (char*)&dig_T3);
pvaibhav 2:3898208e02da 44 bmp280_read_cal_reg(0x8E, (char*)&dig_P1);
pvaibhav 2:3898208e02da 45 bmp280_read_cal_reg(0x90, (char*)&dig_P2);
pvaibhav 2:3898208e02da 46 bmp280_read_cal_reg(0x92, (char*)&dig_P3);
pvaibhav 2:3898208e02da 47 bmp280_read_cal_reg(0x94, (char*)&dig_P4);
pvaibhav 2:3898208e02da 48 bmp280_read_cal_reg(0x96, (char*)&dig_P5);
pvaibhav 2:3898208e02da 49 bmp280_read_cal_reg(0x98, (char*)&dig_P6);
pvaibhav 2:3898208e02da 50 bmp280_read_cal_reg(0x9A, (char*)&dig_P7);
pvaibhav 2:3898208e02da 51 bmp280_read_cal_reg(0x9C, (char*)&dig_P8);
pvaibhav 2:3898208e02da 52 bmp280_read_cal_reg(0x9E, (char*)&dig_P9);
pvaibhav 2:3898208e02da 53 LOG("Calibration parameters: T=[%u, %d, %d] P=[%u, %d, %d, %d, %d, %d, %d, %d, %d]",
pvaibhav 3:ee90a9ada112 54 dig_T1, dig_T2, dig_T3,
pvaibhav 3:ee90a9ada112 55 dig_P1, dig_P2, dig_P3, dig_P4, dig_P5, dig_P6, dig_P7, dig_P8, dig_P9);
pvaibhav 2:3898208e02da 56 }
pvaibhav 2:3898208e02da 57
pvaibhav 2:3898208e02da 58 enum Oversampling {
pvaibhav 2:3898208e02da 59 kSkip = 0,
pvaibhav 2:3898208e02da 60 kOversample_1x = 1,
pvaibhav 2:3898208e02da 61 kOversample_2x = 2,
pvaibhav 2:3898208e02da 62 kOversample_4x = 3,
pvaibhav 2:3898208e02da 63 kOversample_8x = 4,
pvaibhav 2:3898208e02da 64 kOversample_16x = 5,
pvaibhav 2:3898208e02da 65 };
pvaibhav 2:3898208e02da 66
pvaibhav 2:3898208e02da 67 // Time taken to read the pressure at a particular oversampling
pvaibhav 2:3898208e02da 68 // cf. page 18
pvaibhav 2:3898208e02da 69 static float waitTime_ms[] = {
pvaibhav 2:3898208e02da 70 0, // skip
pvaibhav 2:3898208e02da 71 6.4, // 1x
pvaibhav 2:3898208e02da 72 8.7, // 2x
pvaibhav 2:3898208e02da 73 13.3, // 4x
pvaibhav 2:3898208e02da 74 22.5, // 8x
pvaibhav 3:ee90a9ada112 75 50, // 16x
pvaibhav 2:3898208e02da 76 };
pvaibhav 2:3898208e02da 77
pvaibhav 2:3898208e02da 78 enum Filtering {
pvaibhav 2:3898208e02da 79 kFilter_None = 0,
pvaibhav 2:3898208e02da 80 kFilter_2x = 1,
pvaibhav 2:3898208e02da 81 kFilter_4x = 2,
pvaibhav 2:3898208e02da 82 kFilter_8x = 3,
pvaibhav 2:3898208e02da 83 kFilter_16x = 4
pvaibhav 2:3898208e02da 84 };
pvaibhav 2:3898208e02da 85
pvaibhav 2:3898208e02da 86 void Barometer::setFilterCoefficient(const uint8_t iir)
pvaibhav 2:3898208e02da 87 {
pvaibhav 2:3898208e02da 88 write_reg(0xF5, (iir & 0x07) << 1);
pvaibhav 2:3898208e02da 89 INFO("Filter coefficient => %dx", 1 << iir);
pvaibhav 2:3898208e02da 90 }
pvaibhav 2:3898208e02da 91
pvaibhav 2:3898208e02da 92 void Barometer::takeMeasurement(const uint8_t tmpovr,
pvaibhav 2:3898208e02da 93 const uint8_t psrovr)
pvaibhav 2:3898208e02da 94 {
pvaibhav 2:3898208e02da 95 // Start a forced measurement
pvaibhav 2:3898208e02da 96 write_reg(0xF4,
pvaibhav 2:3898208e02da 97 ((tmpovr & 0x07) << 5) |
pvaibhav 2:3898208e02da 98 ((psrovr & 0x07) << 2) |
pvaibhav 2:3898208e02da 99 0x01 /* force reading mode */);
pvaibhav 2:3898208e02da 100
pvaibhav 2:3898208e02da 101 // wait until it's done
pvaibhav 3:ee90a9ada112 102 //wait_ms(waitTime_ms[psrovr]); // XXX: what does this mean for BLE?
pvaibhav 2:3898208e02da 103 }
pvaibhav 2:3898208e02da 104
pvaibhav 3:ee90a9ada112 105 // These typedefs are for Bosch's conversion algorithms below
pvaibhav 3:ee90a9ada112 106 typedef uint32_t BMP280_U32_t;
pvaibhav 2:3898208e02da 107 typedef int32_t BMP280_S32_t;
pvaibhav 3:ee90a9ada112 108 typedef int64_t BMP280_S64_t;
pvaibhav 3:ee90a9ada112 109
pvaibhav 3:ee90a9ada112 110 // Returns temperature in DegC, resolution is 0.01 DegC. Output value of “5123” equals 51.23 DegC.
pvaibhav 3:ee90a9ada112 111 // t_fine carries fine temperature as global value
pvaibhav 2:3898208e02da 112 static BMP280_S32_t t_fine;
pvaibhav 3:ee90a9ada112 113
pvaibhav 3:ee90a9ada112 114 double bmp280_val_to_temp(BMP280_S32_t adc_T)
pvaibhav 2:3898208e02da 115 {
pvaibhav 3:ee90a9ada112 116 BMP280_S32_t var1, var2, T;
pvaibhav 3:ee90a9ada112 117 var1 = ((((adc_T>>3) - ((BMP280_S32_t)dig_T1<<1))) * ((BMP280_S32_t)dig_T2)) >> 11;
pvaibhav 3:ee90a9ada112 118 var2 = (((((adc_T>>4) - ((BMP280_S32_t)dig_T1)) * ((adc_T>>4) - ((BMP280_S32_t)dig_T1))) >> 12) *
pvaibhav 3:ee90a9ada112 119 ((BMP280_S32_t)dig_T3)) >> 14;
pvaibhav 3:ee90a9ada112 120 t_fine = var1 + var2;
pvaibhav 3:ee90a9ada112 121 T =(t_fine*5+128)>>8;
pvaibhav 3:ee90a9ada112 122 return T / 100.0;
pvaibhav 3:ee90a9ada112 123 }
pvaibhav 3:ee90a9ada112 124
pvaibhav 3:ee90a9ada112 125 // Returns pressure in Pa as unsigned 32 bit integer in Q24.8 format (24 integer bits and 8 fractional bits).
pvaibhav 3:ee90a9ada112 126 // Output value of “24674867” represents 24674867/256 = 96386.2 Pa = 963.862 hPa
pvaibhav 3:ee90a9ada112 127 double bmp280_val_to_pa(BMP280_S32_t adc_P)
pvaibhav 3:ee90a9ada112 128 {
pvaibhav 3:ee90a9ada112 129 BMP280_S64_t var1, var2, p;
pvaibhav 3:ee90a9ada112 130 var1 = ((BMP280_S64_t)t_fine) - 128000;
pvaibhav 3:ee90a9ada112 131 var2 = var1 * var1 * (BMP280_S64_t)dig_P6;
pvaibhav 3:ee90a9ada112 132 var2 = var2 + ((var1*(BMP280_S64_t)dig_P5)<<17);
pvaibhav 3:ee90a9ada112 133 var2 = var2 + (((BMP280_S64_t)dig_P4)<<35);
pvaibhav 3:ee90a9ada112 134 var1 = ((var1 * var1 * (BMP280_S64_t)dig_P3)>>8) + ((var1 * (BMP280_S64_t)dig_P2)<<12);
pvaibhav 3:ee90a9ada112 135 var1 = (((((BMP280_S64_t)1)<<47)+var1))*((BMP280_S64_t)dig_P1)>>33;
pvaibhav 3:ee90a9ada112 136 if (var1 == 0) {
pvaibhav 2:3898208e02da 137 return 0; // avoid exception caused by division by zero
pvaibhav 2:3898208e02da 138 }
pvaibhav 3:ee90a9ada112 139 p = 1048576-adc_P;
pvaibhav 3:ee90a9ada112 140 p = (((p<<31)-var2)*3125)/var1;
pvaibhav 3:ee90a9ada112 141 var1 = (((BMP280_S64_t)dig_P9) * (p>>13) * (p>>13)) >> 25;
pvaibhav 3:ee90a9ada112 142 var2 = (((BMP280_S64_t)dig_P8) * p) >> 19;
pvaibhav 3:ee90a9ada112 143 p = ((p + var1 + var2) >> 8) + (((BMP280_S64_t)dig_P7)<<4);
pvaibhav 3:ee90a9ada112 144 return ((BMP280_U32_t)p) / 256.0;
pvaibhav 2:3898208e02da 145 }
pvaibhav 2:3898208e02da 146
pvaibhav 2:3898208e02da 147 double Barometer::getPressure()
pvaibhav 2:3898208e02da 148 {
pvaibhav 2:3898208e02da 149 takeMeasurement(kSkip, kOversample_16x);
pvaibhav 2:3898208e02da 150 const uint8_t msb = read_reg(0xF7);
pvaibhav 2:3898208e02da 151 const uint8_t lsb = read_reg(0xF8);
pvaibhav 2:3898208e02da 152 const uint8_t xlsb = read_reg(0xF9);
pvaibhav 3:ee90a9ada112 153 const uint32_t val = (msb << 12) | (lsb << 4) | ((xlsb & 0xF0) >> 4);
pvaibhav 3:ee90a9ada112 154 return bmp280_val_to_pa(val) / 100.0;
pvaibhav 2:3898208e02da 155 }
pvaibhav 2:3898208e02da 156
pvaibhav 3:ee90a9ada112 157 double Barometer::getTemperature()
pvaibhav 3:ee90a9ada112 158 {
pvaibhav 3:ee90a9ada112 159 takeMeasurement(kOversample_1x, kSkip);
pvaibhav 3:ee90a9ada112 160 const uint8_t msb = read_reg(0xFA);
pvaibhav 3:ee90a9ada112 161 const uint8_t lsb = read_reg(0xFB);
pvaibhav 3:ee90a9ada112 162 const uint8_t xlsb = read_reg(0xFC);
pvaibhav 3:ee90a9ada112 163 const uint32_t val = (msb << 12) | (lsb << 4) | ((xlsb & 0xF0) >> 4);
pvaibhav 3:ee90a9ada112 164 return bmp280_val_to_temp(val);
pvaibhav 3:ee90a9ada112 165 }
pvaibhav 3:ee90a9ada112 166
pvaibhav 3:ee90a9ada112 167 double Barometer::getAltitude()
pvaibhav 3:ee90a9ada112 168 {
pvaibhav 3:ee90a9ada112 169 const double R = 287.05; // general gas constant
pvaibhav 2:3898208e02da 170 const double g = 9.80665; // acceleration due to gravity
pvaibhav 3:ee90a9ada112 171 const double T = 297.6; // supposed to be average temperature between p and p0
pvaibhav 2:3898208e02da 172 const double p0 = 1000.0; // hPa sea level
pvaibhav 3:ee90a9ada112 173 const double p = getPressure();
pvaibhav 2:3898208e02da 174 const double h = (R / g) * T * log(p0 / p);
pvaibhav 2:3898208e02da 175 return h;
pvaibhav 1:c279bc3af90c 176 }