void baroPreInit(void)
{
#ifdef USE_SPI
if (barometerConfig()->baro_bustype == BUSTYPE_SPI) {
spiPreinitRegister(barometerConfig()->baro_spi_csn, IOCFG_IPU, 1);
}
#endif
}
bool sensorsAutodetect(void)
{
// gyro must be initialised before accelerometer
bool gyroDetected = gyroInit();
#ifdef USE_ACC
if (gyroDetected) {
accInit(gyro.targetLooptime);
}
#endif
#ifdef USE_MAG
compassInit();
#endif
#ifdef USE_BARO
baroDetect(&baro.dev, barometerConfig()->baro_hardware);
#endif
#ifdef USE_RANGEFINDER
rangefinderInit();
#endif
#ifdef USE_ADC_INTERNAL
adcInternalInit();
#endif
return gyroDetected;
}
uint32_t baroUpdate(void)
{
static barometerState_e state = BAROMETER_NEEDS_SAMPLES;
switch (state) {
default:
case BAROMETER_NEEDS_SAMPLES:
baro.dev.get_ut(&baro.dev);
baro.dev.start_up(&baro.dev);
state = BAROMETER_NEEDS_CALCULATION;
return baro.dev.up_delay;
break;
case BAROMETER_NEEDS_CALCULATION:
baro.dev.get_up(&baro.dev);
baro.dev.start_ut(&baro.dev);
baro.dev.calculate(&baroPressure, &baroTemperature);
baro.baroPressure = baroPressure;
baro.baroTemperature = baroTemperature;
baroPressureSum = recalculateBarometerTotal(barometerConfig()->baro_sample_count, baroPressureSum, baroPressure);
state = BAROMETER_NEEDS_SAMPLES;
return baro.dev.ut_delay;
break;
}
}
int32_t baroCalculateAltitude(void)
{
int32_t BaroAlt_tmp;
// calculates height from ground via baro readings
// see: https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Baro/AP_Baro.cpp#L140
if (isBaroCalibrationComplete()) {
BaroAlt_tmp = lrintf((1.0f - pow_approx((float)(baroPressureSum / PRESSURE_SAMPLE_COUNT) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm
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 calculateEstimatedAltitude(timeUs_t currentTimeUs)
{
static timeUs_t previousTimeUs = 0;
const uint32_t dTime = currentTimeUs - previousTimeUs;
if (dTime < BARO_UPDATE_FREQUENCY_40HZ) {
return;
}
previousTimeUs = currentTimeUs;
static float vel = 0.0f;
static float accAlt = 0.0f;
int32_t baroAlt = 0;
#ifdef BARO
if (sensors(SENSOR_BARO)) {
if (!isBaroCalibrationComplete()) {
performBaroCalibrationCycle();
vel = 0;
accAlt = 0;
} else {
baroAlt = baroCalculateAltitude();
estimatedAltitude = baroAlt;
}
}
#endif
#ifdef SONAR
if (sensors(SENSOR_SONAR)) {
int32_t sonarAlt = sonarCalculateAltitude(sonarRead(), getCosTiltAngle());
if (sonarAlt > 0 && sonarAlt >= sonarCfAltCm && sonarAlt <= sonarMaxAltWithTiltCm) {
// SONAR in range, so use complementary filter
float sonarTransition = (float)(sonarMaxAltWithTiltCm - sonarAlt) / (sonarMaxAltWithTiltCm - sonarCfAltCm);
sonarAlt = (float)sonarAlt * sonarTransition + baroAlt * (1.0f - sonarTransition);
estimatedAltitude = sonarAlt;
}
}
#endif
float accZ_tmp = 0;
#ifdef ACC
if (sensors(SENSOR_ACC)) {
const float dt = accTimeSum * 1e-6f; // delta acc reading time in seconds
// Integrator - velocity, cm/sec
if (accSumCount) {
accZ_tmp = (float)accSum[2] / accSumCount;
}
const float vel_acc = accZ_tmp * accVelScale * (float)accTimeSum;
// Integrator - Altitude in cm
accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2)
accAlt = accAlt * CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_alt) + (float)baro.BaroAlt * (1.0f - CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_alt)); // complementary filter for altitude estimation (baro & acc)
vel += vel_acc;
estimatedAltitude = accAlt;
}
#endif
DEBUG_SET(DEBUG_ALTITUDE, DEBUG_ALTITUDE_ACC, accSum[2] / accSumCount);
DEBUG_SET(DEBUG_ALTITUDE, DEBUG_ALTITUDE_VEL, vel);
DEBUG_SET(DEBUG_ALTITUDE, DEBUG_ALTITUDE_HEIGHT, accAlt);
imuResetAccelerationSum();
int32_t baroVel = 0;
#ifdef BARO
if (sensors(SENSOR_BARO)) {
if (!isBaroCalibrationComplete()) {
return;
}
static int32_t lastBaroAlt = 0;
baroVel = (baroAlt - lastBaroAlt) * 1000000.0f / dTime;
lastBaroAlt = baroAlt;
baroVel = constrain(baroVel, -1500, 1500); // constrain baro velocity +/- 1500cm/s
baroVel = applyDeadband(baroVel, 10); // to reduce noise near zero
}
#endif
// apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
// By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
vel = vel * CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_vel) + baroVel * (1.0f - CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_vel));
int32_t vel_tmp = lrintf(vel);
// set vario
estimatedVario = applyDeadband(vel_tmp, 5);
static float accZ_old = 0.0f;
altHoldThrottleAdjustment = calculateAltHoldThrottleAdjustment(vel_tmp, accZ_tmp, accZ_old);
accZ_old = accZ_tmp;
}
void calculateEstimatedAltitude(uint32_t currentTime)
{
static uint32_t previousTime;
uint32_t dTime;
int32_t baroVel;
float dt;
float vel_acc;
int32_t vel_tmp;
float accZ_tmp;
static float accZ_old = 0.0f;
static float vel = 0.0f;
static float accAlt = 0.0f;
static int32_t lastBaroAlt;
#ifdef SONAR
int32_t sonarAlt = SONAR_OUT_OF_RANGE;
static int32_t baroAlt_offset = 0;
float sonarTransition;
#endif
dTime = currentTime - previousTime;
if (dTime < BARO_UPDATE_FREQUENCY_40HZ)
return;
previousTime = currentTime;
#ifdef BARO
if (!isBaroCalibrationComplete()) {
performBaroCalibrationCycle();
vel = 0;
accAlt = 0;
}
BaroAlt = baroCalculateAltitude();
#else
BaroAlt = 0;
#endif
#ifdef SONAR
sonarAlt = sonarRead();
sonarAlt = sonarCalculateAltitude(sonarAlt, getCosTiltAngle());
if (sonarAlt > 0 && sonarAlt < sonarCfAltCm) {
// just use the SONAR
baroAlt_offset = BaroAlt - sonarAlt;
BaroAlt = sonarAlt;
} else {
BaroAlt -= baroAlt_offset;
if (sonarAlt > 0 && sonarAlt <= sonarMaxAltWithTiltCm) {
// SONAR in range, so use complementary filter
sonarTransition = (float)(sonarMaxAltWithTiltCm - sonarAlt) / (sonarMaxAltWithTiltCm - sonarCfAltCm);
BaroAlt = sonarAlt * sonarTransition + BaroAlt * (1.0f - sonarTransition);
}
}
#endif
dt = accTimeSum * 1e-6f; // delta acc reading time in seconds
// Integrator - velocity, cm/sec
if (accSumCount) {
accZ_tmp = (float)accSum[2] / (float)accSumCount;
} else {
accZ_tmp = 0;
}
vel_acc = accZ_tmp * accVelScale * (float)accTimeSum;
// Integrator - Altitude in cm
accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2)
accAlt = accAlt * barometerConfig()->baro_cf_alt + (float)BaroAlt * (1.0f - barometerConfig()->baro_cf_alt); // complementary filter for altitude estimation (baro & acc)
vel += vel_acc;
#ifdef DEBUG_ALT_HOLD
debug[1] = accSum[2] / accSumCount; // acceleration
debug[2] = vel; // velocity
debug[3] = accAlt; // height
#endif
imuResetAccelerationSum();
#ifdef BARO
if (!isBaroCalibrationComplete()) {
return;
}
#endif
#ifdef SONAR
if (sonarAlt > 0 && sonarAlt < sonarCfAltCm) {
// the sonar has the best range
EstAlt = BaroAlt;
} else {
EstAlt = accAlt;
}
#else
EstAlt = accAlt;
#endif
baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dTime;
lastBaroAlt = BaroAlt;
baroVel = constrain(baroVel, -1500, 1500); // constrain baro velocity +/- 1500cm/s
baroVel = applyDeadband(baroVel, 10); // to reduce noise near zero
// apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
// By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
vel = vel * barometerConfig()->baro_cf_vel + baroVel * (1.0f - barometerConfig()->baro_cf_vel);
vel_tmp = lrintf(vel);
// set vario
vario = applyDeadband(vel_tmp, 5);
altHoldThrottleAdjustment = calculateAltHoldThrottleAdjustment(vel_tmp, accZ_tmp, accZ_old);
accZ_old = accZ_tmp;
}
bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse)
{
// 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_BMP280) && !defined(USE_BARO_SPI_BMP280)&& !defined(USE_BARO_QMP6988) && !defined(USE_BARO_SPI_QMP6988)
UNUSED(dev);
#endif
switch (barometerConfig()->baro_bustype) {
#ifdef USE_I2C
case BUSTYPE_I2C:
dev->busdev.bustype = BUSTYPE_I2C;
dev->busdev.busdev_u.i2c.device = I2C_CFG_TO_DEV(barometerConfig()->baro_i2c_device);
dev->busdev.busdev_u.i2c.address = barometerConfig()->baro_i2c_address;
break;
#endif
#ifdef USE_SPI
case BUSTYPE_SPI:
{
SPI_TypeDef *instance = spiInstanceByDevice(SPI_CFG_TO_DEV(barometerConfig()->baro_spi_device));
if (!instance) {
return false;
}
dev->busdev.bustype = BUSTYPE_SPI;
spiBusSetInstance(&dev->busdev, instance);
dev->busdev.busdev_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
{
const bmp085Config_t *bmp085Config = NULL;
#if defined(BARO_XCLR_GPIO) && defined(BARO_EOC_GPIO)
static const bmp085Config_t defaultBMP085Config = {
.xclrIO = IO_TAG(BARO_XCLR_PIN),
.eocIO = IO_TAG(BARO_EOC_PIN),
};
bmp085Config = &defaultBMP085Config;
#endif
if (bmp085Detect(bmp085Config, dev)) {
baroHardware = BARO_BMP085;
break;
}
}
#endif
FALLTHROUGH;
case BARO_MS5611:
#if defined(USE_BARO_MS5611) || defined(USE_BARO_SPI_MS5611)
if (ms5611Detect(dev)) {
baroHardware = BARO_MS5611;
break;
}
#endif
FALLTHROUGH;
case BARO_LPS:
#if defined(USE_BARO_SPI_LPS)
if (lpsDetect(dev)) {
baroHardware = BARO_LPS;
break;
}
#endif
FALLTHROUGH;
case BARO_BMP280:
#if defined(USE_BARO_BMP280) || defined(USE_BARO_SPI_BMP280)
if (bmp280Detect(dev)) {
baroHardware = BARO_BMP280;
break;
}
#endif
FALLTHROUGH;
case BARO_QMP6988:
#if defined(USE_BARO_QMP6988) || defined(USE_BARO_SPI_QMP6988)
if (qmp6988Detect(dev)) {
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;
}
