Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Barometer.cpp@3:ee90a9ada112, 2015-01-21 (annotated)
- Committer:
- pvaibhav
- Date:
- Wed Jan 21 10:34:45 2015 +0000
- Revision:
- 3:ee90a9ada112
- Parent:
- 2:3898208e02da
- Child:
- 4:e759b8c756da
Motor driver now "stops" the output if voltage is set below 0.48V
Who changed what in which revision?
User | Revision | Line number | New 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 | 2:3898208e02da | 5 | // Calibration parameters stored on chip |
pvaibhav | 2:3898208e02da | 6 | static uint16_t dig_T1; |
pvaibhav | 2:3898208e02da | 7 | static int16_t dig_T2; |
pvaibhav | 2:3898208e02da | 8 | static int16_t dig_T3; |
pvaibhav | 2:3898208e02da | 9 | static uint16_t dig_P1; |
pvaibhav | 2:3898208e02da | 10 | static int16_t dig_P2; |
pvaibhav | 2:3898208e02da | 11 | static int16_t dig_P3; |
pvaibhav | 2:3898208e02da | 12 | static int16_t dig_P4; |
pvaibhav | 2:3898208e02da | 13 | static int16_t dig_P5; |
pvaibhav | 2:3898208e02da | 14 | static int16_t dig_P6; |
pvaibhav | 2:3898208e02da | 15 | static int16_t dig_P7; |
pvaibhav | 2:3898208e02da | 16 | static int16_t dig_P8; |
pvaibhav | 2:3898208e02da | 17 | static int16_t dig_P9; |
pvaibhav | 2:3898208e02da | 18 | |
pvaibhav | 2:3898208e02da | 19 | void Barometer::bmp280_read_cal_reg(const uint8_t reg, char* val) |
pvaibhav | 2:3898208e02da | 20 | { |
pvaibhav | 2:3898208e02da | 21 | *val = read_reg(reg); |
pvaibhav | 2:3898208e02da | 22 | *(val + 1) = read_reg(reg + 1); |
pvaibhav | 2:3898208e02da | 23 | } |
pvaibhav | 2:3898208e02da | 24 | |
pvaibhav | 2:3898208e02da | 25 | void Barometer::bmp280_read_calibration() |
pvaibhav | 2:3898208e02da | 26 | { |
pvaibhav | 2:3898208e02da | 27 | bmp280_read_cal_reg(0x88, (char*)&dig_T1); |
pvaibhav | 2:3898208e02da | 28 | bmp280_read_cal_reg(0x8A, (char*)&dig_T2); |
pvaibhav | 2:3898208e02da | 29 | bmp280_read_cal_reg(0x8C, (char*)&dig_T3); |
pvaibhav | 2:3898208e02da | 30 | bmp280_read_cal_reg(0x8E, (char*)&dig_P1); |
pvaibhav | 2:3898208e02da | 31 | bmp280_read_cal_reg(0x90, (char*)&dig_P2); |
pvaibhav | 2:3898208e02da | 32 | bmp280_read_cal_reg(0x92, (char*)&dig_P3); |
pvaibhav | 2:3898208e02da | 33 | bmp280_read_cal_reg(0x94, (char*)&dig_P4); |
pvaibhav | 2:3898208e02da | 34 | bmp280_read_cal_reg(0x96, (char*)&dig_P5); |
pvaibhav | 2:3898208e02da | 35 | bmp280_read_cal_reg(0x98, (char*)&dig_P6); |
pvaibhav | 2:3898208e02da | 36 | bmp280_read_cal_reg(0x9A, (char*)&dig_P7); |
pvaibhav | 2:3898208e02da | 37 | bmp280_read_cal_reg(0x9C, (char*)&dig_P8); |
pvaibhav | 2:3898208e02da | 38 | bmp280_read_cal_reg(0x9E, (char*)&dig_P9); |
pvaibhav | 2:3898208e02da | 39 | LOG("Calibration parameters: T=[%u, %d, %d] P=[%u, %d, %d, %d, %d, %d, %d, %d, %d]", |
pvaibhav | 3:ee90a9ada112 | 40 | dig_T1, dig_T2, dig_T3, |
pvaibhav | 3:ee90a9ada112 | 41 | dig_P1, dig_P2, dig_P3, dig_P4, dig_P5, dig_P6, dig_P7, dig_P8, dig_P9); |
pvaibhav | 2:3898208e02da | 42 | } |
pvaibhav | 2:3898208e02da | 43 | |
pvaibhav | 2:3898208e02da | 44 | enum Oversampling { |
pvaibhav | 2:3898208e02da | 45 | kSkip = 0, |
pvaibhav | 2:3898208e02da | 46 | kOversample_1x = 1, |
pvaibhav | 2:3898208e02da | 47 | kOversample_2x = 2, |
pvaibhav | 2:3898208e02da | 48 | kOversample_4x = 3, |
pvaibhav | 2:3898208e02da | 49 | kOversample_8x = 4, |
pvaibhav | 2:3898208e02da | 50 | kOversample_16x = 5, |
pvaibhav | 2:3898208e02da | 51 | }; |
pvaibhav | 2:3898208e02da | 52 | |
pvaibhav | 2:3898208e02da | 53 | // Time taken to read the pressure at a particular oversampling |
pvaibhav | 2:3898208e02da | 54 | // cf. page 18 |
pvaibhav | 2:3898208e02da | 55 | static float waitTime_ms[] = { |
pvaibhav | 2:3898208e02da | 56 | 0, // skip |
pvaibhav | 2:3898208e02da | 57 | 6.4, // 1x |
pvaibhav | 2:3898208e02da | 58 | 8.7, // 2x |
pvaibhav | 2:3898208e02da | 59 | 13.3, // 4x |
pvaibhav | 2:3898208e02da | 60 | 22.5, // 8x |
pvaibhav | 3:ee90a9ada112 | 61 | 50, // 16x |
pvaibhav | 2:3898208e02da | 62 | }; |
pvaibhav | 2:3898208e02da | 63 | |
pvaibhav | 2:3898208e02da | 64 | enum Filtering { |
pvaibhav | 2:3898208e02da | 65 | kFilter_None = 0, |
pvaibhav | 2:3898208e02da | 66 | kFilter_2x = 1, |
pvaibhav | 2:3898208e02da | 67 | kFilter_4x = 2, |
pvaibhav | 2:3898208e02da | 68 | kFilter_8x = 3, |
pvaibhav | 2:3898208e02da | 69 | kFilter_16x = 4 |
pvaibhav | 2:3898208e02da | 70 | }; |
pvaibhav | 2:3898208e02da | 71 | |
pvaibhav | 2:3898208e02da | 72 | void Barometer::setFilterCoefficient(const uint8_t iir) |
pvaibhav | 2:3898208e02da | 73 | { |
pvaibhav | 2:3898208e02da | 74 | write_reg(0xF5, (iir & 0x07) << 1); |
pvaibhav | 2:3898208e02da | 75 | INFO("Filter coefficient => %dx", 1 << iir); |
pvaibhav | 2:3898208e02da | 76 | } |
pvaibhav | 2:3898208e02da | 77 | |
pvaibhav | 2:3898208e02da | 78 | void Barometer::takeMeasurement(const uint8_t tmpovr, |
pvaibhav | 2:3898208e02da | 79 | const uint8_t psrovr) |
pvaibhav | 2:3898208e02da | 80 | { |
pvaibhav | 2:3898208e02da | 81 | // Start a forced measurement |
pvaibhav | 2:3898208e02da | 82 | write_reg(0xF4, |
pvaibhav | 2:3898208e02da | 83 | ((tmpovr & 0x07) << 5) | |
pvaibhav | 2:3898208e02da | 84 | ((psrovr & 0x07) << 2) | |
pvaibhav | 2:3898208e02da | 85 | 0x01 /* force reading mode */); |
pvaibhav | 2:3898208e02da | 86 | |
pvaibhav | 2:3898208e02da | 87 | // wait until it's done |
pvaibhav | 3:ee90a9ada112 | 88 | //wait_ms(waitTime_ms[psrovr]); // XXX: what does this mean for BLE? |
pvaibhav | 2:3898208e02da | 89 | } |
pvaibhav | 2:3898208e02da | 90 | |
pvaibhav | 3:ee90a9ada112 | 91 | // These typedefs are for Bosch's conversion algorithms below |
pvaibhav | 3:ee90a9ada112 | 92 | typedef uint32_t BMP280_U32_t; |
pvaibhav | 2:3898208e02da | 93 | typedef int32_t BMP280_S32_t; |
pvaibhav | 3:ee90a9ada112 | 94 | typedef int64_t BMP280_S64_t; |
pvaibhav | 3:ee90a9ada112 | 95 | |
pvaibhav | 3:ee90a9ada112 | 96 | // Returns temperature in DegC, resolution is 0.01 DegC. Output value of “5123” equals 51.23 DegC. |
pvaibhav | 3:ee90a9ada112 | 97 | // t_fine carries fine temperature as global value |
pvaibhav | 2:3898208e02da | 98 | static BMP280_S32_t t_fine; |
pvaibhav | 3:ee90a9ada112 | 99 | |
pvaibhav | 3:ee90a9ada112 | 100 | double bmp280_val_to_temp(BMP280_S32_t adc_T) |
pvaibhav | 2:3898208e02da | 101 | { |
pvaibhav | 3:ee90a9ada112 | 102 | BMP280_S32_t var1, var2, T; |
pvaibhav | 3:ee90a9ada112 | 103 | var1 = ((((adc_T>>3) - ((BMP280_S32_t)dig_T1<<1))) * ((BMP280_S32_t)dig_T2)) >> 11; |
pvaibhav | 3:ee90a9ada112 | 104 | var2 = (((((adc_T>>4) - ((BMP280_S32_t)dig_T1)) * ((adc_T>>4) - ((BMP280_S32_t)dig_T1))) >> 12) * |
pvaibhav | 3:ee90a9ada112 | 105 | ((BMP280_S32_t)dig_T3)) >> 14; |
pvaibhav | 3:ee90a9ada112 | 106 | t_fine = var1 + var2; |
pvaibhav | 3:ee90a9ada112 | 107 | T =(t_fine*5+128)>>8; |
pvaibhav | 3:ee90a9ada112 | 108 | return T / 100.0; |
pvaibhav | 3:ee90a9ada112 | 109 | } |
pvaibhav | 3:ee90a9ada112 | 110 | |
pvaibhav | 3:ee90a9ada112 | 111 | // Returns pressure in Pa as unsigned 32 bit integer in Q24.8 format (24 integer bits and 8 fractional bits). |
pvaibhav | 3:ee90a9ada112 | 112 | // Output value of “24674867” represents 24674867/256 = 96386.2 Pa = 963.862 hPa |
pvaibhav | 3:ee90a9ada112 | 113 | double bmp280_val_to_pa(BMP280_S32_t adc_P) |
pvaibhav | 3:ee90a9ada112 | 114 | { |
pvaibhav | 3:ee90a9ada112 | 115 | BMP280_S64_t var1, var2, p; |
pvaibhav | 3:ee90a9ada112 | 116 | var1 = ((BMP280_S64_t)t_fine) - 128000; |
pvaibhav | 3:ee90a9ada112 | 117 | var2 = var1 * var1 * (BMP280_S64_t)dig_P6; |
pvaibhav | 3:ee90a9ada112 | 118 | var2 = var2 + ((var1*(BMP280_S64_t)dig_P5)<<17); |
pvaibhav | 3:ee90a9ada112 | 119 | var2 = var2 + (((BMP280_S64_t)dig_P4)<<35); |
pvaibhav | 3:ee90a9ada112 | 120 | var1 = ((var1 * var1 * (BMP280_S64_t)dig_P3)>>8) + ((var1 * (BMP280_S64_t)dig_P2)<<12); |
pvaibhav | 3:ee90a9ada112 | 121 | var1 = (((((BMP280_S64_t)1)<<47)+var1))*((BMP280_S64_t)dig_P1)>>33; |
pvaibhav | 3:ee90a9ada112 | 122 | if (var1 == 0) { |
pvaibhav | 2:3898208e02da | 123 | return 0; // avoid exception caused by division by zero |
pvaibhav | 2:3898208e02da | 124 | } |
pvaibhav | 3:ee90a9ada112 | 125 | p = 1048576-adc_P; |
pvaibhav | 3:ee90a9ada112 | 126 | p = (((p<<31)-var2)*3125)/var1; |
pvaibhav | 3:ee90a9ada112 | 127 | var1 = (((BMP280_S64_t)dig_P9) * (p>>13) * (p>>13)) >> 25; |
pvaibhav | 3:ee90a9ada112 | 128 | var2 = (((BMP280_S64_t)dig_P8) * p) >> 19; |
pvaibhav | 3:ee90a9ada112 | 129 | p = ((p + var1 + var2) >> 8) + (((BMP280_S64_t)dig_P7)<<4); |
pvaibhav | 3:ee90a9ada112 | 130 | return ((BMP280_U32_t)p) / 256.0; |
pvaibhav | 2:3898208e02da | 131 | } |
pvaibhav | 2:3898208e02da | 132 | |
pvaibhav | 2:3898208e02da | 133 | Barometer::Barometer(I2C &i2c) : I2CPeripheral(i2c, 0xEC /* address */) |
pvaibhav | 2:3898208e02da | 134 | { |
pvaibhav | 1:c279bc3af90c | 135 | write_reg(0xE0, 0xB6); // reset |
pvaibhav | 1:c279bc3af90c | 136 | wait_ms(2); // cf. datasheet page 8, t_startup |
pvaibhav | 1:c279bc3af90c | 137 | const uint8_t chip_id = read_reg(0xD0); |
pvaibhav | 1:c279bc3af90c | 138 | if (chip_id == 0x58) { |
pvaibhav | 2:3898208e02da | 139 | bmp280_read_calibration(); |
pvaibhav | 2:3898208e02da | 140 | //setFilterCoefficient(kFilter_16x); |
pvaibhav | 1:c279bc3af90c | 141 | INFO("Bosch Sensortec BMP280 ready"); |
pvaibhav | 1:c279bc3af90c | 142 | } else { |
pvaibhav | 1:c279bc3af90c | 143 | WARN("Bosch Sensortec BMP280 not found (chip ID=0x%02x, expected=0x58)", chip_id); |
pvaibhav | 1:c279bc3af90c | 144 | } |
pvaibhav | 1:c279bc3af90c | 145 | } |
pvaibhav | 1:c279bc3af90c | 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 | } |