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