diff --git a/src/main/build/debug.c b/src/main/build/debug.c index a54abe20e3..410dcf6da3 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -90,4 +90,5 @@ const char * const debugModeNames[DEBUG_COUNT] = { "CRSF_LINK_STATISTICS_UPLINK", "CRSF_LINK_STATISTICS_PWR", "CRSF_LINK_STATISTICS_DOWN", + "BARO", }; diff --git a/src/main/build/debug.h b/src/main/build/debug.h index c916e30721..3ac537eea3 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -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; diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index e12b52f681..cd5d26d1e6 100644 --- a/src/main/cli/cli.c +++ b/src/main/cli/cli.c @@ -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 ), diff --git a/src/main/drivers/barometer/barometer_bmp085.c b/src/main/drivers/barometer/barometer_bmp085.c index d5758ce3c8..ac627f00b4 100644 --- a/src/main/drivers/barometer/barometer_bmp085.c +++ b/src/main/drivers/barometer/barometer_bmp085.c @@ -39,22 +39,19 @@ #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 +#endif typedef struct { int16_t ac1; @@ -132,30 +129,17 @@ 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); - IOConfigGPIO(xclrIO, IOCFG_OUT_PP); - } -} - -void bmp085Disable(const bmp085Config_t *config) -{ - bmp085InitXclrIO(config); - BMP085_OFF; + xclrIO = IOGetByTag(config->xclrTag); + IOInit(xclrIO, OWNER_BARO_XCLR, 0); + IOConfigGPIO(xclrIO, IOCFG_OUT_PP); } 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); - // 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); - EXTIEnable(eocIO, true); - } +#ifdef USE_EXTI + // EXTI interrupt for barometer EOC + + 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 */ diff --git a/src/main/drivers/barometer/barometer_bmp085.h b/src/main/drivers/barometer/barometer_bmp085.h index 9f5a1e5530..d338b4c6c7 100644 --- a/src/main/drivers/barometer/barometer_bmp085.h +++ b/src/main/drivers/barometer/barometer_bmp085.h @@ -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); diff --git a/src/main/drivers/barometer/barometer_bmp280.c b/src/main/drivers/barometer/barometer_bmp280.c index 4d660b5c66..989a48639b 100644 --- a/src/main/drivers/barometer/barometer_bmp280.c +++ b/src/main/drivers/barometer/barometer_bmp280.c @@ -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); diff --git a/src/main/drivers/resource.c b/src/main/drivers/resource.c index 90cebf6234..4447dc9815 100644 --- a/src/main/drivers/resource.c +++ b/src/main/drivers/resource.c @@ -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", }; diff --git a/src/main/drivers/resource.h b/src/main/drivers/resource.h index 99bc7b067a..01192c2274 100644 --- a/src/main/drivers/resource.h +++ b/src/main/drivers/resource.h @@ -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; diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index 2eccc2d4b5..2e0f354ad7 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -26,6 +26,8 @@ #ifdef USE_BARO +#include "build/debug.h" + #include "common/maths.h" #include "pg/pg.h" @@ -102,7 +104,7 @@ void pgResetFn_barometerConfig(barometerConfig_t *barometerConfig) 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_BMP280) || defined(DEFAULT_BARO_BMP085)||defined(DEFAULT_BARO_QMP6988) +#elif defined(DEFAULT_BARO_MS5611) || defined(DEFAULT_BARO_BMP280) || defined(DEFAULT_BARO_BMP085) || defined(DEFAULT_BARO_QMP6988) // All I2C devices shares a default config with address = 0 (per device default) barometerConfig->baro_bustype = BUSTYPE_I2C; barometerConfig->baro_i2c_device = I2C_DEV_TO_CFG(BARO_I2C_INSTANCE); @@ -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; diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index 21755d22eb..6e15e205f6 100644 --- a/src/main/sensors/barometer.h +++ b/src/main/sensors/barometer.h @@ -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); diff --git a/src/main/target/common_defaults_post.h b/src/main/target/common_defaults_post.h index 56fb9757d5..2f2b6950fa 100644 --- a/src/main/target/common_defaults_post.h +++ b/src/main/target/common_defaults_post.h @@ -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 diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index 36394838ca..3ce2e7f54e 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -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 diff --git a/src/test/unit/baro_bmp280_unittest.cc b/src/test/unit/baro_bmp280_unittest.cc index cd9b2b4256..52d7ddd0cc 100644 --- a/src/test/unit/baro_bmp280_unittest.cc +++ b/src/test/unit/baro_bmp280_unittest.cc @@ -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;