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