Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-14 20:10:18 +03:00
Code Issues Wiki Activity

Baro fixes 1 (#7711)

Browse source 
Baro fixes 1
This commit is contained in:
Michael Keller 2019-06-05 01:07:24 +12:00 • committed by GitHub
commit 32416c7972
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
13 changed files with 97 additions and 87 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/main/build/debug.c
View file

@ -90,4 +90,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"CRSF_LINK_STATISTICS_UPLINK",
"CRSF_LINK_STATISTICS_PWR",
"CRSF_LINK_STATISTICS_DOWN",
"BARO",
};

1
src/main/build/debug.h
View file

@ -106,6 +106,7 @@ typedef enum {
DEBUG_CRSF_LINK_STATISTICS_UPLINK,
DEBUG_CRSF_LINK_STATISTICS_PWR,
DEBUG_CRSF_LINK_STATISTICS_DOWN,
DEBUG_BARO,
DEBUG_COUNT
} debugType_e;

2
src/main/cli/cli.c
View file

@ -4577,6 +4577,8 @@ const cliResourceValue_t resourceTable[] = {
#endif
#ifdef USE_BARO
DEFS( OWNER_BARO_CS, PG_BAROMETER_CONFIG, barometerConfig_t, baro_spi_csn ),
DEFS( OWNER_BARO_EOC, PG_BAROMETER_CONFIG, barometerConfig_t, baro_eoc_tag ),
DEFS( OWNER_BARO_XCLR, PG_BAROMETER_CONFIG, barometerConfig_t, baro_xclr_tag ),
#endif
#ifdef USE_MAG
DEFS( OWNER_COMPASS_CS, PG_COMPASS_CONFIG, compassConfig_t, mag_spi_csn ),

95
src/main/drivers/barometer/barometer_bmp085.c
View file

@ -39,21 +39,18 @@
#ifdef USE_BARO
#if defined(BARO_EOC_GPIO)
static IO_t eocIO;
static bool isConversionComplete = false;
static bool isEOCConnected = true;
static bool isEOCConnected = false;
// EXTI14 for BMP085 End of Conversion Interrupt
void bmp085_extiHandler(extiCallbackRec_t* cb)
#ifdef USE_EXTI
static IO_t eocIO;
static extiCallbackRec_t exti;
static void bmp085_extiHandler(extiCallbackRec_t* cb)
{
UNUSED(cb);
isConversionComplete = true;
}
bool bmp085TestEOCConnected(const bmp085Config_t *config);
#endif
typedef struct {
@ -132,31 +129,18 @@ static int32_t bmp085_get_temperature(uint32_t ut);
static int32_t bmp085_get_pressure(uint32_t up);
STATIC_UNIT_TESTED void bmp085_calculate(int32_t *pressure, int32_t *temperature);
static IO_t xclrIO;
static bool bmp085TestEOCConnected(baroDev_t *baro, const bmp085Config_t *config);
#ifdef BARO_XCLR_PIN
static IO_t xclrIO = IO_NONE;
#define BMP085_OFF IOLo(xclrIO);
#define BMP085_ON IOHi(xclrIO);
#else
#define BMP085_OFF
#define BMP085_ON
#endif
void bmp085InitXclrIO(const bmp085Config_t *config)
static void bmp085InitXclrIO(const bmp085Config_t *config)
{
if (!xclrIO && config && config->xclrIO) {
xclrIO = IOGetByTag(config->xclrIO);
IOInit(xclrIO, OWNER_BARO_CS, 0);
xclrIO = IOGetByTag(config->xclrTag);
IOInit(xclrIO, OWNER_BARO_XCLR, 0);
IOConfigGPIO(xclrIO, IOCFG_OUT_PP);
}
}
void bmp085Disable(const bmp085Config_t *config)
{
bmp085InitXclrIO(config);
BMP085_OFF;
}
bool bmp085Detect(const bmp085Config_t *config, baroDev_t *baro)
{
@ -164,26 +148,20 @@ bool bmp085Detect(const bmp085Config_t *config, baroDev_t *baro)
bool ack;
bool defaultAddressApplied = false;
#if defined(BARO_EOC_GPIO)
IO_t eocIO = IO_NONE;
#endif
if (bmp085InitDone)
return true;
bmp085InitXclrIO(config);
BMP085_ON; // enable baro
#if defined(BARO_EOC_GPIO) && defined(USE_EXTI)
if (config && config->eocIO) {
eocIO = IOGetByTag(config->eocIO);
#ifdef USE_EXTI
// EXTI interrupt for barometer EOC
IOInit(eocIO, OWNER_BARO_EXTI, 0);
IOConfigGPIO(eocIO, Mode_IN_FLOATING);
EXTIHandlerInit(&bmp085_extiCallbackRec, bmp085_extiHandler);
EXTIConfig(eocIO, &bmp085_extiCallbackRec, NVIC_PRIO_BARO_EXTI, IOCFG_IN_FLOATING, EXTI_TRIGGER_RISING);
eocIO = IOGetByTag(config->eocTag);
IOInit(eocIO, OWNER_BARO_EOC, 0);
EXTIHandlerInit(&exti, bmp085_extiHandler);
EXTIConfig(eocIO, &exti, NVIC_PRIO_BARO_EXTI, IOCFG_IN_FLOATING, EXTI_TRIGGER_RISING);
EXTIEnable(eocIO, true);
}
#else
UNUSED(config);
#endif
@ -215,17 +193,19 @@ bool bmp085Detect(const bmp085Config_t *config, baroDev_t *baro)
baro->start_up = bmp085_start_up;
baro->get_up = bmp085_get_up;
baro->calculate = bmp085_calculate;
#if defined(BARO_EOC_GPIO)
isEOCConnected = bmp085TestEOCConnected(config);
#endif
isEOCConnected = bmp085TestEOCConnected(baro, config);
bmp085InitDone = true;
return true;
}
}
#if defined(BARO_EOC_GPIO)
if (eocIO)
#ifdef USE_EXTI
if (eocIO) {
IORelease(eocIO);
EXTIRelease(eocIO);
}
#endif
BMP085_OFF;
@ -292,9 +272,8 @@ static int32_t bmp085_get_pressure(uint32_t up)
static void bmp085_start_ut(baroDev_t *baro)
{
#if defined(BARO_EOC_GPIO)
isConversionComplete = false;
#endif
busWriteRegister(&baro->busdev, BMP085_CTRL_MEAS_REG, BMP085_T_MEASURE);
}
@ -302,12 +281,10 @@ static void bmp085_get_ut(baroDev_t *baro)
{
uint8_t data[2];
#if defined(BARO_EOC_GPIO)
// return old baro value if conversion time exceeds datasheet max when EOC is connected
if ((isEOCConnected) && (!isConversionComplete)) {
return;
}
#endif
busReadRegisterBuffer(&baro->busdev, BMP085_ADC_OUT_MSB_REG, data, 2);
bmp085_ut = (data[0] << 8) | data[1];
@ -319,9 +296,7 @@ static void bmp085_start_up(baroDev_t *baro)
ctrl_reg_data = BMP085_P_MEASURE + (bmp085.oversampling_setting << 6);
#if defined(BARO_EOC_GPIO)
isConversionComplete = false;
#endif
busWriteRegister(&baro->busdev, BMP085_CTRL_MEAS_REG, ctrl_reg_data);
}
@ -334,12 +309,10 @@ static void bmp085_get_up(baroDev_t *baro)
{
uint8_t data[3];
#if defined(BARO_EOC_GPIO)
// return old baro value if conversion time exceeds datasheet max when EOC is connected
if ((isEOCConnected) && (!isConversionComplete)) {
return;
}
#endif
busReadRegisterBuffer(&baro->busdev, BMP085_ADC_OUT_MSB_REG, data, 3);
bmp085_up = (((uint32_t) data[0] << 16) | ((uint32_t) data[1] << 8) | (uint32_t) data[2])
@ -381,13 +354,18 @@ static void bmp085_get_cal_param(busDevice_t *busdev)
bmp085.cal_param.md = (data[20] << 8) | data[21];
}
#if defined(BARO_EOC_GPIO)
bool bmp085TestEOCConnected(const bmp085Config_t *config)
static bool bmp085TestEOCConnected(baroDev_t *baro, const bmp085Config_t *config)
{
UNUSED(config);
#ifdef USE_EXTI
if (!bmp085InitDone && eocIO) {
bmp085_start_ut();
// EOC should be low at this point. If not, assume EOC is not working
if (IORead(eocIO)) {
return false;
}
bmp085_start_ut(baro);
delayMicroseconds(UT_DELAY * 2); // wait twice as long as normal, just to be sure
// conversion should have finished now so check if EOC is high
@ -396,8 +374,11 @@ bool bmp085TestEOCConnected(const bmp085Config_t *config)
return true;
}
}
return false; // assume EOC is not connected
}
#else
UNUSED(baro);
#endif
return false; // assume EOC is not connected
}
#endif /* BARO */

9
src/main/drivers/barometer/barometer_bmp085.h
View file

@ -25,16 +25,11 @@
#define BMP085_I2C_ADDR 0x77
typedef struct bmp085Config_s {
ioTag_t xclrIO;
ioTag_t eocIO;
ioTag_t xclrTag;
ioTag_t eocTag;
} bmp085Config_t;
bool bmp085Detect(const bmp085Config_t *config, baroDev_t *baro);
void bmp085Disable(const bmp085Config_t *config);
#if defined(BARO_EOC_GPIO)
bool bmp085TestEOCConnected(const bmp085Config_t *config);
#endif
#ifdef UNIT_TEST
void RCC_APB2PeriphClockCmd(uint32_t RCC_APB2Periph, FunctionalState NewState);

15
src/main/drivers/barometer/barometer_bmp280.c
View file

@ -52,8 +52,11 @@ typedef struct bmp280_calib_param_s {
int16_t dig_P7; /* calibration P7 data */
int16_t dig_P8; /* calibration P8 data */
int16_t dig_P9; /* calibration P9 data */
int32_t t_fine; /* calibration t_fine data */
} bmp280_calib_param_t;
} __attribute__((packed)) bmp280_calib_param_t; // packed as we read directly from the device into this structure.
STATIC_ASSERT(sizeof(bmp280_calib_param_t) == BMP280_PRESSURE_TEMPERATURE_CALIB_DATA_LENGTH, bmp280_calibration_structure_incorrectly_packed);
STATIC_UNIT_TESTED int32_t t_fine; /* calibration t_fine data */
static uint8_t bmp280_chip_id = 0;
STATIC_UNIT_TESTED bmp280_calib_param_t bmp280_cal;
@ -125,7 +128,7 @@ bool bmp280Detect(baroDev_t *baro)
}
// read calibration
busReadRegisterBuffer(busdev, BMP280_TEMPERATURE_CALIB_DIG_T1_LSB_REG, (uint8_t *)&bmp280_cal, 24);
busReadRegisterBuffer(busdev, BMP280_TEMPERATURE_CALIB_DIG_T1_LSB_REG, (uint8_t *)&bmp280_cal, sizeof(bmp280_calib_param_t));
// set oversampling + power mode (forced), and start sampling
busWriteRegister(busdev, BMP280_CTRL_MEAS_REG, BMP280_MODE);
@ -180,8 +183,8 @@ static int32_t bmp280_compensate_T(int32_t adc_T)
var1 = ((((adc_T >> 3) - ((int32_t)bmp280_cal.dig_T1 << 1))) * ((int32_t)bmp280_cal.dig_T2)) >> 11;
var2 = (((((adc_T >> 4) - ((int32_t)bmp280_cal.dig_T1)) * ((adc_T >> 4) - ((int32_t)bmp280_cal.dig_T1))) >> 12) * ((int32_t)bmp280_cal.dig_T3)) >> 14;
bmp280_cal.t_fine = var1 + var2;
T = (bmp280_cal.t_fine * 5 + 128) >> 8;
t_fine = var1 + var2;
T = (t_fine * 5 + 128) >> 8;
return T;
}
@ -191,7 +194,7 @@ static int32_t bmp280_compensate_T(int32_t adc_T)
static uint32_t bmp280_compensate_P(int32_t adc_P)
{
int64_t var1, var2, p;
var1 = ((int64_t)bmp280_cal.t_fine) - 128000;
var1 = ((int64_t)t_fine) - 128000;
var2 = var1 * var1 * (int64_t)bmp280_cal.dig_P6;
var2 = var2 + ((var1*(int64_t)bmp280_cal.dig_P5) << 17);
var2 = var2 + (((int64_t)bmp280_cal.dig_P4) << 35);

3
src/main/drivers/resource.c
View file

@ -56,7 +56,7 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = {
"RX_SPI_CS",
"SPI_CS",
"GYRO_EXTI",
"BARO_EXTI",
"BARO_EOC",
"COMPASS_EXTI",
"USB",
"USB_DETECT",
@ -98,4 +98,5 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = {
"QSPI_BK2IO2",
"QSPI_BK2IO3",
"QSPI_BK2CS",
"BARO_XCLR",
};

3
src/main/drivers/resource.h
View file

@ -54,7 +54,7 @@ typedef enum {
OWNER_RX_SPI_CS,
OWNER_SPI_CS,
OWNER_GYRO_EXTI,
OWNER_BARO_EXTI,
OWNER_BARO_EOC,
OWNER_COMPASS_EXTI,
OWNER_USB,
OWNER_USB_DETECT,
@ -96,6 +96,7 @@ typedef enum {
OWNER_QUADSPI_BK2IO2,
OWNER_QUADSPI_BK2IO3,
OWNER_QUADSPI_BK2CS,
OWNER_BARO_XCLR,
OWNER_TOTAL_COUNT
} resourceOwner_e;

28
src/main/sensors/barometer.c
View file

@ -26,6 +26,8 @@
#ifdef USE_BARO
#include "build/debug.h"
#include "common/maths.h"
#include "pg/pg.h"
@ -117,6 +119,9 @@ void pgResetFn_barometerConfig(barometerConfig_t *barometerConfig)
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
@ -181,15 +186,11 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse)
case BARO_BMP085:
#ifdef USE_BARO_BMP085
{
const bmp085Config_t *bmp085Config = NULL;
static bmp085Config_t defaultBMP085Config;
defaultBMP085Config.xclrTag = barometerConfig()->baro_xclr_tag;
defaultBMP085Config.eocTag = barometerConfig()->baro_eoc_tag;
#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
static const bmp085Config_t *bmp085Config = &defaultBMP085Config;
if (bmp085Detect(bmp085Config, dev)) {
baroHardware = BARO_BMP085;
@ -324,6 +325,10 @@ uint32_t baroUpdate(void)
{
static barometerState_e state = BAROMETER_NEEDS_SAMPLES;
if (debugMode == DEBUG_BARO) {
debug[0] = state;
}
switch (state) {
default:
case BAROMETER_NEEDS_SAMPLES:
@ -340,6 +345,13 @@ uint32_t baroUpdate(void)
baro.baroPressure = baroPressure;
baro.baroTemperature = baroTemperature;
baroPressureSum = recalculateBarometerTotal(barometerConfig()->baro_sample_count, baroPressureSum, baroPressure);
if (debugMode == DEBUG_BARO) {
debug[1] = baroTemperature;
debug[2] = baroPressure;
debug[3] = baroPressureSum;
}
state = BAROMETER_NEEDS_SAMPLES;
return baro.dev.ut_delay;
break;

2
src/main/sensors/barometer.h
View file

@ -45,6 +45,8 @@ typedef struct barometerConfig_s {
uint8_t baro_sample_count; // size of baro filter array
uint16_t baro_noise_lpf; // additional LPF to reduce baro noise
uint16_t baro_cf_vel; // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity)
ioTag_t baro_eoc_tag;
ioTag_t baro_xclr_tag;
} barometerConfig_t;
PG_DECLARE(barometerConfig_t, barometerConfig);

3
src/main/target/common_defaults_post.h
View file

@ -447,6 +447,9 @@
#ifndef BARO_I2C_INSTANCE
#define BARO_I2C_INSTANCE I2C_DEVICE
#endif
#ifndef BARO_XCLR_PIN
#define BARO_XCLR_PIN NONE
#endif
#endif
#ifdef USE_ADC

5
src/main/target/common_post.h
View file

@ -64,6 +64,11 @@
#undef USE_VARIO
#endif
#if defined(USE_BARO) && !defined(BARO_EOC_PIN)
#define BARO_EOC_PIN NONE
#endif
#if !defined(USE_SERIAL_RX)
#undef USE_SERIALRX_CRSF
#undef USE_SERIALRX_IBUS

7
src/test/unit/baro_bmp280_unittest.cc
View file

@ -27,6 +27,7 @@ void bmp280_calculate(int32_t *pressure, int32_t *temperature);
extern uint32_t bmp280_up;
extern uint32_t bmp280_ut;
extern int32_t t_fine; /* calibration t_fine data */
typedef struct bmp280_calib_param_s {
uint16_t dig_T1; /* calibration T1 data */
@ -41,8 +42,7 @@ typedef struct bmp280_calib_param_s {
int16_t dig_P7; /* calibration P7 data */
int16_t dig_P8; /* calibration P8 data */
int16_t dig_P9; /* calibration P9 data */
int32_t t_fine; /* calibration t_fine data */
} bmp280_calib_param_t;
} __attribute__((packed)) bmp280_calib_param_t; // packed as we read directly from the device into this structure.
bmp280_calib_param_t bmp280_cal;
}
@ -58,6 +58,7 @@ TEST(baroBmp280Test, TestBmp280Calculate)
int32_t pressure, temperature;
bmp280_up = 415148; // Digital pressure value
bmp280_ut = 519888; // Digital temperature value
t_fine = 0;
// and
bmp280_cal.dig_T1 = 27504;
@ -87,6 +88,7 @@ TEST(baroBmp280Test, TestBmp280CalculateHighP)
int32_t pressure, temperature;
bmp280_up = 215148; // Digital pressure value
bmp280_ut = 519888; // Digital temperature value
t_fine = 0;
// and
bmp280_cal.dig_T1 = 27504;
@ -116,6 +118,7 @@ TEST(baroBmp280Test, TestBmp280CalculateZeroP)
int32_t pressure, temperature;
bmp280_up = 415148; // Digital pressure value
bmp280_ut = 519888; // Digital temperature value
t_fine = 0;
// and
bmp280_cal.dig_T1 = 27504;