Bremen Team - Hangar / SML2

Implemented first Hangar-Service

Dependencies:   CalibrateMagneto QuaternionMath

Fork of SML2 by TobyRich GmbH

Barometer.cpp@46:fd5a62296b12, 2015-05-27 (annotated)

Committer:
pvaibhav
Date:
Wed May 27 13:01:43 2015 +0000
Revision:
46:fd5a62296b12
Parent:
40:8e852115fe55 
Code reformatted

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 16:3e2468d4f4c1 4 #include <cmath>
pvaibhav 1:c279bc3af90c 5
pvaibhav 25:abb0f208e6a9 6 Barometer::Barometer(I2C &i2c) : I2CPeripheral(i2c, 0xEC /* address */)
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 46:fd5a62296b12 17 bool Barometer::powerOn()
pvaibhav 46:fd5a62296b12 18 {
pvaibhav 16:3e2468d4f4c1 19 write_reg(0xE0, 0xB6); // reset
pvaibhav 16:3e2468d4f4c1 20 wait_ms(2); // cf. datasheet page 8, t_startup
pvaibhav 16:3e2468d4f4c1 21 return read_reg(0xD0) == 0x58; // verify chip ID
pvaibhav 16:3e2468d4f4c1 22 }
pvaibhav 16:3e2468d4f4c1 23
pvaibhav 46:fd5a62296b12 24 void Barometer::powerOff()
pvaibhav 46:fd5a62296b12 25 {
pvaibhav 16:3e2468d4f4c1 26 // nothing to do
pvaibhav 16:3e2468d4f4c1 27 }
pvaibhav 16:3e2468d4f4c1 28
pvaibhav 46:fd5a62296b12 29 void Barometer::start()
pvaibhav 46:fd5a62296b12 30 {
pvaibhav 25:abb0f208e6a9 31 // reset our initial calibration
pvaibhav 25:abb0f208e6a9 32 nsamples = 0;
pvaibhav 25:abb0f208e6a9 33 sum = 0;
pvaibhav 25:abb0f208e6a9 34 avg = 0;
pvaibhav 46:fd5a62296b12 35
pvaibhav 16:3e2468d4f4c1 36 // set parameters for Bosch-recommended "Indoor navigation" preset
pvaibhav 16:3e2468d4f4c1 37 write_reg(0xF5, 0x10); // 0.5ms t_standby, IIR coefficient=16
pvaibhav 16:3e2468d4f4c1 38 write_reg(0xF4, 0x57); // 2x oversampling for temperature, 16x for pressure and power mode "normal"
pvaibhav 16:3e2468d4f4c1 39 }
pvaibhav 16:3e2468d4f4c1 40
pvaibhav 46:fd5a62296b12 41 void Barometer::stop()
pvaibhav 46:fd5a62296b12 42 {
pvaibhav 16:3e2468d4f4c1 43 write_reg(0xF4, 0x54); // keep the oversampling settings but set power mode to "sleep"
pvaibhav 16:3e2468d4f4c1 44 }
pvaibhav 2:3898208e02da 45
pvaibhav 46:fd5a62296b12 46 Vector3 Barometer::read()
pvaibhav 46:fd5a62296b12 47 {
pvaibhav 16:3e2468d4f4c1 48 uint8_t buffer[6];
pvaibhav 40:8e852115fe55 49 /*
pvaibhav 16:3e2468d4f4c1 50 for (int i = 0; i < 6; i++)
pvaibhav 16:3e2468d4f4c1 51 buffer[i] = read_reg(0xF7 + i);
pvaibhav 40:8e852115fe55 52 */
pvaibhav 46:fd5a62296b12 53
pvaibhav 40:8e852115fe55 54 read_reg(0xF7, buffer, sizeof buffer);
pvaibhav 46:fd5a62296b12 55
pvaibhav 16:3e2468d4f4c1 56 const uint32_t adc_P = ((buffer[0] << 16) | (buffer[1] << 8) | buffer[2]) >> 4;
pvaibhav 16:3e2468d4f4c1 57 const uint32_t adc_T = ((buffer[3] << 16) | (buffer[4] << 8) | buffer[5]) >> 4;
pvaibhav 46:fd5a62296b12 58
pvaibhav 16:3e2468d4f4c1 59 const float celsius = bmp280_val_to_temp(adc_T) - 20; // 20 degree offset (?)
pvaibhav 25:abb0f208e6a9 60 const float pa = bmp280_val_to_pa(adc_P);
pvaibhav 19:9e9753b87cfe 61 const float centimeter = pressureToAltitude(pa) * 100.0;
pvaibhav 46:fd5a62296b12 62
pvaibhav 25:abb0f208e6a9 63 if (++nsamples < 10) {
pvaibhav 25:abb0f208e6a9 64 sum += centimeter;
pvaibhav 25:abb0f208e6a9 65 avg = sum / nsamples;
pvaibhav 25:abb0f208e6a9 66 }
pvaibhav 46:fd5a62296b12 67
pvaibhav 25:abb0f208e6a9 68 return Vector3(celsius, pa, centimeter - avg);
pvaibhav 16:3e2468d4f4c1 69 }
pvaibhav 16:3e2468d4f4c1 70
pvaibhav 46:fd5a62296b12 71 float Barometer::pressureToAltitude(const float pa) const
pvaibhav 46:fd5a62296b12 72 {
pvaibhav 16:3e2468d4f4c1 73 return -44330.7692 * (pow(pa * 0.0000098692, 0.1902632365) - 1);
pvaibhav 2:3898208e02da 74 }
pvaibhav 2:3898208e02da 75
pvaibhav 2:3898208e02da 76 void Barometer::bmp280_read_calibration()
pvaibhav 2:3898208e02da 77 {
pvaibhav 16:3e2468d4f4c1 78 struct {
pvaibhav 16:3e2468d4f4c1 79 uint16_t dig_T1;
pvaibhav 16:3e2468d4f4c1 80 int16_t dig_T2;
pvaibhav 16:3e2468d4f4c1 81 int16_t dig_T3;
pvaibhav 16:3e2468d4f4c1 82 uint16_t dig_P1;
pvaibhav 16:3e2468d4f4c1 83 int16_t dig_P2;
pvaibhav 16:3e2468d4f4c1 84 int16_t dig_P3;
pvaibhav 16:3e2468d4f4c1 85 int16_t dig_P4;
pvaibhav 16:3e2468d4f4c1 86 int16_t dig_P5;
pvaibhav 16:3e2468d4f4c1 87 int16_t dig_P6;
pvaibhav 16:3e2468d4f4c1 88 int16_t dig_P7;
pvaibhav 16:3e2468d4f4c1 89 int16_t dig_P8;
pvaibhav 16:3e2468d4f4c1 90 int16_t dig_P9;
pvaibhav 16:3e2468d4f4c1 91 } cal_data;
pvaibhav 46:fd5a62296b12 92
pvaibhav 16:3e2468d4f4c1 93 read_reg(0x88, (uint8_t*)&cal_data, sizeof cal_data);
pvaibhav 46:fd5a62296b12 94
pvaibhav 16:3e2468d4f4c1 95 dig_T1 = cal_data.dig_T1;
pvaibhav 16:3e2468d4f4c1 96 dig_T2 = cal_data.dig_T2;
pvaibhav 16:3e2468d4f4c1 97 dig_T3 = cal_data.dig_T3;
pvaibhav 16:3e2468d4f4c1 98 dig_P1 = cal_data.dig_P1;
pvaibhav 16:3e2468d4f4c1 99 dig_P2 = cal_data.dig_P2;
pvaibhav 16:3e2468d4f4c1 100 dig_P3 = cal_data.dig_P3;
pvaibhav 16:3e2468d4f4c1 101 dig_P4 = cal_data.dig_P4;
pvaibhav 16:3e2468d4f4c1 102 dig_P5 = cal_data.dig_P5;
pvaibhav 16:3e2468d4f4c1 103 dig_P6 = cal_data.dig_P6;
pvaibhav 16:3e2468d4f4c1 104 dig_P7 = cal_data.dig_P7;
pvaibhav 16:3e2468d4f4c1 105 dig_P8 = cal_data.dig_P8;
pvaibhav 16:3e2468d4f4c1 106 dig_P9 = cal_data.dig_P9;
pvaibhav 16:3e2468d4f4c1 107
pvaibhav 2:3898208e02da 108 LOG("Calibration parameters: T=[%u, %d, %d] P=[%u, %d, %d, %d, %d, %d, %d, %d, %d]",
pvaibhav 3:ee90a9ada112 109 dig_T1, dig_T2, dig_T3,
pvaibhav 3:ee90a9ada112 110 dig_P1, dig_P2, dig_P3, dig_P4, dig_P5, dig_P6, dig_P7, dig_P8, dig_P9);
pvaibhav 2:3898208e02da 111 }
pvaibhav 2:3898208e02da 112
pvaibhav 3:ee90a9ada112 113 // Returns temperature in DegC, resolution is 0.01 DegC. Output value of “5123” equals 51.23 DegC.
pvaibhav 16:3e2468d4f4c1 114 // XXX: converted to return float result directly
pvaibhav 16:3e2468d4f4c1 115 float Barometer::bmp280_val_to_temp(BMP280_S32_t adc_T)
pvaibhav 2:3898208e02da 116 {
pvaibhav 3:ee90a9ada112 117 BMP280_S32_t var1, var2, T;
pvaibhav 3:ee90a9ada112 118 var1 = ((((adc_T>>3) - ((BMP280_S32_t)dig_T1<<1))) * ((BMP280_S32_t)dig_T2)) >> 11;
pvaibhav 3:ee90a9ada112 119 var2 = (((((adc_T>>4) - ((BMP280_S32_t)dig_T1)) * ((adc_T>>4) - ((BMP280_S32_t)dig_T1))) >> 12) *
pvaibhav 3:ee90a9ada112 120 ((BMP280_S32_t)dig_T3)) >> 14;
pvaibhav 3:ee90a9ada112 121 t_fine = var1 + var2;
pvaibhav 3:ee90a9ada112 122 T =(t_fine*5+128)>>8;
pvaibhav 16:3e2468d4f4c1 123 return T / 100.0f;
pvaibhav 3:ee90a9ada112 124 }
pvaibhav 3:ee90a9ada112 125
pvaibhav 3:ee90a9ada112 126 // Returns pressure in Pa as unsigned 32 bit integer in Q24.8 format (24 integer bits and 8 fractional bits).
pvaibhav 3:ee90a9ada112 127 // Output value of “24674867” represents 24674867/256 = 96386.2 Pa = 963.862 hPa
pvaibhav 16:3e2468d4f4c1 128 // XXX: converted it to return a float directly.
pvaibhav 16:3e2468d4f4c1 129 // XXX: uses t_fine, so call temperature conversion BEFORE calling this.
pvaibhav 16:3e2468d4f4c1 130 float Barometer::bmp280_val_to_pa(BMP280_S32_t adc_P)
pvaibhav 3:ee90a9ada112 131 {
pvaibhav 3:ee90a9ada112 132 BMP280_S64_t var1, var2, p;
pvaibhav 3:ee90a9ada112 133 var1 = ((BMP280_S64_t)t_fine) - 128000;
pvaibhav 3:ee90a9ada112 134 var2 = var1 * var1 * (BMP280_S64_t)dig_P6;
pvaibhav 3:ee90a9ada112 135 var2 = var2 + ((var1*(BMP280_S64_t)dig_P5)<<17);
pvaibhav 3:ee90a9ada112 136 var2 = var2 + (((BMP280_S64_t)dig_P4)<<35);
pvaibhav 3:ee90a9ada112 137 var1 = ((var1 * var1 * (BMP280_S64_t)dig_P3)>>8) + ((var1 * (BMP280_S64_t)dig_P2)<<12);
pvaibhav 3:ee90a9ada112 138 var1 = (((((BMP280_S64_t)1)<<47)+var1))*((BMP280_S64_t)dig_P1)>>33;
pvaibhav 3:ee90a9ada112 139 if (var1 == 0) {
pvaibhav 2:3898208e02da 140 return 0; // avoid exception caused by division by zero
pvaibhav 2:3898208e02da 141 }
pvaibhav 3:ee90a9ada112 142 p = 1048576-adc_P;
pvaibhav 3:ee90a9ada112 143 p = (((p<<31)-var2)*3125)/var1;
pvaibhav 3:ee90a9ada112 144 var1 = (((BMP280_S64_t)dig_P9) * (p>>13) * (p>>13)) >> 25;
pvaibhav 3:ee90a9ada112 145 var2 = (((BMP280_S64_t)dig_P8) * p) >> 19;
pvaibhav 3:ee90a9ada112 146 p = ((p + var1 + var2) >> 8) + (((BMP280_S64_t)dig_P7)<<4);
pvaibhav 16:3e2468d4f4c1 147 return ((BMP280_U32_t)p) / 256.0f;
pvaibhav 2:3898208e02da 148 }