Bremen Team - Hangar / SML2

Implemented first Hangar-Service

Dependencies:   CalibrateMagneto QuaternionMath

Fork of SML2 by TobyRich GmbH

Barometer.cpp@40:8e852115fe55, 2015-05-26 (annotated)

Committer:
pvaibhav
Date:
Tue May 26 11:28:37 2015 +0000
Revision:
40:8e852115fe55
Parent:
25:abb0f208e6a9
Child:
46:fd5a62296b12
SensorFusion base class and 6 axis derived class. utils, filter and pid classes moved inside SML2.

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