rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
atbetaflight/src/main/sensors/barometer.c

495 lines
15 KiB
C
Raw Blame History

/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <math.h>
#include "platform.h"
#ifdef USE_BARO
#include "build/debug.h"
#include "common/maths.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "drivers/barometer/barometer.h"
#include "drivers/barometer/barometer_bmp085.h"
#include "drivers/barometer/barometer_bmp280.h"
#include "drivers/barometer/barometer_bmp388.h"
#include "drivers/barometer/barometer_dps310.h"
#include "drivers/barometer/barometer_qmp6988.h"
#include "drivers/barometer/barometer_fake.h"
#include "drivers/barometer/barometer_ms5611.h"
#include "drivers/barometer/barometer_lps.h"
#include "drivers/bus.h"
#include "drivers/bus_i2c_busdev.h"
#include "drivers/bus_spi.h"
#include "drivers/io.h"
#include "drivers/time.h"
#include "fc/runtime_config.h"
#include "sensors/sensors.h"
#include "scheduler/scheduler.h"
#include "barometer.h"
baro_t baro; // barometer access functions
PG_REGISTER_WITH_RESET_FN(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 1);
void pgResetFn_barometerConfig(barometerConfig_t *barometerConfig)
{
barometerConfig->baro_sample_count = 21;
barometerConfig->baro_noise_lpf = 600;
barometerConfig->baro_cf_vel = 985;
barometerConfig->baro_hardware = BARO_DEFAULT;
// For backward compatibility; ceate a valid default value for bus parameters
//
// 1. If DEFAULT_BARO_xxx is defined, use it.
// 2. Determine default based on USE_BARO_xxx
// a. Precedence is in the order of popularity; BMP388, BMP280, MS5611 then BMP085, then
// b. If SPI variant is specified, it is likely onboard, so take it.
#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)
#if defined(USE_BARO_DPS310) || defined(USE_BARO_SPI_DPS310)
#if defined(USE_BARO_SPI_DPS310)
#define DEFAULT_BARO_SPI_DPS310
#else
#define DEFAULT_BARO_DPS310
#endif
#elif defined(USE_BARO_BMP388) || defined(USE_BARO_SPI_BMP388)
#if defined(USE_BARO_SPI_BMP388)
#define DEFAULT_BARO_SPI_BMP388
#else
#define DEFAULT_BARO_BMP388
#endif
#elif defined(USE_BARO_BMP280) || defined(USE_BARO_SPI_BMP280)
#if defined(USE_BARO_SPI_BMP280)
#define DEFAULT_BARO_SPI_BMP280
#else
#define DEFAULT_BARO_BMP280
#endif
#elif defined(USE_BARO_MS5611) || defined(USE_BARO_SPI_MS5611)
#if defined(USE_BARO_SPI_MS5611)
#define DEFAULT_BARO_SPI_MS5611
#else
#define DEFAULT_BARO_MS5611
#endif
#elif defined(USE_BARO_QMP6988) || defined(USE_BARO_SPI_QMP6988)
#if defined(USE_BARO_SPI_QMP6988)
#define DEFAULT_BARO_SPI_QMP6988
#else
#define DEFAULT_BARO_QMP6988
#endif
#elif defined(USE_BARO_SPI_LPS)
#define DEFAULT_BARO_SPI_LPS
#elif defined(DEFAULT_BARO_BMP085)
#define DEFAULT_BARO_BMP085
#endif
#endif
#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)
barometerConfig->baro_busType = BUS_TYPE_SPI;
barometerConfig->baro_spi_device = SPI_DEV_TO_CFG(spiDeviceByInstance(BARO_SPI_INSTANCE));
barometerConfig->baro_spi_csn = IO_TAG(BARO_CS_PIN);
barometerConfig->baro_i2c_device = I2C_DEV_TO_CFG(I2CINVALID);
barometerConfig->baro_i2c_address = 0;
#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)
// All I2C devices shares a default config with address = 0 (per device default)
barometerConfig->baro_busType = BUS_TYPE_I2C;
barometerConfig->baro_i2c_device = I2C_DEV_TO_CFG(BARO_I2C_INSTANCE);
barometerConfig->baro_i2c_address = 0;
barometerConfig->baro_spi_device = SPI_DEV_TO_CFG(SPIINVALID);
barometerConfig->baro_spi_csn = IO_TAG_NONE;
#else
barometerConfig->baro_hardware = BARO_NONE;
barometerConfig->baro_busType = BUS_TYPE_NONE;
barometerConfig->baro_i2c_device = I2C_DEV_TO_CFG(I2CINVALID);
barometerConfig->baro_i2c_address = 0;
barometerConfig->baro_spi_device = SPI_DEV_TO_CFG(SPIINVALID);
barometerConfig->baro_spi_csn = IO_TAG_NONE;
#endif
barometerConfig->baro_eoc_tag = IO_TAG(BARO_EOC_PIN);
barometerConfig->baro_xclr_tag = IO_TAG(BARO_XCLR_PIN);
}
static uint16_t calibratingB = 0; // baro calibration = get new ground pressure value
static int32_t baroPressure = 0;
static int32_t baroTemperature = 0;
static int32_t baroGroundAltitude = 0;
static int32_t baroGroundPressure = 8*101325;
static uint32_t baroPressureSum = 0;
#define CALIBRATING_BARO_CYCLES 200 // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
#define SET_GROUND_LEVEL_BARO_CYCLES 10 // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)
static bool baroReady = false;
void baroPreInit(void)
{
#ifdef USE_SPI
if (barometerConfig()->baro_busType == BUS_TYPE_SPI) {
spiPreinitRegister(barometerConfig()->baro_spi_csn, IOCFG_IPU, 1);
}
#endif
}
bool baroDetect(baroDev_t *baroDev, baroSensor_e baroHardwareToUse)
{
extDevice_t *dev = &baroDev->dev;
// Detect what pressure sensors are available. baro->update() is set to sensor-specific update function
baroSensor_e baroHardware = baroHardwareToUse;
#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)
UNUSED(dev);
#endif
switch (barometerConfig()->baro_busType) {
#ifdef USE_I2C
case BUS_TYPE_I2C:
i2cBusSetInstance(dev, barometerConfig()->baro_i2c_device);
dev->busType_u.i2c.address = barometerConfig()->baro_i2c_address;
break;
#endif
#ifdef USE_SPI
case BUS_TYPE_SPI:
{
if (!spiSetBusInstance(dev, barometerConfig()->baro_spi_device, OWNER_BARO_CS)) {
return false;
}
dev->busType_u.spi.csnPin = IOGetByTag(barometerConfig()->baro_spi_csn);
}
break;
#endif
default:
return false;
}
switch (baroHardware) {
case BARO_DEFAULT:
FALLTHROUGH;
case BARO_BMP085:
#ifdef USE_BARO_BMP085
{
static bmp085Config_t defaultBMP085Config;
defaultBMP085Config.xclrTag = barometerConfig()->baro_xclr_tag;
defaultBMP085Config.eocTag = barometerConfig()->baro_eoc_tag;
static const bmp085Config_t *bmp085Config = &defaultBMP085Config;
if (bmp085Detect(bmp085Config, baroDev)) {
baroHardware = BARO_BMP085;
break;
}
}
#endif
FALLTHROUGH;
case BARO_MS5611:
#if defined(USE_BARO_MS5611) || defined(USE_BARO_SPI_MS5611)
if (ms5611Detect(baroDev)) {
baroHardware = BARO_MS5611;
break;
}
#endif
FALLTHROUGH;
case BARO_LPS:
#if defined(USE_BARO_SPI_LPS)
if (lpsDetect(baroDev)) {
baroHardware = BARO_LPS;
break;
}
#endif
FALLTHROUGH;
case BARO_DPS310:
#if defined(USE_BARO_DPS310) || defined(USE_BARO_SPI_DPS310)
{
if (baroDPS310Detect(baroDev)) {
baroHardware = BARO_DPS310;
break;
}
}
#endif
FALLTHROUGH;
case BARO_BMP388:
#if defined(USE_BARO_BMP388) || defined(USE_BARO_SPI_BMP388)
{
static bmp388Config_t defaultBMP388Config;
defaultBMP388Config.eocTag = barometerConfig()->baro_eoc_tag;
static const bmp388Config_t *bmp388Config = &defaultBMP388Config;
if (bmp388Detect(bmp388Config, baroDev)) {
baroHardware = BARO_BMP388;
break;
}
}
#endif
FALLTHROUGH;
case BARO_BMP280:
#if defined(USE_BARO_BMP280) || defined(USE_BARO_SPI_BMP280)
if (bmp280Detect(baroDev)) {
baroHardware = BARO_BMP280;
break;
}
#endif
FALLTHROUGH;
case BARO_QMP6988:
#if defined(USE_BARO_QMP6988) || defined(USE_BARO_SPI_QMP6988)
if (qmp6988Detect(baroDev)) {
baroHardware = BARO_QMP6988;
break;
}
#endif
FALLTHROUGH;
case BARO_NONE:
baroHardware = BARO_NONE;
break;
}
if (baroHardware == BARO_NONE) {
return false;
}
detectedSensors[SENSOR_INDEX_BARO] = baroHardware;
sensorsSet(SENSOR_BARO);
return true;
}
bool baroIsCalibrationComplete(void)
{
return calibratingB == 0;
}
static void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
{
calibratingB = calibrationCyclesRequired;
}
void baroStartCalibration(void)
{
baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
}
void baroSetGroundLevel(void)
{
baroSetCalibrationCycles(SET_GROUND_LEVEL_BARO_CYCLES);
}
#define PRESSURE_SAMPLES_MEDIAN 3
static int32_t applyBarometerMedianFilter(int32_t newPressureReading)
{
static int32_t barometerFilterSamples[PRESSURE_SAMPLES_MEDIAN];
static int currentFilterSampleIndex = 0;
static bool medianFilterReady = false;
int nextSampleIndex;
nextSampleIndex = (currentFilterSampleIndex + 1);
if (nextSampleIndex == PRESSURE_SAMPLES_MEDIAN) {
nextSampleIndex = 0;
medianFilterReady = true;
}
barometerFilterSamples[currentFilterSampleIndex] = newPressureReading;
currentFilterSampleIndex = nextSampleIndex;
if (medianFilterReady)
return quickMedianFilter3(barometerFilterSamples);
else
return newPressureReading;
}
static uint32_t recalculateBarometerTotal(uint32_t pressureTotal, int32_t newPressureReading)
{
static int32_t barometerSamples[BARO_SAMPLE_COUNT_MAX + 1];
static int currentSampleIndex = 0;
int nextSampleIndex;
// store current pressure in barometerSamples
if (currentSampleIndex >= barometerConfig()->baro_sample_count) {
nextSampleIndex = 0;
baroReady = true;
} else {
nextSampleIndex = (currentSampleIndex + 1);
}
barometerSamples[currentSampleIndex] = applyBarometerMedianFilter(newPressureReading);
// recalculate pressure total
pressureTotal += barometerSamples[currentSampleIndex];
pressureTotal -= barometerSamples[nextSampleIndex];
currentSampleIndex = nextSampleIndex;
return pressureTotal;
}
typedef enum {
BAROMETER_NEEDS_TEMPERATURE_READ = 0,
BAROMETER_NEEDS_TEMPERATURE_SAMPLE,
BAROMETER_NEEDS_PRESSURE_START,
BAROMETER_NEEDS_PRESSURE_READ,
BAROMETER_NEEDS_PRESSURE_SAMPLE,
BAROMETER_NEEDS_TEMPERATURE_START
} barometerState_e;
bool isBaroReady(void) {
return baroReady;
}
uint32_t baroUpdate(void)
{
static barometerState_e state = BAROMETER_NEEDS_PRESSURE_START;
timeUs_t sleepTime = 1000; // Wait 1ms between states
if (debugMode == DEBUG_BARO) {
debug[0] = state;
}
// Tell the scheduler to ignore how long this task takes unless the pressure is being read
// as that takes the longest
if (state != BAROMETER_NEEDS_PRESSURE_READ) {
ignoreTaskTime();
}
switch (state) {
default:
case BAROMETER_NEEDS_TEMPERATURE_START:
baro.dev.start_ut(&baro.dev);
state = BAROMETER_NEEDS_TEMPERATURE_READ;
sleepTime = baro.dev.ut_delay;
break;
case BAROMETER_NEEDS_TEMPERATURE_READ:
if (baro.dev.read_ut(&baro.dev)) {
state = BAROMETER_NEEDS_TEMPERATURE_SAMPLE;
}
break;
case BAROMETER_NEEDS_TEMPERATURE_SAMPLE:
if (baro.dev.get_ut(&baro.dev)) {
state = BAROMETER_NEEDS_PRESSURE_START;
}
break;
case BAROMETER_NEEDS_PRESSURE_START:
baro.dev.start_up(&baro.dev);
state = BAROMETER_NEEDS_PRESSURE_READ;
sleepTime = baro.dev.up_delay;
break;
case BAROMETER_NEEDS_PRESSURE_READ:
if (baro.dev.read_up(&baro.dev)) {
state = BAROMETER_NEEDS_PRESSURE_SAMPLE;
} else {
ignoreTaskTime();
}
break;
case BAROMETER_NEEDS_PRESSURE_SAMPLE:
if (!baro.dev.get_up(&baro.dev)) {
break;
}
baro.dev.calculate(&baroPressure, &baroTemperature);
baro.baroPressure = baroPressure;
baro.baroTemperature = baroTemperature;
baroPressureSum = recalculateBarometerTotal(baroPressureSum, baroPressure);
if (baro.dev.combined_read) {
state = BAROMETER_NEEDS_PRESSURE_START;
} else {
state = BAROMETER_NEEDS_TEMPERATURE_START;
}
if (debugMode == DEBUG_BARO) {
debug[1] = baroTemperature;
debug[2] = baroPressure;
debug[3] = baroPressureSum;
}
sleepTime = baro.dev.ut_delay;
break;
}
return sleepTime;
}
static float pressureToAltitude(const float pressure)
{
return (1.0f - powf(pressure / 101325.0f, 0.190295f)) * 4433000.0f;
}
int32_t baroCalculateAltitude(void)
{
int32_t BaroAlt_tmp;
// calculates height from ground via baro readings
if (baroIsCalibrationComplete()) {
BaroAlt_tmp = lrintf(pressureToAltitude((float)(baroPressureSum / barometerConfig()->baro_sample_count)));
BaroAlt_tmp -= baroGroundAltitude;
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
}
else {
baro.BaroAlt = 0;
}
return baro.BaroAlt;
}
void performBaroCalibrationCycle(void)
{
static int32_t savedGroundPressure = 0;
baroGroundPressure -= baroGroundPressure / 8;
baroGroundPressure += baroPressureSum / barometerConfig()->baro_sample_count;
baroGroundAltitude = (1.0f - pow_approx((baroGroundPressure / 8) / 101325.0f, 0.190259f)) * 4433000.0f;
if (baroGroundPressure == savedGroundPressure) {
calibratingB = 0;
} else {
calibratingB--;
savedGroundPressure = baroGroundPressure;
}
}
#endif /* BARO */
Reference in New Issue View Git Blame Copy Permalink