diff --git a/Makefile b/Makefile
index 9988ce818f..dc7d0ccb52 100644
--- a/Makefile
+++ b/Makefile
@@ -683,6 +683,7 @@ COMMON_SRC = \
config/config_streamer.c \
drivers/adc.c \
drivers/buf_writer.c \
+ drivers/bus_i2c_busdev.c \
drivers/bus_i2c_config.c \
drivers/bus_i2c_soft.c \
drivers/bus_spi.c \
@@ -847,6 +848,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
common/typeconversion.c \
drivers/adc.c \
drivers/buf_writer.c \
+ drivers/bus_i2c_busdev.c \
drivers/bus_spi.c \
drivers/exti.c \
drivers/gyro_sync.c \
diff --git a/src/main/drivers/barometer/barometer_bmp085.c b/src/main/drivers/barometer/barometer_bmp085.c
index bbe2b7950f..c67250dd9a 100644
--- a/src/main/drivers/barometer/barometer_bmp085.c
+++ b/src/main/drivers/barometer/barometer_bmp085.c
@@ -26,6 +26,7 @@
#include "drivers/bus.h"
#include "drivers/bus_i2c.h"
+#include "drivers/bus_i2c_busdev.h"
#include "drivers/exti.h"
#include "drivers/gpio.h"
#include "drivers/io.h"
@@ -157,12 +158,12 @@ void bmp085Disable(const bmp085Config_t *config)
bool bmp085ReadRegister(busDevice_t *pBusdev, uint8_t cmd, uint8_t len, uint8_t *data)
{
- return i2cRead(pBusdev->busdev_u.i2c.device, pBusdev->busdev_u.i2c.address, cmd, len, data);
+ return i2cReadRegisterBuffer(pBusdev, cmd, len, data);
}
bool bmp085WriteRegister(busDevice_t *pBusdev, uint8_t cmd, uint8_t byte)
{
- return i2cWrite(pBusdev->busdev_u.i2c.device, pBusdev->busdev_u.i2c.address, cmd, byte);
+ return i2cWriteRegister(pBusdev, cmd, byte);
}
bool bmp085Detect(const bmp085Config_t *config, baroDev_t *baro)
diff --git a/src/main/drivers/barometer/barometer_bmp280.c b/src/main/drivers/barometer/barometer_bmp280.c
index 356f79a779..fbb32cfde0 100644
--- a/src/main/drivers/barometer/barometer_bmp280.c
+++ b/src/main/drivers/barometer/barometer_bmp280.c
@@ -27,6 +27,7 @@
#include "drivers/bus.h"
#include "drivers/bus_i2c.h"
+#include "drivers/bus_i2c_busdev.h"
#include "drivers/bus_spi.h"
#include "drivers/io.h"
#include "drivers/time.h"
@@ -35,8 +36,6 @@
#if defined(BARO) && (defined(USE_BARO_BMP280) || defined(USE_BARO_SPI_BMP280))
-// BMP280, address 0x76
-
typedef struct bmp280_calib_param_s {
uint16_t dig_T1; /* calibration T1 data */
int16_t dig_T2; /* calibration T2 data */
@@ -76,7 +75,7 @@ bool bmp280ReadRegister(busDevice_t *pBusdev, uint8_t reg, uint8_t length, uint8
#endif
#ifdef USE_BARO_BMP280
case BUSTYPE_I2C:
- return i2cRead(pBusdev->busdev_u.i2c.device, pBusdev->busdev_u.i2c.address, reg, length, data);
+ return i2cReadRegisterBuffer(pBusdev, reg, length, data);
#endif
}
return false;
@@ -91,7 +90,7 @@ bool bmp280WriteRegister(busDevice_t *pBusdev, uint8_t reg, uint8_t data)
#endif
#ifdef USE_BARO_BMP280
case BUSTYPE_I2C:
- return i2cWrite(pBusdev->busdev_u.i2c.device, pBusdev->busdev_u.i2c.address, reg, data);
+ return i2cWriteRegister(pBusdev, reg, data);
#endif
}
return false;
@@ -101,10 +100,9 @@ void bmp280BusInit(busDevice_t *pBusdev)
{
#ifdef USE_BARO_SPI_BMP280
if (pBusdev->bustype == BUSTYPE_SPI) {
-#define DISABLE_BMP280(pBusdev) IOHi((pBusdev)->busdev_u.spi.csnPin)
IOInit(pBusdev->busdev_u.spi.csnPin, OWNER_BARO_CS, 0);
IOConfigGPIO(pBusdev->busdev_u.spi.csnPin, IOCFG_OUT_PP);
- DISABLE_BMP280(pBusdev);
+ IOHi((pBusdev)->busdev_u.spi.csnPin); // Disable
spiSetDivisor(pBusdev->busdev_u.spi.instance, SPI_CLOCK_STANDARD); // XXX
}
#else
diff --git a/src/main/drivers/barometer/barometer_ms5611.c b/src/main/drivers/barometer/barometer_ms5611.c
index 41dc1268e0..424fb52349 100644
--- a/src/main/drivers/barometer/barometer_ms5611.c
+++ b/src/main/drivers/barometer/barometer_ms5611.c
@@ -28,6 +28,7 @@
#include "barometer_ms5611.h"
#include "drivers/bus_i2c.h"
+#include "drivers/bus_i2c_busdev.h"
#include "drivers/bus_spi.h"
#include "drivers/io.h"
#include "drivers/time.h"
@@ -70,7 +71,7 @@ bool ms5611ReadCommand(busDevice_t *pBusdev, uint8_t cmd, uint8_t len, uint8_t *
#endif
#ifdef USE_BARO_MS5611
case BUSTYPE_I2C:
- return i2cRead(pBusdev->busdev_u.i2c.device, pBusdev->busdev_u.i2c.address, cmd, len, data);
+ return i2cReadRegisterBuffer(pBusdev, cmd, len, data);
#endif
}
return false;
@@ -85,7 +86,7 @@ bool ms5611WriteCommand(busDevice_t *pBusdev, uint8_t cmd, uint8_t byte)
#endif
#ifdef USE_BARO_MS5611
case BUSTYPE_I2C:
- return i2cWrite(pBusdev->busdev_u.i2c.device, pBusdev->busdev_u.i2c.address, cmd, byte);
+ return i2cWriteRegister(pBusdev, cmd, byte);
#endif
}
return false;
@@ -97,10 +98,7 @@ void ms5611BusInit(busDevice_t *pBusdev)
if (pBusdev->bustype == BUSTYPE_SPI) {
IOInit(pBusdev->busdev_u.spi.csnPin, OWNER_BARO_CS, 0);
IOConfigGPIO(pBusdev->busdev_u.spi.csnPin, IOCFG_OUT_PP);
-
-#define DISABLE_MS5611 IOHi(pBusdev->busdev_u.spi.csnPin)
- DISABLE_MS5611;
-
+ IOHi(pBusdev->busdev_u.spi.csnPin); // Disable
spiSetDivisor(pBusdev->busdev_u.spi.csnPin, SPI_CLOCK_STANDARD);
}
#else
diff --git a/src/main/drivers/bus_i2c_busdev.c b/src/main/drivers/bus_i2c_busdev.c
new file mode 100644
index 0000000000..2381890b48
--- /dev/null
+++ b/src/main/drivers/bus_i2c_busdev.c
@@ -0,0 +1,39 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it 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 is distributed in the hope that it 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 Cleanflight. If not, see .
+ */
+
+#include
+#include
+#include
+
+#include
+
+#if defined(USE_I2C)
+
+#include "drivers/bus_i2c.h"
+#include "drivers/bus.h"
+
+bool i2cReadRegisterBuffer(busDevice_t *busdev, uint8_t reg, uint8_t len, uint8_t *buffer)
+{
+ return i2cRead(busdev->busdev_u.i2c.device, busdev->busdev_u.i2c.address, reg, len, buffer);
+}
+
+bool i2cWriteRegister(busDevice_t *busdev, uint8_t reg, uint8_t data)
+{
+ return i2cWrite(busdev->busdev_u.i2c.device, busdev->busdev_u.i2c.address, reg, data);
+}
+
+#endif
diff --git a/src/main/drivers/bus_i2c_busdev.h b/src/main/drivers/bus_i2c_busdev.h
new file mode 100644
index 0000000000..352ff3b658
--- /dev/null
+++ b/src/main/drivers/bus_i2c_busdev.h
@@ -0,0 +1,21 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it 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 is distributed in the hope that it 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 Cleanflight. If not, see .
+ */
+
+#pragma once
+
+bool i2cReadRegisterBuffer(busDevice_t *busdev, uint8_t reg, uint8_t len, uint8_t *buffer);
+bool i2cWriteRegister(busDevice_t *busdev, uint8_t reg, uint8_t data);
diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c
index b0d4aee683..08a9323f6b 100644
--- a/src/main/sensors/barometer.c
+++ b/src/main/sensors/barometer.c
@@ -20,7 +20,6 @@
#include
#include "platform.h"
-#include "build/debug.h"
#include "common/maths.h"
@@ -58,6 +57,8 @@ void pgResetFn_barometerConfig(barometerConfig_t *barometerConfig)
barometerConfig->baro_cf_alt = 965;
barometerConfig->baro_hardware = BARO_DEFAULT;
+ // For backward compatibility; ceate a valid default value for bus parameters
+
#ifdef USE_BARO_BMP085
barometerConfig->baro_bustype = BUSTYPE_I2C;
barometerConfig->baro_i2c_device = I2C_DEV_TO_CFG(BARO_I2C_INSTANCE);
@@ -112,8 +113,6 @@ static int32_t baroGroundAltitude = 0;
static int32_t baroGroundPressure = 8*101325;
static uint32_t baroPressureSum = 0;
-#include "build/debug.h"
-
bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse)
{
// Detect what pressure sensors are available. baro->update() is set to sensor-specific update function
@@ -145,28 +144,27 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse)
return false;
}
-#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
-
-#endif
-
switch (baroHardware) {
case BARO_DEFAULT:
; // fallthough
case BARO_BMP085:
#ifdef USE_BARO_BMP085
- if (bmp085Detect(bmp085Config, dev)) {
- baroHardware = BARO_BMP085;
- break;
+ {
+ 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
; // fallthough
diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h
index 55162736d1..932282de37 100644
--- a/src/main/target/OMNIBUSF4/target.h
+++ b/src/main/target/OMNIBUSF4/target.h
@@ -99,11 +99,12 @@
//#define USE_BARO_MS5611
#if defined(OMNIBUSF4SD)
#define USE_BARO_BMP280
-//#define USE_BARO_SPI_BMP280
+#define USE_BARO_SPI_BMP280
#define BMP280_SPI_INSTANCE SPI3
#define BMP280_CS_PIN PB3 // v1
-#define BARO_I2C_INSTANCE (I2CDEV_2)
#endif
+#define USE_BARO_BMP280
+#define BARO_I2C_INSTANCE (I2CDEV_2)
#define OSD
#define USE_MAX7456