Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Diff: Barometer.cpp
- Revision:
- 3:ee90a9ada112
- Parent:
- 2:3898208e02da
- Child:
- 4:e759b8c756da
--- a/Barometer.cpp Wed Jan 14 17:57:11 2015 +0000 +++ b/Barometer.cpp Wed Jan 21 10:34:45 2015 +0000 @@ -37,8 +37,8 @@ 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); + 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 { @@ -58,7 +58,7 @@ 8.7, // 2x 13.3, // 4x 22.5, // 8x - 43.2, // 16x + 50, // 16x }; enum Filtering { @@ -85,30 +85,49 @@ 0x01 /* force reading mode */); // wait until it's done - wait_ms(waitTime_ms[psrovr]); // XXX: what does this mean for BLE? + //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 +// These typedefs are for Bosch's conversion algorithms below +typedef uint32_t BMP280_U32_t; typedef int32_t BMP280_S32_t; +typedef int64_t BMP280_S64_t; + +// Returns temperature in DegC, resolution is 0.01 DegC. Output value of “5123” equals 51.23 DegC. +// t_fine carries fine temperature as global value static BMP280_S32_t t_fine; -static double bmp280_val_to_Pa(BMP280_S32_t adc_P) + +double bmp280_val_to_temp(BMP280_S32_t adc_T) { - 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) { + BMP280_S32_t var1, var2, T; + var1 = ((((adc_T>>3) - ((BMP280_S32_t)dig_T1<<1))) * ((BMP280_S32_t)dig_T2)) >> 11; + var2 = (((((adc_T>>4) - ((BMP280_S32_t)dig_T1)) * ((adc_T>>4) - ((BMP280_S32_t)dig_T1))) >> 12) * + ((BMP280_S32_t)dig_T3)) >> 14; + t_fine = var1 + var2; + T =(t_fine*5+128)>>8; + return T / 100.0; +} + +// Returns pressure in Pa as unsigned 32 bit integer in Q24.8 format (24 integer bits and 8 fractional bits). +// Output value of “24674867” represents 24674867/256 = 96386.2 Pa = 963.862 hPa +double bmp280_val_to_pa(BMP280_S32_t adc_P) +{ + BMP280_S64_t var1, var2, p; + var1 = ((BMP280_S64_t)t_fine) - 128000; + var2 = var1 * var1 * (BMP280_S64_t)dig_P6; + var2 = var2 + ((var1*(BMP280_S64_t)dig_P5)<<17); + var2 = var2 + (((BMP280_S64_t)dig_P4)<<35); + var1 = ((var1 * var1 * (BMP280_S64_t)dig_P3)>>8) + ((var1 * (BMP280_S64_t)dig_P2)<<12); + var1 = (((((BMP280_S64_t)1)<<47)+var1))*((BMP280_S64_t)dig_P1)>>33; + if (var1 == 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; + p = 1048576-adc_P; + p = (((p<<31)-var2)*3125)/var1; + var1 = (((BMP280_S64_t)dig_P9) * (p>>13) * (p>>13)) >> 25; + var2 = (((BMP280_S64_t)dig_P8) * p) >> 19; + p = ((p + var1 + var2) >> 8) + (((BMP280_S64_t)dig_P7)<<4); + return ((BMP280_U32_t)p) / 256.0; } Barometer::Barometer(I2C &i2c) : I2CPeripheral(i2c, 0xEC /* address */) @@ -131,15 +150,27 @@ 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; + const uint32_t val = (msb << 12) | (lsb << 4) | ((xlsb & 0xF0) >> 4); + return bmp280_val_to_pa(val) / 100.0; } -double Barometer::getHeightFromPressure(const double p) { - const double R = 287.05; //general gas constant +double Barometer::getTemperature() +{ + takeMeasurement(kOversample_1x, kSkip); + const uint8_t msb = read_reg(0xFA); + const uint8_t lsb = read_reg(0xFB); + const uint8_t xlsb = read_reg(0xFC); + const uint32_t val = (msb << 12) | (lsb << 4) | ((xlsb & 0xF0) >> 4); + return bmp280_val_to_temp(val); +} + +double Barometer::getAltitude() +{ + 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 T = 297.6; // supposed to be average temperature between p and p0 const double p0 = 1000.0; // hPa sea level + const double p = getPressure(); const double h = (R / g) * T * log(p0 / p); return h; } \ No newline at end of file