src/main/sensors/barometer.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <math.h>
  24
  25 #include "platform.h"
  26
  27 #ifdef USE_BARO
  28
  29 #include "build/debug.h"
  30
  31 #include "common/maths.h"
  32
  33 #include "pg/pg.h"
  34 #include "pg/pg_ids.h"
  35
  36 #include "drivers/barometer/barometer.h"
  37 #include "drivers/barometer/barometer_bmp085.h"
  38 #include "drivers/barometer/barometer_bmp280.h"
  39 #include "drivers/barometer/barometer_bmp388.h"
  40 #include "drivers/barometer/barometer_dps310.h"
  41 #include "drivers/barometer/barometer_qmp6988.h"
  42 #include "drivers/barometer/barometer_fake.h"
  43 #include "drivers/barometer/barometer_ms5611.h"
  44 #include "drivers/barometer/barometer_lps.h"
  45 #include "drivers/bus.h"
  46 #include "drivers/bus_i2c_busdev.h"
  47 #include "drivers/bus_spi.h"
  48 #include "drivers/io.h"
  49 #include "drivers/time.h"
  50
  51 #include "fc/runtime_config.h"
  52
  53 #include "sensors/sensors.h"
  54
  55 #include "scheduler/scheduler.h"
  56
  57 #include "barometer.h"
  58
  59 baro_t baro;                        // barometer access functions
  60
  61 PG_REGISTER_WITH_RESET_FN(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 1);
  62
  63 void pgResetFn_barometerConfig(barometerConfig_t *barometerConfig)
  64 {
  65     barometerConfig->baro_sample_count = 21;
  66     barometerConfig->baro_noise_lpf = 600;
  67     barometerConfig->baro_cf_vel = 985;
  68     barometerConfig->baro_hardware = BARO_DEFAULT;
  69
  70     // For backward compatibility; ceate a valid default value for bus parameters
  71     //
  72     // 1. If DEFAULT_BARO_xxx is defined, use it.
  73     // 2. Determine default based on USE_BARO_xxx
  74     //   a. Precedence is in the order of popularity; BMP388, BMP280, MS5611 then BMP085, then
  75     //   b. If SPI variant is specified, it is likely onboard, so take it.
  76
  77 #if !(defined(DEFAULT_BARO_SPI_BMP388) || defined(DEFAULT_BARO_BMP388) || defined(DEFAULT_BARO_SPI_BMP280) || defined(DEFAULT_BARO_BMP280) || defined(DEFAULT_BARO_SPI_MS5611) || defined(DEFAULT_BARO_MS5611) || defined(DEFAULT_BARO_BMP085) || defined(DEFAULT_BARO_SPI_LPS) || defined(DEFAULT_BARO_SPI_QMP6988) || defined(DEFAULT_BARO_QMP6988)) || defined(DEFAULT_BARO_DPS310) || defined(DEFAULT_BARO_SPI_DPS310)
  78
  79 #if defined(USE_BARO_DPS310) || defined(USE_BARO_SPI_DPS310)
  80 #if defined(USE_BARO_SPI_DPS310)
  81 #define DEFAULT_BARO_SPI_DPS310
  82 #else
  83 #define DEFAULT_BARO_DPS310
  84 #endif
  85 #elif defined(USE_BARO_BMP388) || defined(USE_BARO_SPI_BMP388)
  86 #if defined(USE_BARO_SPI_BMP388)
  87 #define DEFAULT_BARO_SPI_BMP388
  88 #else
  89 #define DEFAULT_BARO_BMP388
  90 #endif
  91 #elif defined(USE_BARO_BMP280) || defined(USE_BARO_SPI_BMP280)
  92 #if defined(USE_BARO_SPI_BMP280)
  93 #define DEFAULT_BARO_SPI_BMP280
  94 #else
  95 #define DEFAULT_BARO_BMP280
  96 #endif
  97 #elif defined(USE_BARO_MS5611) || defined(USE_BARO_SPI_MS5611)
  98 #if defined(USE_BARO_SPI_MS5611)
  99 #define DEFAULT_BARO_SPI_MS5611
 100 #else
 101 #define DEFAULT_BARO_MS5611
 102 #endif
 103 #elif defined(USE_BARO_QMP6988) || defined(USE_BARO_SPI_QMP6988)
 104 #if defined(USE_BARO_SPI_QMP6988)
 105 #define DEFAULT_BARO_SPI_QMP6988
 106 #else
 107 #define DEFAULT_BARO_QMP6988
 108 #endif
 109 #elif defined(USE_BARO_SPI_LPS)
 110 #define DEFAULT_BARO_SPI_LPS
 111 #elif defined(DEFAULT_BARO_BMP085)
 112 #define DEFAULT_BARO_BMP085
 113 #endif
 114 #endif
 115
 116 #if defined(DEFAULT_BARO_SPI_BMP388) || defined(DEFAULT_BARO_SPI_BMP280) || defined(DEFAULT_BARO_SPI_MS5611) || defined(DEFAULT_BARO_SPI_QMP6988) || defined(DEFAULT_BARO_SPI_LPS) || defined(DEFAULT_BARO_SPI_DPS310)
 117     barometerConfig->baro_busType = BUS_TYPE_SPI;
 118     barometerConfig->baro_spi_device = SPI_DEV_TO_CFG(spiDeviceByInstance(BARO_SPI_INSTANCE));
 119     barometerConfig->baro_spi_csn = IO_TAG(BARO_CS_PIN);
 120     barometerConfig->baro_i2c_device = I2C_DEV_TO_CFG(I2CINVALID);
 121     barometerConfig->baro_i2c_address = 0;
 122 #elif defined(DEFAULT_BARO_MS5611) || defined(DEFAULT_BARO_BMP388) || defined(DEFAULT_BARO_BMP280) || defined(DEFAULT_BARO_BMP085) ||defined(DEFAULT_BARO_QMP6988) || defined(DEFAULT_BARO_DPS310)
 123     // All I2C devices shares a default config with address = 0 (per device default)
 124     barometerConfig->baro_busType = BUS_TYPE_I2C;
 125     barometerConfig->baro_i2c_device = I2C_DEV_TO_CFG(BARO_I2C_INSTANCE);
 126     barometerConfig->baro_i2c_address = 0;
 127     barometerConfig->baro_spi_device = SPI_DEV_TO_CFG(SPIINVALID);
 128     barometerConfig->baro_spi_csn = IO_TAG_NONE;
 129 #else
 130     barometerConfig->baro_hardware = BARO_NONE;
 131     barometerConfig->baro_busType = BUS_TYPE_NONE;
 132     barometerConfig->baro_i2c_device = I2C_DEV_TO_CFG(I2CINVALID);
 133     barometerConfig->baro_i2c_address = 0;
 134     barometerConfig->baro_spi_device = SPI_DEV_TO_CFG(SPIINVALID);
 135     barometerConfig->baro_spi_csn = IO_TAG_NONE;
 136 #endif
 137
 138     barometerConfig->baro_eoc_tag = IO_TAG(BARO_EOC_PIN);
 139     barometerConfig->baro_xclr_tag = IO_TAG(BARO_XCLR_PIN);
 140 }
 141
 142 static uint16_t calibratingB = 0;      // baro calibration = get new ground pressure value
 143 static int32_t baroPressure = 0;
 144 static int32_t baroTemperature = 0;
 145
 146 static int32_t baroGroundAltitude = 0;
 147 static int32_t baroGroundPressure = 8*101325;
 148 static uint32_t baroPressureSum = 0;
 149
 150 #define CALIBRATING_BARO_CYCLES 200 // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
 151 #define SET_GROUND_LEVEL_BARO_CYCLES 10 // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)
 152
 153 static bool baroReady = false;
 154
 155 void baroPreInit(void)
 156 {
 157 #ifdef USE_SPI
 158     if (barometerConfig()->baro_busType == BUS_TYPE_SPI) {
 159         spiPreinitRegister(barometerConfig()->baro_spi_csn, IOCFG_IPU, 1);
 160     }
 161 #endif
 162 }
 163
 164 bool baroDetect(baroDev_t *baroDev, baroSensor_e baroHardwareToUse)
 165 {
 166     extDevice_t *dev = &baroDev->dev;
 167
 168     // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function
 169
 170     baroSensor_e baroHardware = baroHardwareToUse;
 171
 172 #if !defined(USE_BARO_BMP085) && !defined(USE_BARO_MS5611) && !defined(USE_BARO_SPI_MS5611) && !defined(USE_BARO_BMP388) && !defined(USE_BARO_BMP280) && !defined(USE_BARO_SPI_BMP280)&& !defined(USE_BARO_QMP6988) && !defined(USE_BARO_SPI_QMP6988) && !defined(USE_BARO_DPS310) && !defined(USE_BARO_SPI_DPS310)
 173     UNUSED(dev);
 174 #endif
 175
 176     switch (barometerConfig()->baro_busType) {
 177 #ifdef USE_I2C
 178     case BUS_TYPE_I2C:
 179         i2cBusSetInstance(dev, barometerConfig()->baro_i2c_device);
 180         dev->busType_u.i2c.address = barometerConfig()->baro_i2c_address;
 181         break;
 182 #endif
 183
 184 #ifdef USE_SPI
 185     case BUS_TYPE_SPI:
 186         {
 187             if (!spiSetBusInstance(dev, barometerConfig()->baro_spi_device, OWNER_BARO_CS)) {
 188                 return false;
 189             }
 190
 191             dev->busType_u.spi.csnPin = IOGetByTag(barometerConfig()->baro_spi_csn);
 192         }
 193         break;
 194 #endif
 195
 196     default:
 197         return false;
 198     }
 199
 200     switch (baroHardware) {
 201     case BARO_DEFAULT:
 202         FALLTHROUGH;
 203
 204     case BARO_BMP085:
 205 #ifdef USE_BARO_BMP085
 206         {
 207             static bmp085Config_t defaultBMP085Config;
 208             defaultBMP085Config.xclrTag = barometerConfig()->baro_xclr_tag;
 209             defaultBMP085Config.eocTag = barometerConfig()->baro_eoc_tag;
 210
 211             static const bmp085Config_t *bmp085Config = &defaultBMP085Config;
 212
 213             if (bmp085Detect(bmp085Config, baroDev)) {
 214                 baroHardware = BARO_BMP085;
 215                 break;
 216             }
 217         }
 218 #endif
 219         FALLTHROUGH;
 220
 221     case BARO_MS5611:
 222 #if defined(USE_BARO_MS5611) || defined(USE_BARO_SPI_MS5611)
 223         if (ms5611Detect(baroDev)) {
 224             baroHardware = BARO_MS5611;
 225             break;
 226         }
 227 #endif
 228         FALLTHROUGH;
 229
 230     case BARO_LPS:
 231 #if defined(USE_BARO_SPI_LPS)
 232         if (lpsDetect(baroDev)) {
 233             baroHardware = BARO_LPS;
 234             break;
 235         }
 236 #endif
 237         FALLTHROUGH;
 238
 239     case BARO_DPS310:
 240 #if defined(USE_BARO_DPS310) || defined(USE_BARO_SPI_DPS310)
 241         {
 242             if (baroDPS310Detect(baroDev)) {
 243                 baroHardware = BARO_DPS310;
 244                 break;
 245             }
 246         }
 247 #endif
 248         FALLTHROUGH;
 249
 250     case BARO_BMP388:
 251 #if defined(USE_BARO_BMP388) || defined(USE_BARO_SPI_BMP388)
 252         {
 253             static bmp388Config_t defaultBMP388Config;
 254
 255             defaultBMP388Config.eocTag = barometerConfig()->baro_eoc_tag;
 256
 257             static const bmp388Config_t *bmp388Config = &defaultBMP388Config;
 258
 259             if (bmp388Detect(bmp388Config, baroDev)) {
 260                 baroHardware = BARO_BMP388;
 261                 break;
 262             }
 263         }
 264 #endif
 265         FALLTHROUGH;
 266
 267     case BARO_BMP280:
 268 #if defined(USE_BARO_BMP280) || defined(USE_BARO_SPI_BMP280)
 269         if (bmp280Detect(baroDev)) {
 270             baroHardware = BARO_BMP280;
 271             break;
 272         }
 273 #endif
 274         FALLTHROUGH;
 275
 276      case BARO_QMP6988:
 277 #if defined(USE_BARO_QMP6988) || defined(USE_BARO_SPI_QMP6988)
 278         if (qmp6988Detect(baroDev)) {
 279             baroHardware = BARO_QMP6988;
 280             break;
 281         }
 282 #endif
 283         FALLTHROUGH;
 284     case BARO_NONE:
 285         baroHardware = BARO_NONE;
 286         break;
 287     }
 288
 289     if (baroHardware == BARO_NONE) {
 290         return false;
 291     }
 292
 293     detectedSensors[SENSOR_INDEX_BARO] = baroHardware;
 294     sensorsSet(SENSOR_BARO);
 295     return true;
 296 }
 297
 298 bool baroIsCalibrationComplete(void)
 299 {
 300     return calibratingB == 0;
 301 }
 302
 303 static void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
 304 {
 305     calibratingB = calibrationCyclesRequired;
 306 }
 307
 308 void baroStartCalibration(void)
 309 {
 310     baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
 311 }
 312
 313 void baroSetGroundLevel(void)
 314 {
 315     baroSetCalibrationCycles(SET_GROUND_LEVEL_BARO_CYCLES);
 316 }
 317
 318 #define PRESSURE_SAMPLES_MEDIAN 3
 319
 320 static int32_t applyBarometerMedianFilter(int32_t newPressureReading)
 321 {
 322     static int32_t barometerFilterSamples[PRESSURE_SAMPLES_MEDIAN];
 323     static int currentFilterSampleIndex = 0;
 324     static bool medianFilterReady = false;
 325     int nextSampleIndex;
 326
 327     nextSampleIndex = (currentFilterSampleIndex + 1);
 328     if (nextSampleIndex == PRESSURE_SAMPLES_MEDIAN) {
 329         nextSampleIndex = 0;
 330         medianFilterReady = true;
 331     }
 332
 333     barometerFilterSamples[currentFilterSampleIndex] = newPressureReading;
 334     currentFilterSampleIndex = nextSampleIndex;
 335
 336     if (medianFilterReady)
 337         return quickMedianFilter3(barometerFilterSamples);
 338     else
 339         return newPressureReading;
 340 }
 341
 342 static uint32_t recalculateBarometerTotal(uint32_t pressureTotal, int32_t newPressureReading)
 343 {
 344     static int32_t barometerSamples[BARO_SAMPLE_COUNT_MAX + 1];
 345     static int currentSampleIndex = 0;
 346     int nextSampleIndex;
 347
 348     // store current pressure in barometerSamples
 349     if (currentSampleIndex >= barometerConfig()->baro_sample_count) {
 350         nextSampleIndex = 0;
 351         baroReady = true;
 352     } else {
 353         nextSampleIndex = (currentSampleIndex + 1);
 354     }
 355     barometerSamples[currentSampleIndex] = applyBarometerMedianFilter(newPressureReading);
 356
 357     // recalculate pressure total
 358     pressureTotal += barometerSamples[currentSampleIndex];
 359     pressureTotal -= barometerSamples[nextSampleIndex];
 360
 361     currentSampleIndex = nextSampleIndex;
 362
 363     return pressureTotal;
 364 }
 365
 366 typedef enum {
 367     BAROMETER_NEEDS_TEMPERATURE_READ = 0,
 368     BAROMETER_NEEDS_TEMPERATURE_SAMPLE,
 369     BAROMETER_NEEDS_PRESSURE_START,
 370     BAROMETER_NEEDS_PRESSURE_READ,
 371     BAROMETER_NEEDS_PRESSURE_SAMPLE,
 372     BAROMETER_NEEDS_TEMPERATURE_START
 373 } barometerState_e;
 374
 375
 376 bool isBaroReady(void) {
 377     return baroReady;
 378 }
 379
 380 uint32_t baroUpdate(void)
 381 {
 382     static barometerState_e state = BAROMETER_NEEDS_PRESSURE_START;
 383     timeUs_t sleepTime = 1000; // Wait 1ms between states
 384
 385     if (debugMode == DEBUG_BARO) {
 386         debug[0] = state;
 387     }
 388
 389     // Tell the scheduler to ignore how long this task takes unless the pressure is being read
 390     // as that takes the longest
 391     if (state != BAROMETER_NEEDS_PRESSURE_READ) {
 392         ignoreTaskTime();
 393     }
 394
 395     switch (state) {
 396         default:
 397         case BAROMETER_NEEDS_TEMPERATURE_START:
 398             baro.dev.start_ut(&baro.dev);
 399             state = BAROMETER_NEEDS_TEMPERATURE_READ;
 400             sleepTime = baro.dev.ut_delay;
 401             break;
 402
 403         case BAROMETER_NEEDS_TEMPERATURE_READ:
 404             if (baro.dev.read_ut(&baro.dev)) {
 405                 state = BAROMETER_NEEDS_TEMPERATURE_SAMPLE;
 406             }
 407         break;
 408
 409         case BAROMETER_NEEDS_TEMPERATURE_SAMPLE:
 410             if (baro.dev.get_ut(&baro.dev)) {
 411                 state = BAROMETER_NEEDS_PRESSURE_START;
 412             }
 413         break;
 414
 415         case BAROMETER_NEEDS_PRESSURE_START:
 416             baro.dev.start_up(&baro.dev);
 417             state = BAROMETER_NEEDS_PRESSURE_READ;
 418             sleepTime = baro.dev.up_delay;
 419         break;
 420
 421         case BAROMETER_NEEDS_PRESSURE_READ:
 422             if (baro.dev.read_up(&baro.dev)) {
 423                 state = BAROMETER_NEEDS_PRESSURE_SAMPLE;
 424             } else {
 425                 ignoreTaskTime();
 426             }
 427         break;
 428
 429         case BAROMETER_NEEDS_PRESSURE_SAMPLE:
 430             if (!baro.dev.get_up(&baro.dev)) {
 431                 break;
 432             }
 433
 434             baro.dev.calculate(&baroPressure, &baroTemperature);
 435             baro.baroPressure = baroPressure;
 436             baro.baroTemperature = baroTemperature;
 437             baroPressureSum = recalculateBarometerTotal(baroPressureSum, baroPressure);
 438             if (baro.dev.combined_read) {
 439                 state = BAROMETER_NEEDS_PRESSURE_START;
 440             } else {
 441                 state = BAROMETER_NEEDS_TEMPERATURE_START;
 442             }
 443
 444             if (debugMode == DEBUG_BARO) {
 445                 debug[1] = baroTemperature;
 446                 debug[2] = baroPressure;
 447                 debug[3] = baroPressureSum;
 448             }
 449
 450             sleepTime = baro.dev.ut_delay;
 451             break;
 452     }
 453
 454     return sleepTime;
 455 }
 456
 457 static float pressureToAltitude(const float pressure)
 458 {
 459     return (1.0f - powf(pressure / 101325.0f, 0.190295f)) * 4433000.0f;
 460 }
 461
 462 int32_t baroCalculateAltitude(void)
 463 {
 464     int32_t BaroAlt_tmp;
 465
 466     // calculates height from ground via baro readings
 467     if (baroIsCalibrationComplete()) {
 468         BaroAlt_tmp = lrintf(pressureToAltitude((float)(baroPressureSum / barometerConfig()->baro_sample_count)));
 469         BaroAlt_tmp -= baroGroundAltitude;
 470         baro.BaroAlt = lrintf((float)baro.BaroAlt * CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_noise_lpf) + (float)BaroAlt_tmp * (1.0f - CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_noise_lpf))); // additional LPF to reduce baro noise
 471     }
 472     else {
 473         baro.BaroAlt = 0;
 474     }
 475     return baro.BaroAlt;
 476 }
 477
 478 void performBaroCalibrationCycle(void)
 479 {
 480     static int32_t savedGroundPressure = 0;
 481
 482     baroGroundPressure -= baroGroundPressure / 8;
 483     baroGroundPressure += baroPressureSum / barometerConfig()->baro_sample_count;
 484     baroGroundAltitude = (1.0f - pow_approx((baroGroundPressure / 8) / 101325.0f, 0.190259f)) * 4433000.0f;
 485
 486     if (baroGroundPressure == savedGroundPressure) {
 487         calibratingB = 0;
 488     } else {
 489         calibratingB--;
 490         savedGroundPressure = baroGroundPressure;
 491     }
 492 }
 493
 494 #endif /* BARO */