src/main/drivers/barometer/barometer_bmp280.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20
  21 #include <platform.h>
  22 #include "build/build_config.h"
  23 #include "build/debug.h"
  24 #include "common/utils.h"
  25
  26 #include "drivers/time.h"
  27 #include "drivers/io.h"
  28 #include "drivers/bus.h"
  29 #include "drivers/barometer/barometer.h"
  30 #include "drivers/barometer/barometer_bmp280.h"
  31
  32 #if defined(USE_BARO_BMP280)
  33
  34 // BMP280, address 0x76
  35
  36 typedef struct bmp280_calib_param_s {
  37     uint16_t dig_T1; /* calibration T1 data */
  38     int16_t dig_T2; /* calibration T2 data */
  39     int16_t dig_T3; /* calibration T3 data */
  40     uint16_t dig_P1; /* calibration P1 data */
  41     int16_t dig_P2; /* calibration P2 data */
  42     int16_t dig_P3; /* calibration P3 data */
  43     int16_t dig_P4; /* calibration P4 data */
  44     int16_t dig_P5; /* calibration P5 data */
  45     int16_t dig_P6; /* calibration P6 data */
  46     int16_t dig_P7; /* calibration P7 data */
  47     int16_t dig_P8; /* calibration P8 data */
  48     int16_t dig_P9; /* calibration P9 data */
  49     int32_t t_fine; /* calibration t_fine data */
  50 } bmp280_calib_param_t;
  51
  52 STATIC_UNIT_TESTED bmp280_calib_param_t bmp280_cal;
  53 // uncompensated pressure and temperature
  54 int32_t bmp280_up = 0;
  55 int32_t bmp280_ut = 0;
  56
  57 static bool bmp280_start_ut(baroDev_t * baro)
  58 {
  59     UNUSED(baro);
  60     return true;
  61 }
  62
  63 static bool bmp280_get_ut(baroDev_t * baro)
  64 {
  65     UNUSED(baro);
  66     return true;
  67 }
  68
  69 static bool bmp280_start_up(baroDev_t * baro)
  70 {
  71     // start measurement
  72     // set oversampling + power mode (forced), and start sampling
  73     busWrite(baro->busDev, BMP280_CTRL_MEAS_REG, BMP280_MODE);
  74     return true;
  75 }
  76
  77 static bool bmp280_get_up(baroDev_t * baro)
  78 {
  79     uint8_t data[BMP280_DATA_FRAME_SIZE];
  80
  81     //error free measurements
  82     static int32_t bmp280_up_valid;
  83     static int32_t bmp280_ut_valid;
  84
  85     //read data from sensor
  86     bool ack = busReadBuf(baro->busDev, BMP280_PRESSURE_MSB_REG, data, BMP280_DATA_FRAME_SIZE);
  87
  88     //check if pressure and temperature readings are valid, otherwise use previous measurements from the moment
  89     if (ack) {
  90         bmp280_up = (int32_t)((((uint32_t)(data[0])) << 12) | (((uint32_t)(data[1])) << 4) | ((uint32_t)data[2] >> 4));
  91         bmp280_ut = (int32_t)((((uint32_t)(data[3])) << 12) | (((uint32_t)(data[4])) << 4) | ((uint32_t)data[5] >> 4));
  92         bmp280_up_valid = bmp280_up;
  93         bmp280_ut_valid = bmp280_ut;
  94     }
  95     else {
  96         //assign previous valid measurements
  97         bmp280_up = bmp280_up_valid;
  98         bmp280_ut = bmp280_ut_valid;
  99     }
 100
 101     return ack;
 102 }
 103
 104 // Returns temperature in DegC, resolution is 0.01 DegC. Output value of "5123" equals 51.23 DegC
 105 // t_fine carries fine temperature as global value
 106 static int32_t bmp280_compensate_T(int32_t adc_T)
 107 {
 108     int32_t var1, var2, T;
 109
 110     var1 = ((((adc_T >> 3) - ((int32_t)bmp280_cal.dig_T1 << 1))) * ((int32_t)bmp280_cal.dig_T2)) >> 11;
 111     var2  = (((((adc_T >> 4) - ((int32_t)bmp280_cal.dig_T1)) * ((adc_T >> 4) - ((int32_t)bmp280_cal.dig_T1))) >> 12) * ((int32_t)bmp280_cal.dig_T3)) >> 14;
 112     bmp280_cal.t_fine = var1 + var2;
 113     T = (bmp280_cal.t_fine * 5 + 128) >> 8;
 114
 115     return T;
 116 }
 117
 118 // Returns pressure in Pa as unsigned 32 bit integer in Q24.8 format (24 integer bits and 8 fractional bits).
 119 // Output value of "24674867" represents 24674867/256 = 96386.2 Pa = 963.862 hPa
 120 static uint32_t bmp280_compensate_P(int32_t adc_P)
 121 {
 122     int64_t var1, var2, p;
 123     var1 = ((int64_t)bmp280_cal.t_fine) - 128000;
 124     var2 = var1 * var1 * (int64_t)bmp280_cal.dig_P6;
 125     var2 = var2 + ((var1*(int64_t)bmp280_cal.dig_P5) << 17);
 126     var2 = var2 + (((int64_t)bmp280_cal.dig_P4) << 35);
 127     var1 = ((var1 * var1 * (int64_t)bmp280_cal.dig_P3) >> 8) + ((var1 * (int64_t)bmp280_cal.dig_P2) << 12);
 128     var1 = (((((int64_t)1) << 47) + var1)) * ((int64_t)bmp280_cal.dig_P1) >> 33;
 129     if (var1 == 0) {
 130         return 0;
 131     }
 132
 133     p = 1048576 - adc_P;
 134     p = (((p << 31) - var2) * 3125) / var1;
 135     var1 = (((int64_t)bmp280_cal.dig_P9) * (p >> 13) * (p >> 13)) >> 25;
 136     var2 = (((int64_t)bmp280_cal.dig_P8) * p) >> 19;
 137     p = ((p + var1 + var2) >> 8) + (((int64_t)bmp280_cal.dig_P7) << 4);
 138     return (uint32_t)p;
 139 }
 140
 141 STATIC_UNIT_TESTED bool bmp280_calculate(baroDev_t * baro, int32_t * pressure, int32_t * temperature)
 142 {
 143     UNUSED(baro);
 144
 145     int32_t t = bmp280_compensate_T(bmp280_ut);
 146     uint32_t p = bmp280_compensate_P(bmp280_up);
 147
 148     if (pressure) {
 149         *pressure = (int32_t)(p / 256);
 150     }
 151
 152     if (temperature) {
 153         *temperature = t;
 154     }
 155
 156     return true;
 157 }
 158
 159 #define DETECTION_MAX_RETRY_COUNT   5
 160 static bool deviceDetect(busDevice_t * busDev)
 161 {
 162     for (int retry = 0; retry < DETECTION_MAX_RETRY_COUNT; retry++) {
 163         uint8_t chipId = 0;
 164
 165         delay(100);
 166
 167         bool ack = busRead(busDev, BMP280_CHIP_ID_REG, &chipId);
 168
 169         if ((ack && chipId == BMP280_DEFAULT_CHIP_ID)  || (ack && chipId == BME280_DEFAULT_CHIP_ID)){
 170             return true;
 171         }
 172     };
 173
 174     return false;
 175 }
 176
 177 bool bmp280Detect(baroDev_t *baro)
 178 {
 179     baro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_BMP280, 0, OWNER_BARO);
 180     if (baro->busDev == NULL) {
 181         return false;
 182     }
 183
 184     busSetSpeed(baro->busDev, BUS_SPEED_STANDARD);
 185
 186     if (!deviceDetect(baro->busDev)) {
 187         busDeviceDeInit(baro->busDev);
 188         return false;
 189     }
 190
 191     // read calibration
 192     busReadBuf(baro->busDev, BMP280_TEMPERATURE_CALIB_DIG_T1_LSB_REG, (uint8_t *)&bmp280_cal, 24);
 193
 194     //set filter setting
 195     busWrite(baro->busDev, BMP280_CONFIG_REG, BMP280_FILTER);
 196
 197     // set oversampling + power mode (forced), and start sampling
 198     busWrite(baro->busDev, BMP280_CTRL_MEAS_REG, BMP280_MODE);
 199
 200     baro->ut_delay = 0;
 201     baro->get_ut = bmp280_get_ut;
 202     baro->start_ut = bmp280_start_ut;
 203
 204     baro->up_delay = ((T_INIT_MAX + T_MEASURE_PER_OSRS_MAX * (((1 << BMP280_TEMPERATURE_OSR) >> 1) + ((1 << BMP280_PRESSURE_OSR) >> 1)) + (BMP280_PRESSURE_OSR ? T_SETUP_PRESSURE_MAX : 0) + 15) / 16) * 1000;
 205     baro->start_up = bmp280_start_up;
 206     baro->get_up = bmp280_get_up;
 207
 208     baro->calculate = bmp280_calculate;
 209
 210     return true;
 211 }
 212
 213 #endif