Bremen Team - Hangar / SML2

Implemented first Hangar-Service

Dependencies:   CalibrateMagneto QuaternionMath

Fork of SML2 by TobyRich GmbH

Barometer.cpp@16:3e2468d4f4c1, 2015-03-20 (annotated)

Committer:
pvaibhav
Date:
Fri Mar 20 10:30:01 2015 +0000
Revision:
16:3e2468d4f4c1
Parent:
4:e759b8c756da
Child:
19:9e9753b87cfe
Barometer driver converted to new Sensor infrastructure

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 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 16:3e2468d4f4c1 52 const float centimeter = (pressureToAltitude(pa) * 100.0) - startingAltitude;
pvaibhav 16:3e2468d4f4c1 53 if (startingAltitude == 0)
pvaibhav 16:3e2468d4f4c1 54 startingAltitude = centimeter;
pvaibhav 16:3e2468d4f4c1 55
pvaibhav 16:3e2468d4f4c1 56 return Vector3(celsius, pa, centimeter);
pvaibhav 16:3e2468d4f4c1 57 }
pvaibhav 16:3e2468d4f4c1 58
pvaibhav 16:3e2468d4f4c1 59 float Barometer::pressureToAltitude(const float pa) const {
pvaibhav 16:3e2468d4f4c1 60 return -44330.7692 * (pow(pa * 0.0000098692, 0.1902632365) - 1);
pvaibhav 2:3898208e02da 61 }
pvaibhav 2:3898208e02da 62
pvaibhav 2:3898208e02da 63 void Barometer::bmp280_read_calibration()
pvaibhav 2:3898208e02da 64 {
pvaibhav 16:3e2468d4f4c1 65 struct {
pvaibhav 16:3e2468d4f4c1 66 uint16_t dig_T1;
pvaibhav 16:3e2468d4f4c1 67 int16_t dig_T2;
pvaibhav 16:3e2468d4f4c1 68 int16_t dig_T3;
pvaibhav 16:3e2468d4f4c1 69 uint16_t dig_P1;
pvaibhav 16:3e2468d4f4c1 70 int16_t dig_P2;
pvaibhav 16:3e2468d4f4c1 71 int16_t dig_P3;
pvaibhav 16:3e2468d4f4c1 72 int16_t dig_P4;
pvaibhav 16:3e2468d4f4c1 73 int16_t dig_P5;
pvaibhav 16:3e2468d4f4c1 74 int16_t dig_P6;
pvaibhav 16:3e2468d4f4c1 75 int16_t dig_P7;
pvaibhav 16:3e2468d4f4c1 76 int16_t dig_P8;
pvaibhav 16:3e2468d4f4c1 77 int16_t dig_P9;
pvaibhav 16:3e2468d4f4c1 78 } cal_data;
pvaibhav 16:3e2468d4f4c1 79
pvaibhav 16:3e2468d4f4c1 80 read_reg(0x88, (uint8_t*)&cal_data, sizeof cal_data);
pvaibhav 16:3e2468d4f4c1 81
pvaibhav 16:3e2468d4f4c1 82 dig_T1 = cal_data.dig_T1;
pvaibhav 16:3e2468d4f4c1 83 dig_T2 = cal_data.dig_T2;
pvaibhav 16:3e2468d4f4c1 84 dig_T3 = cal_data.dig_T3;
pvaibhav 16:3e2468d4f4c1 85 dig_P1 = cal_data.dig_P1;
pvaibhav 16:3e2468d4f4c1 86 dig_P2 = cal_data.dig_P2;
pvaibhav 16:3e2468d4f4c1 87 dig_P3 = cal_data.dig_P3;
pvaibhav 16:3e2468d4f4c1 88 dig_P4 = cal_data.dig_P4;
pvaibhav 16:3e2468d4f4c1 89 dig_P5 = cal_data.dig_P5;
pvaibhav 16:3e2468d4f4c1 90 dig_P6 = cal_data.dig_P6;
pvaibhav 16:3e2468d4f4c1 91 dig_P7 = cal_data.dig_P7;
pvaibhav 16:3e2468d4f4c1 92 dig_P8 = cal_data.dig_P8;
pvaibhav 16:3e2468d4f4c1 93 dig_P9 = cal_data.dig_P9;
pvaibhav 16:3e2468d4f4c1 94
pvaibhav 2:3898208e02da 95 LOG("Calibration parameters: T=[%u, %d, %d] P=[%u, %d, %d, %d, %d, %d, %d, %d, %d]",
pvaibhav 3:ee90a9ada112 96 dig_T1, dig_T2, dig_T3,
pvaibhav 3:ee90a9ada112 97 dig_P1, dig_P2, dig_P3, dig_P4, dig_P5, dig_P6, dig_P7, dig_P8, dig_P9);
pvaibhav 2:3898208e02da 98 }
pvaibhav 2:3898208e02da 99
pvaibhav 3:ee90a9ada112 100 // Returns temperature in DegC, resolution is 0.01 DegC. Output value of “5123” equals 51.23 DegC.
pvaibhav 16:3e2468d4f4c1 101 // XXX: converted to return float result directly
pvaibhav 16:3e2468d4f4c1 102 float Barometer::bmp280_val_to_temp(BMP280_S32_t adc_T)
pvaibhav 2:3898208e02da 103 {
pvaibhav 3:ee90a9ada112 104 BMP280_S32_t var1, var2, T;
pvaibhav 3:ee90a9ada112 105 var1 = ((((adc_T>>3) - ((BMP280_S32_t)dig_T1<<1))) * ((BMP280_S32_t)dig_T2)) >> 11;
pvaibhav 3:ee90a9ada112 106 var2 = (((((adc_T>>4) - ((BMP280_S32_t)dig_T1)) * ((adc_T>>4) - ((BMP280_S32_t)dig_T1))) >> 12) *
pvaibhav 3:ee90a9ada112 107 ((BMP280_S32_t)dig_T3)) >> 14;
pvaibhav 3:ee90a9ada112 108 t_fine = var1 + var2;
pvaibhav 3:ee90a9ada112 109 T =(t_fine*5+128)>>8;
pvaibhav 16:3e2468d4f4c1 110 return T / 100.0f;
pvaibhav 3:ee90a9ada112 111 }
pvaibhav 3:ee90a9ada112 112
pvaibhav 3:ee90a9ada112 113 // Returns pressure in Pa as unsigned 32 bit integer in Q24.8 format (24 integer bits and 8 fractional bits).
pvaibhav 3:ee90a9ada112 114 // Output value of “24674867” represents 24674867/256 = 96386.2 Pa = 963.862 hPa
pvaibhav 16:3e2468d4f4c1 115 // XXX: converted it to return a float directly.
pvaibhav 16:3e2468d4f4c1 116 // XXX: uses t_fine, so call temperature conversion BEFORE calling this.
pvaibhav 16:3e2468d4f4c1 117 float Barometer::bmp280_val_to_pa(BMP280_S32_t adc_P)
pvaibhav 3:ee90a9ada112 118 {
pvaibhav 3:ee90a9ada112 119 BMP280_S64_t var1, var2, p;
pvaibhav 3:ee90a9ada112 120 var1 = ((BMP280_S64_t)t_fine) - 128000;
pvaibhav 3:ee90a9ada112 121 var2 = var1 * var1 * (BMP280_S64_t)dig_P6;
pvaibhav 3:ee90a9ada112 122 var2 = var2 + ((var1*(BMP280_S64_t)dig_P5)<<17);
pvaibhav 3:ee90a9ada112 123 var2 = var2 + (((BMP280_S64_t)dig_P4)<<35);
pvaibhav 3:ee90a9ada112 124 var1 = ((var1 * var1 * (BMP280_S64_t)dig_P3)>>8) + ((var1 * (BMP280_S64_t)dig_P2)<<12);
pvaibhav 3:ee90a9ada112 125 var1 = (((((BMP280_S64_t)1)<<47)+var1))*((BMP280_S64_t)dig_P1)>>33;
pvaibhav 3:ee90a9ada112 126 if (var1 == 0) {
pvaibhav 2:3898208e02da 127 return 0; // avoid exception caused by division by zero
pvaibhav 2:3898208e02da 128 }
pvaibhav 3:ee90a9ada112 129 p = 1048576-adc_P;
pvaibhav 3:ee90a9ada112 130 p = (((p<<31)-var2)*3125)/var1;
pvaibhav 3:ee90a9ada112 131 var1 = (((BMP280_S64_t)dig_P9) * (p>>13) * (p>>13)) >> 25;
pvaibhav 3:ee90a9ada112 132 var2 = (((BMP280_S64_t)dig_P8) * p) >> 19;
pvaibhav 3:ee90a9ada112 133 p = ((p + var1 + var2) >> 8) + (((BMP280_S64_t)dig_P7)<<4);
pvaibhav 16:3e2468d4f4c1 134 return ((BMP280_U32_t)p) / 256.0f;
pvaibhav 2:3898208e02da 135 }