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)) {
 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     BARO_STATE_TEMPERATURE_READ = 0,
 368     BARO_STATE_TEMPERATURE_SAMPLE,
 369     BARO_STATE_PRESSURE_START,
 370     BARO_STATE_PRESSURE_READ,
 371     BARO_STATE_PRESSURE_SAMPLE,
 372     BARO_STATE_TEMPERATURE_START,
 373     BARO_STATE_COUNT
 374 } barometerState_e;
 375
 376
 377 bool isBaroReady(void) {
 378     return baroReady;
 379 }
 380
 381 uint32_t baroUpdate(timeUs_t currentTimeUs)
 382 {
 383     static timeUs_t baroStateDurationUs[BARO_STATE_COUNT];
 384     static barometerState_e state = BARO_STATE_PRESSURE_START;
 385     barometerState_e oldState = state;
 386     timeUs_t executeTimeUs;
 387     timeUs_t sleepTime = 1000; // Wait 1ms between states
 388
 389     DEBUG_SET(DEBUG_BARO, 0, state);
 390
 391     if (busBusy(&baro.dev.dev, NULL)) {
 392         // If the bus is busy, simply return to have another go later
 393         schedulerIgnoreTaskStateTime();
 394         return sleepTime;
 395     }
 396
 397     switch (state) {
 398         default:
 399         case BARO_STATE_TEMPERATURE_START:
 400             baro.dev.start_ut(&baro.dev);
 401             state = BARO_STATE_TEMPERATURE_READ;
 402             sleepTime = baro.dev.ut_delay;
 403             break;
 404
 405         case BARO_STATE_TEMPERATURE_READ:
 406             if (baro.dev.read_ut(&baro.dev)) {
 407                 state = BARO_STATE_TEMPERATURE_SAMPLE;
 408             } else {
 409                 // No action was taken as the read has not completed
 410                 schedulerIgnoreTaskExecTime();
 411             }
 412             break;
 413
 414         case BARO_STATE_TEMPERATURE_SAMPLE:
 415             if (baro.dev.get_ut(&baro.dev)) {
 416                 state = BARO_STATE_PRESSURE_START;
 417             } else {
 418                 // No action was taken as the read has not completed
 419                 schedulerIgnoreTaskExecTime();
 420             }
 421             break;
 422
 423         case BARO_STATE_PRESSURE_START:
 424             baro.dev.start_up(&baro.dev);
 425             state = BARO_STATE_PRESSURE_READ;
 426             sleepTime = baro.dev.up_delay;
 427             break;
 428
 429         case BARO_STATE_PRESSURE_READ:
 430             if (baro.dev.read_up(&baro.dev)) {
 431                 state = BARO_STATE_PRESSURE_SAMPLE;
 432             } else {
 433                 // No action was taken as the read has not completed
 434                 schedulerIgnoreTaskExecTime();
 435             }
 436             break;
 437
 438         case BARO_STATE_PRESSURE_SAMPLE:
 439             if (!baro.dev.get_up(&baro.dev)) {
 440                 // No action was taken as the read has not completed
 441                 schedulerIgnoreTaskExecTime();
 442                 break;
 443             }
 444
 445             baro.dev.calculate(&baroPressure, &baroTemperature);
 446             baro.baroPressure = baroPressure;
 447             baro.baroTemperature = baroTemperature;
 448             baroPressureSum = recalculateBarometerTotal(baroPressureSum, baroPressure);
 449             if (baro.dev.combined_read) {
 450                 state = BARO_STATE_PRESSURE_START;
 451             } else {
 452                 state = BARO_STATE_TEMPERATURE_START;
 453             }
 454
 455             DEBUG_SET(DEBUG_BARO, 1, baroTemperature);
 456             DEBUG_SET(DEBUG_BARO, 2, baroPressure);
 457             DEBUG_SET(DEBUG_BARO, 3, baroPressureSum);
 458
 459             sleepTime = baro.dev.ut_delay;
 460             break;
 461     }
 462
 463     // Where we are using a state machine call schedulerIgnoreTaskExecRate() for all states bar one
 464     if (sleepTime != baro.dev.ut_delay) {
 465         schedulerIgnoreTaskExecRate();
 466     }
 467
 468     executeTimeUs = micros() - currentTimeUs;
 469
 470     if (executeTimeUs > baroStateDurationUs[oldState]) {
 471         baroStateDurationUs[oldState] = executeTimeUs;
 472     }
 473
 474     schedulerSetNextStateTime(baroStateDurationUs[state]);
 475
 476     return sleepTime;
 477 }
 478
 479 static float pressureToAltitude(const float pressure)
 480 {
 481     return (1.0f - powf(pressure / 101325.0f, 0.190295f)) * 4433000.0f;
 482 }
 483
 484 int32_t baroCalculateAltitude(void)
 485 {
 486     int32_t BaroAlt_tmp;
 487
 488     // calculates height from ground via baro readings
 489     if (baroIsCalibrationComplete()) {
 490         BaroAlt_tmp = lrintf(pressureToAltitude((float)(baroPressureSum / barometerConfig()->baro_sample_count)));
 491         BaroAlt_tmp -= baroGroundAltitude;
 492         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
 493     }
 494     else {
 495         baro.BaroAlt = 0;
 496     }
 497     return baro.BaroAlt;
 498 }
 499
 500 void performBaroCalibrationCycle(void)
 501 {
 502     static int32_t savedGroundPressure = 0;
 503
 504     baroGroundPressure -= baroGroundPressure / 8;
 505     baroGroundPressure += baroPressureSum / barometerConfig()->baro_sample_count;
 506     baroGroundAltitude = (1.0f - pow_approx((baroGroundPressure / 8) / 101325.0f, 0.190259f)) * 4433000.0f;
 507
 508     if (baroGroundPressure == savedGroundPressure) {
 509         calibratingB = 0;
 510     } else {
 511         calibratingB--;
 512         savedGroundPressure = baroGroundPressure;
 513     }
 514 }
 515
 516 #endif /* BARO */