Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Barometer.cpp@19:9e9753b87cfe, 2015-04-02 (annotated)
- Committer:
- pvaibhav
- Date:
- Thu Apr 02 14:13:01 2015 +0000
- Revision:
- 19:9e9753b87cfe
- Parent:
- 16:3e2468d4f4c1
- Child:
- 25:abb0f208e6a9
SensorFusion beta increased, Barometer does not subtract height offset anymore.
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 | 16:3e2468d4f4c1 | 4 | #include <cmath> |
pvaibhav | 1:c279bc3af90c | 5 | |
pvaibhav | 16:3e2468d4f4c1 | 6 | Barometer::Barometer(I2C &i2c) : I2CPeripheral(i2c, 0xEC /* address */), startingAltitude(0) |
pvaibhav | 4:e759b8c756da | 7 | { |
pvaibhav | 16:3e2468d4f4c1 | 8 | if (powerOn()) { |
pvaibhav | 16:3e2468d4f4c1 | 9 | INFO("Bosch Sensortec BMP280 atmospheric pressure sensor found"); |
pvaibhav | 4:e759b8c756da | 10 | bmp280_read_calibration(); |
pvaibhav | 16:3e2468d4f4c1 | 11 | powerOff(); |
pvaibhav | 4:e759b8c756da | 12 | } else { |
pvaibhav | 16:3e2468d4f4c1 | 13 | WARN("Bosch Sensortec BMP280 atmospheric pressure sensor not found"); |
pvaibhav | 4:e759b8c756da | 14 | } |
pvaibhav | 4:e759b8c756da | 15 | } |
pvaibhav | 4:e759b8c756da | 16 | |
pvaibhav | 16:3e2468d4f4c1 | 17 | bool Barometer::powerOn() { |
pvaibhav | 16:3e2468d4f4c1 | 18 | write_reg(0xE0, 0xB6); // reset |
pvaibhav | 16:3e2468d4f4c1 | 19 | wait_ms(2); // cf. datasheet page 8, t_startup |
pvaibhav | 16:3e2468d4f4c1 | 20 | return read_reg(0xD0) == 0x58; // verify chip ID |
pvaibhav | 16:3e2468d4f4c1 | 21 | } |
pvaibhav | 16:3e2468d4f4c1 | 22 | |
pvaibhav | 16:3e2468d4f4c1 | 23 | void Barometer::powerOff() { |
pvaibhav | 16:3e2468d4f4c1 | 24 | // nothing to do |
pvaibhav | 16:3e2468d4f4c1 | 25 | } |
pvaibhav | 16:3e2468d4f4c1 | 26 | |
pvaibhav | 16:3e2468d4f4c1 | 27 | void Barometer::start() { |
pvaibhav | 16:3e2468d4f4c1 | 28 | startingAltitude = 0; |
pvaibhav | 16:3e2468d4f4c1 | 29 | lastPressure = 102000; |
pvaibhav | 16:3e2468d4f4c1 | 30 | // set parameters for Bosch-recommended "Indoor navigation" preset |
pvaibhav | 16:3e2468d4f4c1 | 31 | write_reg(0xF5, 0x10); // 0.5ms t_standby, IIR coefficient=16 |
pvaibhav | 16:3e2468d4f4c1 | 32 | write_reg(0xF4, 0x57); // 2x oversampling for temperature, 16x for pressure and power mode "normal" |
pvaibhav | 16:3e2468d4f4c1 | 33 | } |
pvaibhav | 16:3e2468d4f4c1 | 34 | |
pvaibhav | 16:3e2468d4f4c1 | 35 | void Barometer::stop() { |
pvaibhav | 16:3e2468d4f4c1 | 36 | write_reg(0xF4, 0x54); // keep the oversampling settings but set power mode to "sleep" |
pvaibhav | 16:3e2468d4f4c1 | 37 | } |
pvaibhav | 2:3898208e02da | 38 | |
pvaibhav | 16:3e2468d4f4c1 | 39 | Vector3 Barometer::read() { |
pvaibhav | 16:3e2468d4f4c1 | 40 | uint8_t buffer[6]; |
pvaibhav | 16:3e2468d4f4c1 | 41 | for (int i = 0; i < 6; i++) |
pvaibhav | 16:3e2468d4f4c1 | 42 | buffer[i] = read_reg(0xF7 + i); |
pvaibhav | 16:3e2468d4f4c1 | 43 | |
pvaibhav | 16:3e2468d4f4c1 | 44 | const uint32_t adc_P = ((buffer[0] << 16) | (buffer[1] << 8) | buffer[2]) >> 4; |
pvaibhav | 16:3e2468d4f4c1 | 45 | const uint32_t adc_T = ((buffer[3] << 16) | (buffer[4] << 8) | buffer[5]) >> 4; |
pvaibhav | 16:3e2468d4f4c1 | 46 | |
pvaibhav | 16:3e2468d4f4c1 | 47 | const float celsius = bmp280_val_to_temp(adc_T) - 20; // 20 degree offset (?) |
pvaibhav | 16:3e2468d4f4c1 | 48 | const float A = 0.5; |
pvaibhav | 16:3e2468d4f4c1 | 49 | const float pa = A * bmp280_val_to_pa(adc_P) + (1-A)*lastPressure; |
pvaibhav | 16:3e2468d4f4c1 | 50 | lastPressure = pa; |
pvaibhav | 16:3e2468d4f4c1 | 51 | |
pvaibhav | 19:9e9753b87cfe | 52 | const float centimeter = pressureToAltitude(pa) * 100.0; |
pvaibhav | 16:3e2468d4f4c1 | 53 | |
pvaibhav | 16:3e2468d4f4c1 | 54 | return Vector3(celsius, pa, centimeter); |
pvaibhav | 16:3e2468d4f4c1 | 55 | } |
pvaibhav | 16:3e2468d4f4c1 | 56 | |
pvaibhav | 16:3e2468d4f4c1 | 57 | float Barometer::pressureToAltitude(const float pa) const { |
pvaibhav | 16:3e2468d4f4c1 | 58 | return -44330.7692 * (pow(pa * 0.0000098692, 0.1902632365) - 1); |
pvaibhav | 2:3898208e02da | 59 | } |
pvaibhav | 2:3898208e02da | 60 | |
pvaibhav | 2:3898208e02da | 61 | void Barometer::bmp280_read_calibration() |
pvaibhav | 2:3898208e02da | 62 | { |
pvaibhav | 16:3e2468d4f4c1 | 63 | struct { |
pvaibhav | 16:3e2468d4f4c1 | 64 | uint16_t dig_T1; |
pvaibhav | 16:3e2468d4f4c1 | 65 | int16_t dig_T2; |
pvaibhav | 16:3e2468d4f4c1 | 66 | int16_t dig_T3; |
pvaibhav | 16:3e2468d4f4c1 | 67 | uint16_t dig_P1; |
pvaibhav | 16:3e2468d4f4c1 | 68 | int16_t dig_P2; |
pvaibhav | 16:3e2468d4f4c1 | 69 | int16_t dig_P3; |
pvaibhav | 16:3e2468d4f4c1 | 70 | int16_t dig_P4; |
pvaibhav | 16:3e2468d4f4c1 | 71 | int16_t dig_P5; |
pvaibhav | 16:3e2468d4f4c1 | 72 | int16_t dig_P6; |
pvaibhav | 16:3e2468d4f4c1 | 73 | int16_t dig_P7; |
pvaibhav | 16:3e2468d4f4c1 | 74 | int16_t dig_P8; |
pvaibhav | 16:3e2468d4f4c1 | 75 | int16_t dig_P9; |
pvaibhav | 16:3e2468d4f4c1 | 76 | } cal_data; |
pvaibhav | 16:3e2468d4f4c1 | 77 | |
pvaibhav | 16:3e2468d4f4c1 | 78 | read_reg(0x88, (uint8_t*)&cal_data, sizeof cal_data); |
pvaibhav | 16:3e2468d4f4c1 | 79 | |
pvaibhav | 16:3e2468d4f4c1 | 80 | dig_T1 = cal_data.dig_T1; |
pvaibhav | 16:3e2468d4f4c1 | 81 | dig_T2 = cal_data.dig_T2; |
pvaibhav | 16:3e2468d4f4c1 | 82 | dig_T3 = cal_data.dig_T3; |
pvaibhav | 16:3e2468d4f4c1 | 83 | dig_P1 = cal_data.dig_P1; |
pvaibhav | 16:3e2468d4f4c1 | 84 | dig_P2 = cal_data.dig_P2; |
pvaibhav | 16:3e2468d4f4c1 | 85 | dig_P3 = cal_data.dig_P3; |
pvaibhav | 16:3e2468d4f4c1 | 86 | dig_P4 = cal_data.dig_P4; |
pvaibhav | 16:3e2468d4f4c1 | 87 | dig_P5 = cal_data.dig_P5; |
pvaibhav | 16:3e2468d4f4c1 | 88 | dig_P6 = cal_data.dig_P6; |
pvaibhav | 16:3e2468d4f4c1 | 89 | dig_P7 = cal_data.dig_P7; |
pvaibhav | 16:3e2468d4f4c1 | 90 | dig_P8 = cal_data.dig_P8; |
pvaibhav | 16:3e2468d4f4c1 | 91 | dig_P9 = cal_data.dig_P9; |
pvaibhav | 16:3e2468d4f4c1 | 92 | |
pvaibhav | 2:3898208e02da | 93 | LOG("Calibration parameters: T=[%u, %d, %d] P=[%u, %d, %d, %d, %d, %d, %d, %d, %d]", |
pvaibhav | 3:ee90a9ada112 | 94 | dig_T1, dig_T2, dig_T3, |
pvaibhav | 3:ee90a9ada112 | 95 | dig_P1, dig_P2, dig_P3, dig_P4, dig_P5, dig_P6, dig_P7, dig_P8, dig_P9); |
pvaibhav | 2:3898208e02da | 96 | } |
pvaibhav | 2:3898208e02da | 97 | |
pvaibhav | 3:ee90a9ada112 | 98 | // Returns temperature in DegC, resolution is 0.01 DegC. Output value of “5123” equals 51.23 DegC. |
pvaibhav | 16:3e2468d4f4c1 | 99 | // XXX: converted to return float result directly |
pvaibhav | 16:3e2468d4f4c1 | 100 | float Barometer::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 | 16:3e2468d4f4c1 | 108 | return T / 100.0f; |
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 | 16:3e2468d4f4c1 | 113 | // XXX: converted it to return a float directly. |
pvaibhav | 16:3e2468d4f4c1 | 114 | // XXX: uses t_fine, so call temperature conversion BEFORE calling this. |
pvaibhav | 16:3e2468d4f4c1 | 115 | float Barometer::bmp280_val_to_pa(BMP280_S32_t adc_P) |
pvaibhav | 3:ee90a9ada112 | 116 | { |
pvaibhav | 3:ee90a9ada112 | 117 | BMP280_S64_t var1, var2, p; |
pvaibhav | 3:ee90a9ada112 | 118 | var1 = ((BMP280_S64_t)t_fine) - 128000; |
pvaibhav | 3:ee90a9ada112 | 119 | var2 = var1 * var1 * (BMP280_S64_t)dig_P6; |
pvaibhav | 3:ee90a9ada112 | 120 | var2 = var2 + ((var1*(BMP280_S64_t)dig_P5)<<17); |
pvaibhav | 3:ee90a9ada112 | 121 | var2 = var2 + (((BMP280_S64_t)dig_P4)<<35); |
pvaibhav | 3:ee90a9ada112 | 122 | var1 = ((var1 * var1 * (BMP280_S64_t)dig_P3)>>8) + ((var1 * (BMP280_S64_t)dig_P2)<<12); |
pvaibhav | 3:ee90a9ada112 | 123 | var1 = (((((BMP280_S64_t)1)<<47)+var1))*((BMP280_S64_t)dig_P1)>>33; |
pvaibhav | 3:ee90a9ada112 | 124 | if (var1 == 0) { |
pvaibhav | 2:3898208e02da | 125 | return 0; // avoid exception caused by division by zero |
pvaibhav | 2:3898208e02da | 126 | } |
pvaibhav | 3:ee90a9ada112 | 127 | p = 1048576-adc_P; |
pvaibhav | 3:ee90a9ada112 | 128 | p = (((p<<31)-var2)*3125)/var1; |
pvaibhav | 3:ee90a9ada112 | 129 | var1 = (((BMP280_S64_t)dig_P9) * (p>>13) * (p>>13)) >> 25; |
pvaibhav | 3:ee90a9ada112 | 130 | var2 = (((BMP280_S64_t)dig_P8) * p) >> 19; |
pvaibhav | 3:ee90a9ada112 | 131 | p = ((p + var1 + var2) >> 8) + (((BMP280_S64_t)dig_P7)<<4); |
pvaibhav | 16:3e2468d4f4c1 | 132 | return ((BMP280_U32_t)p) / 256.0f; |
pvaibhav | 2:3898208e02da | 133 | } |