From 2a00115895f4469a70a3c1daffa896f22a3a76c1 Mon Sep 17 00:00:00 2001 From: jflyper Date: Mon, 24 Jul 2017 07:05:08 +0900 Subject: [PATCH] New baro defaults For I2C connected devices, default i2c address is now 0, meaning "per device default". The address is set to the default when zero in xxxDetect. --- src/main/drivers/barometer/barometer_bmp085.c | 5 +++++ src/main/drivers/barometer/barometer_bmp280.c | 5 +++++ src/main/drivers/barometer/barometer_bmp280.h | 3 +-- src/main/drivers/barometer/barometer_ms5611.c | 5 +++++ src/main/sensors/barometer.c | 17 +++-------------- 5 files changed, 19 insertions(+), 16 deletions(-) diff --git a/src/main/drivers/barometer/barometer_bmp085.c b/src/main/drivers/barometer/barometer_bmp085.c index ebfbede646..49a94b7a5b 100644 --- a/src/main/drivers/barometer/barometer_bmp085.c +++ b/src/main/drivers/barometer/barometer_bmp085.c @@ -198,6 +198,11 @@ bool bmp085Detect(const bmp085Config_t *config, baroDev_t *baro) busDevice_t *busdev = &baro->busdev; + if ((busdev->bustype == BUSTYPE_I2C) && (busdev->busdev_u.i2c.address == 0)) { + // Default address for BMP085 + busdev->busdev_u.i2c.address = BMP085_I2C_ADDR; + } + ack = bmp085ReadRegister(busdev, BMP085_CHIP_ID__REG, 1, &data); /* read Chip Id */ if (ack) { bmp085.chip_id = BMP085_GET_BITSLICE(data, BMP085_CHIP_ID); diff --git a/src/main/drivers/barometer/barometer_bmp280.c b/src/main/drivers/barometer/barometer_bmp280.c index d55cac9b37..d914b37f9b 100644 --- a/src/main/drivers/barometer/barometer_bmp280.c +++ b/src/main/drivers/barometer/barometer_bmp280.c @@ -130,6 +130,11 @@ bool bmp280Detect(baroDev_t *baro) bmp280BusInit(busdev); + if ((busdev->bustype == BUSTYPE_I2C) && (busdev->busdev_u.i2c.address == 0)) { + // Default address for BMP280 + busdev->busdev_u.i2c.address = BMP280_I2C_ADDR; + } + bmp280ReadRegister(busdev, BMP280_CHIP_ID_REG, 1, &bmp280_chip_id); /* read Chip Id */ if (bmp280_chip_id != BMP280_DEFAULT_CHIP_ID) { diff --git a/src/main/drivers/barometer/barometer_bmp280.h b/src/main/drivers/barometer/barometer_bmp280.h index be049b6081..ee8368b364 100644 --- a/src/main/drivers/barometer/barometer_bmp280.h +++ b/src/main/drivers/barometer/barometer_bmp280.h @@ -17,8 +17,7 @@ #pragma once -//#define BMP280_I2C_ADDR (0x76) -#define BMP280_I2C_ADDR (0x77) // Adafruit 2651 +#define BMP280_I2C_ADDR (0x76) #define BMP280_DEFAULT_CHIP_ID (0x58) #define BMP280_CHIP_ID_REG (0xD0) /* Chip ID Register */ diff --git a/src/main/drivers/barometer/barometer_ms5611.c b/src/main/drivers/barometer/barometer_ms5611.c index c97e535505..d55b9a24eb 100644 --- a/src/main/drivers/barometer/barometer_ms5611.c +++ b/src/main/drivers/barometer/barometer_ms5611.c @@ -129,6 +129,11 @@ bool ms5611Detect(baroDev_t *baro) ms5611BusInit(busdev); + if ((busdev->bustype == BUSTYPE_I2C) && (busdev->busdev_u.i2c.address == 0)) { + // Default address for MS5611 + busdev->busdev_u.i2c.address = MS5611_I2C_ADDR; + } + if (!ms5611ReadCommand(busdev, CMD_PROM_RD, 1, &sig) || sig == 0xFF) { ms5611BusDeinit(busdev); return false; diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index bc357c3c51..e8f5aa8a28 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -88,28 +88,17 @@ void pgResetFn_barometerConfig(barometerConfig_t *barometerConfig) barometerConfig->baro_spi_csn = IO_TAG(BMP280_CS_PIN); barometerConfig->baro_i2c_device = I2C_DEV_TO_CFG(I2CINVALID); barometerConfig->baro_i2c_address = 0; -#elif defined(DEFAULT_BARO_BMP280) - barometerConfig->baro_bustype = BUSTYPE_I2C; - barometerConfig->baro_i2c_device = I2C_DEV_TO_CFG(BARO_I2C_INSTANCE); - barometerConfig->baro_i2c_address = BMP280_I2C_ADDR; - barometerConfig->baro_spi_device = SPI_DEV_TO_CFG(SPIINVALID); - barometerConfig->baro_spi_csn = IO_TAG_NONE; #elif defined(DEFAULT_BARO_SPI_MS5611) barometerConfig->baro_bustype = BUSTYPE_SPI; barometerConfig->baro_spi_device = SPI_DEV_TO_CFG(spiDeviceByInstance(MS5611_SPI_INSTANCE)); barometerConfig->baro_spi_csn = IO_TAG(MS5611_CS_PIN); barometerConfig->baro_i2c_device = I2C_DEV_TO_CFG(I2CINVALID); barometerConfig->baro_i2c_address = 0; -#elif defined(DEFAULT_BARO_BARO_MS5611) +#elif defined(DEFAULT_BARO_MS5611) || defined(DEFAULT_BARO_BMP280) || defined(DEFAULT_BARO_BMP085) + // 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); - barometerConfig->baro_i2c_address = MS5611_I2C_ADDR; - barometerConfig->baro_spi_device = SPI_DEV_TO_CFG(SPIINVALID); - barometerConfig->baro_spi_csn = IO_TAG_NONE; -#elif defined(DEFAULT_BARO_BMP085) - barometerConfig->baro_bustype = BUSTYPE_I2C; - barometerConfig->baro_i2c_device = I2C_DEV_TO_CFG(BARO_I2C_INSTANCE); - barometerConfig->baro_i2c_address = BMP085_I2C_ADDR; + barometerConfig->baro_i2c_address = 0; barometerConfig->baro_spi_device = SPI_DEV_TO_CFG(SPIINVALID); barometerConfig->baro_spi_csn = IO_TAG_NONE; #else