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 #include "common/filter.h"
  33
  34 #include "pg/pg.h"
  35 #include "pg/pg_ids.h"
  36
  37 #include "drivers/barometer/barometer.h"
  38 #include "drivers/barometer/barometer_bmp085.h"
  39 #include "drivers/barometer/barometer_bmp280.h"
  40 #include "drivers/barometer/barometer_bmp388.h"
  41 #include "drivers/barometer/barometer_dps310.h"
  42 #include "drivers/barometer/barometer_qmp6988.h"
  43 #include "drivers/barometer/barometer_virtual.h"
  44 #include "drivers/barometer/barometer_ms5611.h"
  45 #include "drivers/barometer/barometer_lps.h"
  46 #include "drivers/barometer/barometer_2smpb_02b.h"
  47 #include "drivers/bus.h"
  48 #include "drivers/bus_i2c_busdev.h"
  49 #include "drivers/bus_spi.h"
  50 #include "drivers/io.h"
  51 #include "drivers/time.h"
  52
  53 #include "fc/runtime_config.h"
  54
  55 #include "sensors/sensors.h"
  56
  57 #include "scheduler/scheduler.h"
  58
  59 #include "barometer.h"
  60
  61 baro_t baro;                        // barometer access functions
  62
  63 PG_REGISTER_WITH_RESET_FN(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 3);
  64
  65 #ifndef DEFAULT_BARO_DEVICE
  66 #define DEFAULT_BARO_DEVICE BARO_DEFAULT
  67 #endif
  68
  69 void pgResetFn_barometerConfig(barometerConfig_t *barometerConfig)
  70 {
  71     barometerConfig->baro_hardware = DEFAULT_BARO_DEVICE;
  72
  73     // For backward compatibility; ceate a valid default value for bus parameters
  74     //
  75     // 1. If DEFAULT_BARO_xxx is defined, use it.
  76     // 2. Determine default based on USE_BARO_xxx
  77     //   a. Precedence is in the order of popularity; BMP388, BMP280, MS5611 then BMP085, then
  78     //   b. If SPI variant is specified, it is likely onboard, so take it.
  79
  80 #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)
  81
  82 #if defined(USE_BARO_DPS310) || defined(USE_BARO_SPI_DPS310)
  83 #if defined(USE_BARO_SPI_DPS310)
  84 #define DEFAULT_BARO_SPI_DPS310
  85 #else
  86 #define DEFAULT_BARO_DPS310
  87 #endif
  88 #elif defined(USE_BARO_BMP388) || defined(USE_BARO_SPI_BMP388)
  89 #if defined(USE_BARO_SPI_BMP388)
  90 #define DEFAULT_BARO_SPI_BMP388
  91 #else
  92 #define DEFAULT_BARO_BMP388
  93 #endif
  94 #elif defined(USE_BARO_BMP280) || defined(USE_BARO_SPI_BMP280)
  95 #if defined(USE_BARO_SPI_BMP280)
  96 #define DEFAULT_BARO_SPI_BMP280
  97 #else
  98 #define DEFAULT_BARO_BMP280
  99 #endif
 100 #elif defined(USE_BARO_MS5611) || defined(USE_BARO_SPI_MS5611)
 101 #if defined(USE_BARO_SPI_MS5611)
 102 #define DEFAULT_BARO_SPI_MS5611
 103 #else
 104 #define DEFAULT_BARO_MS5611
 105 #endif
 106 #elif defined(USE_BARO_QMP6988) || defined(USE_BARO_SPI_QMP6988)
 107 #if defined(USE_BARO_SPI_QMP6988)
 108 #define DEFAULT_BARO_SPI_QMP6988
 109 #else
 110 #define DEFAULT_BARO_QMP6988
 111 #endif
 112 #elif defined(USE_BARO_SPI_LPS)
 113 #define DEFAULT_BARO_SPI_LPS
 114 #elif defined(DEFAULT_BARO_BMP085)
 115 #define DEFAULT_BARO_BMP085
 116 #endif
 117 #elif defined(USE_BARO_2SMBP_02B) || defined(USE_BARO_SPI_2SMBP_02B)
 118 #if defined(USE_BARO_SPI_2SMBP_02B)
 119 #define DEFAULT_BARO_SPI_2SMBP_02B
 120 #else
 121 #define DEFAULT_BARO_2SMBP_02B
 122 #endif
 123 #endif
 124
 125 #ifndef DEFAULT_BARO_I2C_ADDRESS
 126 #define DEFAULT_BARO_I2C_ADDRESS 0
 127 #endif
 128
 129 #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) || defined(DEFAULT_BARO_SPI_2SMBP_02B)
 130     barometerConfig->baro_busType = BUS_TYPE_SPI;
 131     barometerConfig->baro_spi_device = SPI_DEV_TO_CFG(spiDeviceByInstance(BARO_SPI_INSTANCE));
 132     barometerConfig->baro_spi_csn = IO_TAG(BARO_CS_PIN);
 133     barometerConfig->baro_i2c_device = I2C_DEV_TO_CFG(I2CINVALID);
 134     barometerConfig->baro_i2c_address = 0;
 135 #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) || defined(DEFAULT_BARO_2SMBP_02B)
 136     // All I2C devices shares a default config with address = 0 (per device default)
 137     barometerConfig->baro_busType = BUS_TYPE_I2C;
 138     barometerConfig->baro_i2c_device = I2C_DEV_TO_CFG(BARO_I2C_INSTANCE);
 139     barometerConfig->baro_i2c_address = DEFAULT_BARO_I2C_ADDRESS;
 140     barometerConfig->baro_spi_device = SPI_DEV_TO_CFG(SPIINVALID);
 141     barometerConfig->baro_spi_csn = IO_TAG_NONE;
 142 #else
 143     barometerConfig->baro_hardware = BARO_NONE;
 144     barometerConfig->baro_busType = BUS_TYPE_NONE;
 145     barometerConfig->baro_i2c_device = I2C_DEV_TO_CFG(I2CINVALID);
 146     barometerConfig->baro_i2c_address = 0;
 147     barometerConfig->baro_spi_device = SPI_DEV_TO_CFG(SPIINVALID);
 148     barometerConfig->baro_spi_csn = IO_TAG_NONE;
 149 #endif
 150
 151 #ifndef BARO_EOC_PIN
 152 #define BARO_EOC_PIN NONE
 153 #endif
 154
 155     barometerConfig->baro_eoc_tag = IO_TAG(BARO_EOC_PIN);
 156     barometerConfig->baro_xclr_tag = IO_TAG(BARO_XCLR_PIN);
 157 }
 158
 159 #define NUM_CALIBRATION_CYCLES   100        // 10 seconds init_delay + 100 * 25 ms = 12.5 seconds before valid baro altitude
 160 #define NUM_GROUND_LEVEL_CYCLES   10        // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)
 161
 162 static uint16_t calibrationCycles = 0;      // baro calibration = get new ground pressure value
 163 static float baroGroundAltitude = 0.0f;
 164 static bool baroCalibrated = false;
 165 static bool baroReady = false;
 166
 167 void baroPreInit(void)
 168 {
 169 #ifdef USE_SPI
 170     if (barometerConfig()->baro_busType == BUS_TYPE_SPI) {
 171         spiPreinitRegister(barometerConfig()->baro_spi_csn, IOCFG_IPU, 1);
 172     }
 173 #endif
 174 }
 175
 176 static bool baroDetect(baroDev_t *baroDev, baroSensor_e baroHardwareToUse)
 177 {
 178     extDevice_t *dev = &baroDev->dev;
 179
 180     // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function
 181
 182     baroSensor_e baroHardware = baroHardwareToUse;
 183
 184 #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) && !defined(DEFAULT_BARO_SPI_2SMBP_02B) && !defined(DEFAULT_BARO_2SMBP_02B)
 185     UNUSED(dev);
 186 #endif
 187
 188 #ifndef USE_VIRTUAL_BARO
 189     switch (barometerConfig()->baro_busType) {
 190 #ifdef USE_I2C
 191     case BUS_TYPE_I2C:
 192         i2cBusSetInstance(dev, barometerConfig()->baro_i2c_device);
 193         dev->busType_u.i2c.address = barometerConfig()->baro_i2c_address;
 194         break;
 195 #endif
 196
 197 #ifdef USE_SPI
 198     case BUS_TYPE_SPI:
 199         {
 200             if (!spiSetBusInstance(dev, barometerConfig()->baro_spi_device)) {
 201                 return false;
 202             }
 203
 204             dev->busType_u.spi.csnPin = IOGetByTag(barometerConfig()->baro_spi_csn);
 205         }
 206         break;
 207 #endif
 208     default:
 209         return false;
 210     }
 211 #endif // USE_VIRTUAL_BARO
 212
 213     switch (baroHardware) {
 214     case BARO_DEFAULT:
 215         FALLTHROUGH;
 216
 217     case BARO_BMP085:
 218 #ifdef USE_BARO_BMP085
 219         {
 220             static bmp085Config_t defaultBMP085Config;
 221             defaultBMP085Config.xclrTag = barometerConfig()->baro_xclr_tag;
 222             defaultBMP085Config.eocTag = barometerConfig()->baro_eoc_tag;
 223
 224             static const bmp085Config_t *bmp085Config = &defaultBMP085Config;
 225
 226             if (bmp085Detect(bmp085Config, baroDev)) {
 227                 baroHardware = BARO_BMP085;
 228                 break;
 229             }
 230         }
 231 #endif
 232         FALLTHROUGH;
 233
 234     case BARO_MS5611:
 235 #if defined(USE_BARO_MS5611) || defined(USE_BARO_SPI_MS5611)
 236         if (ms5611Detect(baroDev)) {
 237             baroHardware = BARO_MS5611;
 238             break;
 239         }
 240 #endif
 241         FALLTHROUGH;
 242
 243     case BARO_LPS:
 244 #if defined(USE_BARO_SPI_LPS)
 245         if (lpsDetect(baroDev)) {
 246             baroHardware = BARO_LPS;
 247             break;
 248         }
 249 #endif
 250         FALLTHROUGH;
 251
 252     case BARO_DPS310:
 253 #if defined(USE_BARO_DPS310) || defined(USE_BARO_SPI_DPS310)
 254         {
 255             if (baroDPS310Detect(baroDev)) {
 256                 baroHardware = BARO_DPS310;
 257                 break;
 258             }
 259         }
 260 #endif
 261         FALLTHROUGH;
 262
 263     case BARO_BMP388:
 264 #if defined(USE_BARO_BMP388) || defined(USE_BARO_SPI_BMP388)
 265         {
 266             static bmp388Config_t defaultBMP388Config;
 267
 268             defaultBMP388Config.eocTag = barometerConfig()->baro_eoc_tag;
 269
 270             static const bmp388Config_t *bmp388Config = &defaultBMP388Config;
 271
 272             if (bmp388Detect(bmp388Config, baroDev)) {
 273                 baroHardware = BARO_BMP388;
 274                 break;
 275             }
 276         }
 277 #endif
 278         FALLTHROUGH;
 279
 280     case BARO_BMP280:
 281 #if defined(USE_BARO_BMP280) || defined(USE_BARO_SPI_BMP280)
 282         if (bmp280Detect(baroDev)) {
 283             baroHardware = BARO_BMP280;
 284             break;
 285         }
 286 #endif
 287         FALLTHROUGH;
 288
 289      case BARO_QMP6988:
 290 #if defined(USE_BARO_QMP6988) || defined(USE_BARO_SPI_QMP6988)
 291         if (qmp6988Detect(baroDev)) {
 292             baroHardware = BARO_QMP6988;
 293             break;
 294         }
 295 #endif
 296         FALLTHROUGH;
 297
 298      case BARO_2SMPB_02B:
 299 #if defined(USE_BARO_2SMBP_02B) || defined(USE_BARO_SPI_2SMBP_02B)
 300         if (baro2SMPB02BDetect(baroDev)) {
 301             baroHardware = BARO_2SMPB_02B;
 302             break;
 303         }
 304 #endif
 305         FALLTHROUGH;
 306
 307     case BARO_VIRTUAL:
 308 #ifdef USE_VIRTUAL_BARO
 309         if (virtualBaroDetect(baroDev)) {
 310             baroHardware = BARO_VIRTUAL;
 311             break;
 312         }
 313 #endif
 314         FALLTHROUGH;
 315
 316     case BARO_NONE:
 317         baroHardware = BARO_NONE;
 318         break;
 319     }
 320
 321     if (baroHardware == BARO_NONE) {
 322         return false;
 323     }
 324
 325     detectedSensors[SENSOR_INDEX_BARO] = baroHardware;
 326     sensorsSet(SENSOR_BARO);
 327     return true;
 328 }
 329
 330 void baroInit(void)
 331 {
 332 #ifndef USE_VIRTUAL_BARO
 333     baroReady = baroDetect(&baro.dev, barometerConfig()->baro_hardware);
 334 #else
 335     baroReady = baroDetect(&baro.dev, BARO_VIRTUAL);
 336 #endif
 337 }
 338
 339 bool baroIsCalibrated(void)
 340 {
 341     return baroCalibrated;
 342 }
 343
 344 static void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
 345 {
 346     calibrationCycles = calibrationCyclesRequired;
 347 }
 348
 349 void baroStartCalibration(void)
 350 {
 351     baroSetCalibrationCycles(NUM_CALIBRATION_CYCLES);
 352     baroGroundAltitude = 0;
 353     baroCalibrated = false;
 354 }
 355
 356 void baroSetGroundLevel(void)
 357 {
 358     baroSetCalibrationCycles(NUM_GROUND_LEVEL_CYCLES);
 359     baroGroundAltitude = 0;
 360     baroCalibrated = false;
 361 }
 362
 363 typedef enum {
 364     BARO_STATE_TEMPERATURE_READ = 0,
 365     BARO_STATE_TEMPERATURE_SAMPLE,
 366     BARO_STATE_PRESSURE_START,
 367     BARO_STATE_PRESSURE_READ,
 368     BARO_STATE_PRESSURE_SAMPLE,
 369     BARO_STATE_TEMPERATURE_START,
 370     BARO_STATE_COUNT
 371 } barometerState_e;
 372
 373
 374 bool isBaroReady(void)
 375 {
 376     return baroReady;
 377 }
 378
 379 static float pressureToAltitude(const float pressure)
 380 {
 381     return (1.0f - powf(pressure / 101325.0f, 0.190295f)) * 4433000.0f;
 382 }
 383
 384 static void performBaroCalibrationCycle(const float altitude);
 385
 386 uint32_t baroUpdate(timeUs_t currentTimeUs)
 387 {
 388     static timeUs_t baroStateDurationUs[BARO_STATE_COUNT];
 389     static barometerState_e state = BARO_STATE_PRESSURE_START;
 390     barometerState_e oldState = state;
 391     timeUs_t executeTimeUs;
 392     timeUs_t sleepTime = 1000; // Wait 1ms between states
 393
 394     DEBUG_SET(DEBUG_BARO, 0, state);
 395
 396     if (busBusy(&baro.dev.dev, NULL)) {
 397         // If the bus is busy, simply return to have another go later
 398         schedulerIgnoreTaskStateTime();
 399         return sleepTime;
 400     }
 401
 402     switch (state) {
 403         default:
 404         case BARO_STATE_TEMPERATURE_START:
 405             baro.dev.start_ut(&baro.dev);
 406             state = BARO_STATE_TEMPERATURE_READ;
 407             sleepTime = baro.dev.ut_delay;
 408             break;
 409
 410         case BARO_STATE_TEMPERATURE_READ:
 411             if (baro.dev.read_ut(&baro.dev)) {
 412                 state = BARO_STATE_TEMPERATURE_SAMPLE;
 413             } else {
 414                 // No action was taken as the read has not completed
 415                 schedulerIgnoreTaskExecTime();
 416             }
 417             break;
 418
 419         case BARO_STATE_TEMPERATURE_SAMPLE:
 420             if (baro.dev.get_ut(&baro.dev)) {
 421                 state = BARO_STATE_PRESSURE_START;
 422             } else {
 423                 // No action was taken as the read has not completed
 424                 schedulerIgnoreTaskExecTime();
 425             }
 426             break;
 427
 428         case BARO_STATE_PRESSURE_START:
 429             baro.dev.start_up(&baro.dev);
 430             state = BARO_STATE_PRESSURE_READ;
 431             sleepTime = baro.dev.up_delay;
 432             break;
 433
 434         case BARO_STATE_PRESSURE_READ:
 435             if (baro.dev.read_up(&baro.dev)) {
 436                 state = BARO_STATE_PRESSURE_SAMPLE;
 437             } else {
 438                 // No action was taken as the read has not completed
 439                 schedulerIgnoreTaskExecTime();
 440             }
 441             break;
 442
 443         case BARO_STATE_PRESSURE_SAMPLE:
 444             if (!baro.dev.get_up(&baro.dev)) {
 445                 // No action was taken as the read has not completed
 446                 schedulerIgnoreTaskExecTime();
 447                 break;
 448             }
 449
 450             // update baro data
 451             baro.dev.calculate(&baro.pressure, &baro.temperature);
 452             baro.altitude = pressureToAltitude(baro.pressure);
 453
 454             if (baroIsCalibrated()) {
 455                 // zero baro altitude
 456                 baro.altitude -= baroGroundAltitude;
 457             } else {
 458                 // establish stable baroGroundAltitude value to zero baro altitude with
 459                 performBaroCalibrationCycle(baro.altitude);
 460                 baro.altitude = 0.0f;
 461             }
 462
 463             DEBUG_SET(DEBUG_BARO, 1, lrintf(baro.pressure / 100.0f));   // hPa
 464             DEBUG_SET(DEBUG_BARO, 2, baro.temperature);                 // c°C
 465             DEBUG_SET(DEBUG_BARO, 3, lrintf(baro.altitude));            // cm
 466
 467             if (baro.dev.combined_read) {
 468                 state = BARO_STATE_PRESSURE_START;
 469             } else {
 470                 state = BARO_STATE_TEMPERATURE_START;
 471             }
 472             break;
 473     }
 474
 475     // Where we are using a state machine call schedulerIgnoreTaskExecRate() for all states bar one
 476     if (state != BARO_STATE_PRESSURE_START) {
 477         schedulerIgnoreTaskExecRate();
 478     }
 479
 480     executeTimeUs = micros() - currentTimeUs;
 481
 482     if (executeTimeUs > baroStateDurationUs[oldState]) {
 483         baroStateDurationUs[oldState] = executeTimeUs;
 484     }
 485
 486     schedulerSetNextStateTime(baroStateDurationUs[state]);
 487
 488     return sleepTime;
 489 }
 490
 491 float getBaroAltitude(void)
 492 {
 493     return baro.altitude;
 494 }
 495
 496 static void performBaroCalibrationCycle(const float altitude)
 497 {
 498     static uint16_t cycleCount = 0;
 499
 500     baroGroundAltitude += altitude;
 501     cycleCount++;
 502
 503     if (cycleCount >= calibrationCycles) {
 504         baroGroundAltitude /= cycleCount;  // simple average
 505         baroCalibrated = true;
 506         cycleCount = 0;
 507     }
 508 }
 509
 510 #endif /* BARO */