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_fake.h"
  44 #include "drivers/barometer/barometer_ms5611.h"
  45 #include "drivers/barometer/barometer_lps.h"
  46 #include "drivers/bus.h"
  47 #include "drivers/bus_i2c_busdev.h"
  48 #include "drivers/bus_spi.h"
  49 #include "drivers/io.h"
  50 #include "drivers/time.h"
  51
  52 #include "fc/runtime_config.h"
  53
  54 #include "sensors/sensors.h"
  55
  56 #include "scheduler/scheduler.h"
  57
  58 #include "barometer.h"
  59
  60 baro_t baro;                        // barometer access functions
  61
  62 PG_REGISTER_WITH_RESET_FN(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 2);
  63
  64 void pgResetFn_barometerConfig(barometerConfig_t *barometerConfig)
  65 {
  66     barometerConfig->baro_noise_lpf = 50;
  67     barometerConfig->baro_vario_lpf = 10;
  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 calibrationCycles = 0;      // baro calibration = get new ground pressure value
 143 static bool baroCalibrated = false;
 144 static int32_t baroPressure = 0;
 145 static int32_t baroTemperature = 0;
 146 static float baroGroundAltitude = 0.0f;
 147 static float baroAltitudeRaw = 0.0f;
 148 static pt2Filter_t baroUpsampleLpf;
 149
 150
 151 #define CALIBRATING_BARO_CYCLES 100 // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
 152 #define SET_GROUND_LEVEL_BARO_CYCLES 10 // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)
 153
 154 static bool baroReady = false;
 155 static bool baroSampleReady = false;
 156
 157 void baroPreInit(void)
 158 {
 159 #ifdef USE_SPI
 160     if (barometerConfig()->baro_busType == BUS_TYPE_SPI) {
 161         spiPreinitRegister(barometerConfig()->baro_spi_csn, IOCFG_IPU, 1);
 162     }
 163 #endif
 164 }
 165
 166 static bool baroDetect(baroDev_t *baroDev, baroSensor_e baroHardwareToUse);
 167
 168 void baroInit(void)
 169 {
 170     baroDetect(&baro.dev, barometerConfig()->baro_hardware);
 171
 172     const float cutoffHz = barometerConfig()->baro_noise_lpf / 100.0f;
 173     const float sampleTimeS = HZ_TO_INTERVAL(TASK_ALTITUDE_RATE_HZ);
 174     const float gain = pt2FilterGain(cutoffHz, sampleTimeS);
 175     pt2FilterInit(&baroUpsampleLpf, gain);
 176
 177     baroReady = true;
 178 }
 179
 180 static bool baroDetect(baroDev_t *baroDev, baroSensor_e baroHardwareToUse)
 181 {
 182     extDevice_t *dev = &baroDev->dev;
 183
 184     // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function
 185
 186     baroSensor_e baroHardware = baroHardwareToUse;
 187
 188 #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)
 189     UNUSED(dev);
 190 #endif
 191
 192     switch (barometerConfig()->baro_busType) {
 193 #ifdef USE_I2C
 194     case BUS_TYPE_I2C:
 195         i2cBusSetInstance(dev, barometerConfig()->baro_i2c_device);
 196         dev->busType_u.i2c.address = barometerConfig()->baro_i2c_address;
 197         break;
 198 #endif
 199
 200 #ifdef USE_SPI
 201     case BUS_TYPE_SPI:
 202         {
 203             if (!spiSetBusInstance(dev, barometerConfig()->baro_spi_device)) {
 204                 return false;
 205             }
 206
 207             dev->busType_u.spi.csnPin = IOGetByTag(barometerConfig()->baro_spi_csn);
 208         }
 209         break;
 210 #endif
 211
 212     default:
 213         return false;
 214     }
 215
 216     switch (baroHardware) {
 217     case BARO_DEFAULT:
 218         FALLTHROUGH;
 219
 220     case BARO_BMP085:
 221 #ifdef USE_BARO_BMP085
 222         {
 223             static bmp085Config_t defaultBMP085Config;
 224             defaultBMP085Config.xclrTag = barometerConfig()->baro_xclr_tag;
 225             defaultBMP085Config.eocTag = barometerConfig()->baro_eoc_tag;
 226
 227             static const bmp085Config_t *bmp085Config = &defaultBMP085Config;
 228
 229             if (bmp085Detect(bmp085Config, baroDev)) {
 230                 baroHardware = BARO_BMP085;
 231                 break;
 232             }
 233         }
 234 #endif
 235         FALLTHROUGH;
 236
 237     case BARO_MS5611:
 238 #if defined(USE_BARO_MS5611) || defined(USE_BARO_SPI_MS5611)
 239         if (ms5611Detect(baroDev)) {
 240             baroHardware = BARO_MS5611;
 241             break;
 242         }
 243 #endif
 244         FALLTHROUGH;
 245
 246     case BARO_LPS:
 247 #if defined(USE_BARO_SPI_LPS)
 248         if (lpsDetect(baroDev)) {
 249             baroHardware = BARO_LPS;
 250             break;
 251         }
 252 #endif
 253         FALLTHROUGH;
 254
 255     case BARO_DPS310:
 256 #if defined(USE_BARO_DPS310) || defined(USE_BARO_SPI_DPS310)
 257         {
 258             if (baroDPS310Detect(baroDev)) {
 259                 baroHardware = BARO_DPS310;
 260                 break;
 261             }
 262         }
 263 #endif
 264         FALLTHROUGH;
 265
 266     case BARO_BMP388:
 267 #if defined(USE_BARO_BMP388) || defined(USE_BARO_SPI_BMP388)
 268         {
 269             static bmp388Config_t defaultBMP388Config;
 270
 271             defaultBMP388Config.eocTag = barometerConfig()->baro_eoc_tag;
 272
 273             static const bmp388Config_t *bmp388Config = &defaultBMP388Config;
 274
 275             if (bmp388Detect(bmp388Config, baroDev)) {
 276                 baroHardware = BARO_BMP388;
 277                 break;
 278             }
 279         }
 280 #endif
 281         FALLTHROUGH;
 282
 283     case BARO_BMP280:
 284 #if defined(USE_BARO_BMP280) || defined(USE_BARO_SPI_BMP280)
 285         if (bmp280Detect(baroDev)) {
 286             baroHardware = BARO_BMP280;
 287             break;
 288         }
 289 #endif
 290         FALLTHROUGH;
 291
 292      case BARO_QMP6988:
 293 #if defined(USE_BARO_QMP6988) || defined(USE_BARO_SPI_QMP6988)
 294         if (qmp6988Detect(baroDev)) {
 295             baroHardware = BARO_QMP6988;
 296             break;
 297         }
 298 #endif
 299         FALLTHROUGH;
 300     case BARO_NONE:
 301         baroHardware = BARO_NONE;
 302         break;
 303     }
 304
 305     if (baroHardware == BARO_NONE) {
 306         return false;
 307     }
 308
 309     detectedSensors[SENSOR_INDEX_BARO] = baroHardware;
 310     sensorsSet(SENSOR_BARO);
 311     return true;
 312 }
 313
 314 bool baroIsCalibrated(void)
 315 {
 316     return baroCalibrated;
 317 }
 318
 319 static void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
 320 {
 321     calibrationCycles = calibrationCyclesRequired;
 322 }
 323
 324 void baroStartCalibration(void)
 325 {
 326     baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
 327     baroCalibrated = false;
 328 }
 329
 330 void baroSetGroundLevel(void)
 331 {
 332     baroSetCalibrationCycles(SET_GROUND_LEVEL_BARO_CYCLES);
 333     baroCalibrated = false;
 334 }
 335
 336 typedef enum {
 337     BARO_STATE_TEMPERATURE_READ = 0,
 338     BARO_STATE_TEMPERATURE_SAMPLE,
 339     BARO_STATE_PRESSURE_START,
 340     BARO_STATE_PRESSURE_READ,
 341     BARO_STATE_PRESSURE_SAMPLE,
 342     BARO_STATE_TEMPERATURE_START,
 343     BARO_STATE_COUNT
 344 } barometerState_e;
 345
 346 bool isBaroReady(void) {
 347     return baroReady;
 348 }
 349
 350 bool isBaroSampleReady(void)
 351 {
 352      if (baroSampleReady) {
 353          baroSampleReady = false;
 354          return true;
 355      }
 356      return false;
 357  }
 358
 359 static float pressureToAltitude(const float pressure)
 360 {
 361     return (1.0f - powf(pressure / 101325.0f, 0.190295f)) * 4433000.0f;
 362 }
 363
 364 uint32_t baroUpdate(timeUs_t currentTimeUs)
 365 {
 366     static timeUs_t baroStateDurationUs[BARO_STATE_COUNT];
 367     static barometerState_e state = BARO_STATE_PRESSURE_START;
 368     barometerState_e oldState = state;
 369     timeUs_t executeTimeUs;
 370     timeUs_t sleepTime = 1000; // Wait 1ms between states
 371
 372     DEBUG_SET(DEBUG_BARO, 0, state);
 373
 374     if (busBusy(&baro.dev.dev, NULL)) {
 375         // If the bus is busy, simply return to have another go later
 376         schedulerIgnoreTaskStateTime();
 377         return sleepTime;
 378     }
 379
 380     switch (state) {
 381         default:
 382         case BARO_STATE_TEMPERATURE_START:
 383             baro.dev.start_ut(&baro.dev);
 384             state = BARO_STATE_TEMPERATURE_READ;
 385             sleepTime = baro.dev.ut_delay;
 386             break;
 387
 388         case BARO_STATE_TEMPERATURE_READ:
 389             if (baro.dev.read_ut(&baro.dev)) {
 390                 state = BARO_STATE_TEMPERATURE_SAMPLE;
 391             } else {
 392                 // No action was taken as the read has not completed
 393                 schedulerIgnoreTaskExecTime();
 394             }
 395             break;
 396
 397         case BARO_STATE_TEMPERATURE_SAMPLE:
 398             if (baro.dev.get_ut(&baro.dev)) {
 399                 state = BARO_STATE_PRESSURE_START;
 400             } else {
 401                 // No action was taken as the read has not completed
 402                 schedulerIgnoreTaskExecTime();
 403             }
 404             break;
 405
 406         case BARO_STATE_PRESSURE_START:
 407             baro.dev.start_up(&baro.dev);
 408             state = BARO_STATE_PRESSURE_READ;
 409             sleepTime = baro.dev.up_delay;
 410             break;
 411
 412         case BARO_STATE_PRESSURE_READ:
 413             if (baro.dev.read_up(&baro.dev)) {
 414                 state = BARO_STATE_PRESSURE_SAMPLE;
 415             } else {
 416                 // No action was taken as the read has not completed
 417                 schedulerIgnoreTaskExecTime();
 418             }
 419             break;
 420
 421         case BARO_STATE_PRESSURE_SAMPLE:
 422             if (!baro.dev.get_up(&baro.dev)) {
 423                 // No action was taken as the read has not completed
 424                 schedulerIgnoreTaskExecTime();
 425                 break;
 426             }
 427             baro.dev.calculate(&baroPressure, &baroTemperature);
 428             baro.baroPressure = baroPressure;
 429             baro.baroTemperature = baroTemperature;
 430
 431             baroAltitudeRaw = pressureToAltitude(baroPressure);
 432
 433             performBaroCalibrationCycle();
 434
 435             DEBUG_SET(DEBUG_BARO, 1, baro.baroTemperature);
 436             DEBUG_SET(DEBUG_BARO, 2, baroAltitudeRaw - baroGroundAltitude);
 437
 438             baroSampleReady = true;
 439
 440             if (baro.dev.combined_read) {
 441                 state = BARO_STATE_PRESSURE_START;
 442             } else {
 443                 state = BARO_STATE_TEMPERATURE_START;
 444             }
 445             break;
 446     }
 447
 448     // Where we are using a state machine call schedulerIgnoreTaskExecRate() for all states bar one
 449     if (state != BARO_STATE_PRESSURE_START) {
 450         schedulerIgnoreTaskExecRate();
 451     }
 452
 453     executeTimeUs = micros() - currentTimeUs;
 454
 455     if (executeTimeUs > baroStateDurationUs[oldState]) {
 456         baroStateDurationUs[oldState] = executeTimeUs;
 457     }
 458
 459     schedulerSetNextStateTime(baroStateDurationUs[state]);
 460
 461     return sleepTime;
 462 }
 463
 464 // baroAltitudeRaw will get updated in the BARO task while baroUpsampleAltitude() will run in the ALTITUDE task.
 465 float baroUpsampleAltitude(void)
 466 {
 467     const float baroAltitudeRawFiltered = pt2FilterApply(&baroUpsampleLpf, baroAltitudeRaw);
 468     if (baroIsCalibrated()) {
 469         baro.BaroAlt = baroAltitudeRawFiltered - baroGroundAltitude;
 470     } else {
 471         baro.BaroAlt = 0.0f;
 472     }
 473     DEBUG_SET(DEBUG_BARO, 3, baro.BaroAlt); // cm
 474     return baro.BaroAlt;
 475 }
 476
 477 void performBaroCalibrationCycle(void)
 478 {
 479     static uint16_t cycleCount = 0;
 480     if (!baroCalibrated) {
 481         cycleCount++;
 482         baroGroundAltitude += baroAltitudeRaw;
 483         if (cycleCount == calibrationCycles) {
 484             baroGroundAltitude = baroGroundAltitude / calibrationCycles; // simple average
 485             baroCalibrated = true;
 486             cycleCount = 0;
 487         }
 488     }
 489 }
 490
 491 #endif /* BARO */