diff --git a/make/source.mk b/make/source.mk
index 788fd023b2..0cdc394aff 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -61,6 +61,7 @@ COMMON_SRC = \
io/transponder_ir.c \
msp/msp_serial.c \
pg/adc.c \
+ pg/bus_i2c.c \
pg/pg.c \
scheduler/scheduler.c \
sensors/battery.c \
diff --git a/src/main/drivers/bus_i2c.h b/src/main/drivers/bus_i2c.h
index 0e8383154e..b7131cedea 100644
--- a/src/main/drivers/bus_i2c.h
+++ b/src/main/drivers/bus_i2c.h
@@ -19,7 +19,6 @@
#include "platform.h"
-#include "pg/pg.h"
#include "drivers/io_types.h"
#include "drivers/rcc_types.h"
@@ -53,14 +52,8 @@ typedef enum I2CDevice {
#define I2C_ADDR7_MIN 8
#define I2C_ADDR7_MAX 119
-typedef struct i2cConfig_s {
- ioTag_t ioTagScl[I2CDEV_COUNT];
- ioTag_t ioTagSda[I2CDEV_COUNT];
- bool overClock[I2CDEV_COUNT];
- bool pullUp[I2CDEV_COUNT];
-} i2cConfig_t;
-
-void i2cHardwareConfigure(void);
+struct i2cConfig_s;
+void i2cHardwareConfigure(const struct i2cConfig_s *i2cConfig);
void i2cInit(I2CDevice device);
bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data);
bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t data);
diff --git a/src/main/drivers/bus_i2c_config.c b/src/main/drivers/bus_i2c_config.c
index 466bab7210..b7d7d5c720 100644
--- a/src/main/drivers/bus_i2c_config.c
+++ b/src/main/drivers/bus_i2c_config.c
@@ -30,133 +30,11 @@
#include "build/build_config.h"
#include "build/debug.h"
-#include "drivers/io.h"
#include "drivers/bus_i2c.h"
#include "drivers/bus_i2c_impl.h"
+#include "drivers/io.h"
-#include "pg/pg.h"
-#include "pg/pg_ids.h"
-
-#ifdef I2C_FULL_RECONFIGURABILITY
-#if I2CDEV_COUNT >= 1
-#ifndef I2C1_SCL
-#define I2C1_SCL NONE
-#endif
-#ifndef I2C1_SDA
-#define I2C1_SDA NONE
-#endif
-#endif
-
-#if I2CDEV_COUNT >= 2
-#ifndef I2C2_SCL
-#define I2C2_SCL NONE
-#endif
-#ifndef I2C2_SDA
-#define I2C2_SDA NONE
-#endif
-#endif
-
-#if I2CDEV_COUNT >= 3
-#ifndef I2C3_SCL
-#define I2C3_SCL NONE
-#endif
-#ifndef I2C3_SDA
-#define I2C3_SDA NONE
-#endif
-#endif
-
-#if I2CDEV_COUNT >= 4
-#ifndef I2C4_SCL
-#define I2C4_SCL NONE
-#endif
-#ifndef I2C4_SDA
-#define I2C4_SDA NONE
-#endif
-#endif
-
-#else // I2C_FULL_RECONFIGURABILITY
-
-// Backward compatibility for exisiting targets
-
-#ifdef STM32F1
-#ifndef I2C1_SCL
-#define I2C1_SCL PB8
-#endif
-#ifndef I2C1_SDA
-#define I2C1_SDA PB9
-#endif
-#ifndef I2C2_SCL
-#define I2C2_SCL PB10
-#endif
-#ifndef I2C2_SDA
-#define I2C2_SDA PB11
-#endif
-#endif // STM32F1
-
-#ifdef STM32F3
-#ifndef I2C1_SCL
-#define I2C1_SCL PB6
-#endif
-#ifndef I2C1_SDA
-#define I2C1_SDA PB7
-#endif
-#ifndef I2C2_SCL
-#define I2C2_SCL PA9
-#endif
-#ifndef I2C2_SDA
-#define I2C2_SDA PA10
-#endif
-#endif // STM32F3
-
-#ifdef STM32F4
-#ifndef I2C1_SCL
-#define I2C1_SCL PB6
-#endif
-#ifndef I2C1_SDA
-#define I2C1_SDA PB7
-#endif
-#ifndef I2C2_SCL
-#define I2C2_SCL PB10
-#endif
-#ifndef I2C2_SDA
-#define I2C2_SDA PB11
-#endif
-#ifndef I2C3_SCL
-#define I2C3_SCL PA8
-#endif
-#ifndef I2C3_SDA
-#define I2C3_SDA PC9
-#endif
-#endif // STM32F4
-
-#ifdef STM32F7
-#ifndef I2C1_SCL
-#define I2C1_SCL PB6
-#endif
-#ifndef I2C1_SDA
-#define I2C1_SDA PB7
-#endif
-#ifndef I2C2_SCL
-#define I2C2_SCL PB10
-#endif
-#ifndef I2C2_SDA
-#define I2C2_SDA PB11
-#endif
-#ifndef I2C3_SCL
-#define I2C3_SCL PA8
-#endif
-#ifndef I2C3_SDA
-#define I2C3_SDA PB4
-#endif
-#ifndef I2C4_SCL
-#define I2C4_SCL PD12
-#endif
-#ifndef I2C4_SDA
-#define I2C4_SDA PD13
-#endif
-#endif // STM32F7
-
-#endif // I2C_FULL_RECONFIGURABILITY
+#include "pg/bus_i2c.h"
// Backward compatibility for overclocking and internal pullup.
// These will eventually be configurable through PG-based configurator
@@ -175,62 +53,8 @@
#define I2C4_OVERCLOCK false
#endif
-// Default values for internal pullup
-
-#if defined(USE_I2C_PULLUP)
-#define I2C1_PULLUP true
-#define I2C2_PULLUP true
-#define I2C3_PULLUP true
-#define I2C4_PULLUP true
-#else
-#define I2C1_PULLUP false
-#define I2C2_PULLUP false
-#define I2C3_PULLUP false
-#define I2C4_PULLUP false
-#endif
-
-typedef struct i2cDefaultConfig_s {
- I2CDevice device;
- ioTag_t ioTagScl, ioTagSda;
- bool overClock;
- bool pullUp;
-} i2cDefaultConfig_t;
-
-static const i2cDefaultConfig_t i2cDefaultConfig[] = {
-#ifdef USE_I2C_DEVICE_1
- { I2CDEV_1, IO_TAG(I2C1_SCL), IO_TAG(I2C1_SDA), I2C1_OVERCLOCK, I2C1_PULLUP },
-#endif
-#ifdef USE_I2C_DEVICE_2
- { I2CDEV_2, IO_TAG(I2C2_SCL), IO_TAG(I2C2_SDA), I2C2_OVERCLOCK, I2C2_PULLUP },
-#endif
-#ifdef USE_I2C_DEVICE_3
- { I2CDEV_3, IO_TAG(I2C3_SCL), IO_TAG(I2C3_SDA), I2C3_OVERCLOCK, I2C3_PULLUP },
-#endif
-#ifdef USE_I2C_DEVICE_4
- { I2CDEV_4, IO_TAG(I2C4_SCL), IO_TAG(I2C4_SDA), I2C4_OVERCLOCK, I2C4_PULLUP },
-#endif
-};
-
-PG_DECLARE(i2cConfig_t, i2cConfig);
-PG_REGISTER_WITH_RESET_FN(i2cConfig_t, i2cConfig, PG_I2C_CONFIG, 0);
-
-void pgResetFn_i2cConfig(i2cConfig_t *i2cConfig)
+void i2cHardwareConfigure(const i2cConfig_t *i2cConfig)
{
- memset(i2cConfig, 0, sizeof(*i2cConfig));
-
- for (size_t index = 0 ; index < ARRAYLEN(i2cDefaultConfig) ; index++) {
- const i2cDefaultConfig_t *defconf = &i2cDefaultConfig[index];
- i2cConfig->ioTagScl[defconf->device] = defconf->ioTagScl;
- i2cConfig->ioTagSda[defconf->device] = defconf->ioTagSda;
- i2cConfig->overClock[defconf->device] = defconf->overClock;
- i2cConfig->pullUp[defconf->device] = defconf->pullUp;
- }
-}
-
-void i2cHardwareConfigure(void)
-{
- const i2cConfig_t *pConfig = i2cConfig();
-
for (int index = 0 ; index < I2CDEV_COUNT ; index++) {
const i2cHardware_t *hardware = &i2cHardware[index];
@@ -244,17 +68,17 @@ void i2cHardwareConfigure(void)
memset(pDev, 0, sizeof(*pDev));
for (int pindex = 0 ; pindex < I2C_PIN_SEL_MAX ; pindex++) {
- if (pConfig->ioTagScl[device] == hardware->sclPins[pindex])
- pDev->scl = IOGetByTag(pConfig->ioTagScl[device]);
- if (pConfig->ioTagSda[device] == hardware->sdaPins[pindex])
- pDev->sda = IOGetByTag(pConfig->ioTagSda[device]);
+ if (i2cConfig->ioTagScl[device] == hardware->sclPins[pindex])
+ pDev->scl = IOGetByTag(i2cConfig->ioTagScl[device]);
+ if (i2cConfig->ioTagSda[device] == hardware->sdaPins[pindex])
+ pDev->sda = IOGetByTag(i2cConfig->ioTagSda[device]);
}
if (pDev->scl && pDev->sda) {
pDev->hardware = hardware;
pDev->reg = hardware->reg;
- pDev->overClock = pConfig->overClock[device];
- pDev->pullUp = pConfig->pullUp[device];
+ pDev->overClock = i2cConfig->overClock[device];
+ pDev->pullUp = i2cConfig->pullUp[device];
}
}
}
diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index ff3341d1a0..832658773c 100644
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@@ -80,6 +80,7 @@
#include "msp/msp_serial.h"
#include "pg/adc.h"
+#include "pg/bus_i2c.h"
#include "rx/rx.h"
#include "rx/rx_spi.h"
@@ -438,7 +439,7 @@ void init(void)
#endif // USE_SPI
#ifdef USE_I2C
- i2cHardwareConfigure();
+ i2cHardwareConfigure(i2cConfig());
// Note: Unlike UARTs which are configured when client is present,
// I2C buses are initialized unconditionally if they are configured.
diff --git a/src/main/interface/cli.c b/src/main/interface/cli.c
index e229501b5c..5785822319 100644
--- a/src/main/interface/cli.c
+++ b/src/main/interface/cli.c
@@ -58,7 +58,6 @@ extern uint8_t __config_end;
#include "drivers/accgyro/accgyro.h"
#include "drivers/buf_writer.h"
-#include "drivers/bus_i2c.h"
#include "drivers/bus_spi.h"
#include "drivers/compass/compass.h"
#include "drivers/display.h"
@@ -117,6 +116,7 @@ extern uint8_t __config_end;
#include "io/vtx.h"
#include "pg/adc.h"
+#include "pg/bus_i2c.h"
#include "rx/rx.h"
#include "rx/spektrum.h"
diff --git a/src/main/osd_slave/osd_slave_init.c b/src/main/osd_slave/osd_slave_init.c
index 55d39d5ab2..825a12cad3 100644
--- a/src/main/osd_slave/osd_slave_init.c
+++ b/src/main/osd_slave/osd_slave_init.c
@@ -78,6 +78,7 @@
#include "osd_slave/osd_slave_init.h"
#include "pg/adc.h"
+#include "pg/bus_i2c.h"
#include "scheduler/scheduler.h"
@@ -199,7 +200,7 @@ void init(void)
#endif /* USE_SPI */
#ifdef USE_I2C
- i2cHardwareConfigure();
+ i2cHardwareConfigure(i2cConfig());
// Note: Unlike UARTs which are configured when client is present,
// I2C buses are initialized unconditionally if they are configured.
diff --git a/src/main/pg/bus_i2c.c b/src/main/pg/bus_i2c.c
new file mode 100644
index 0000000000..c30b15176c
--- /dev/null
+++ b/src/main/pg/bus_i2c.c
@@ -0,0 +1,211 @@
+/*
+ * 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 .
+ */
+
+/*
+ * Created by jflyper
+ */
+
+#include
+#include
+#include
+
+#include "platform.h"
+
+#if defined(USE_I2C) && !defined(SOFT_I2C)
+
+#include "common/utils.h"
+
+#include "drivers/io.h"
+
+#include "pg/pg.h"
+#include "pg/pg_ids.h"
+
+#include "pg/bus_i2c.h"
+
+#ifdef I2C_FULL_RECONFIGURABILITY
+#if I2CDEV_COUNT >= 1
+#ifndef I2C1_SCL
+#define I2C1_SCL NONE
+#endif
+#ifndef I2C1_SDA
+#define I2C1_SDA NONE
+#endif
+#endif
+
+#if I2CDEV_COUNT >= 2
+#ifndef I2C2_SCL
+#define I2C2_SCL NONE
+#endif
+#ifndef I2C2_SDA
+#define I2C2_SDA NONE
+#endif
+#endif
+
+#if I2CDEV_COUNT >= 3
+#ifndef I2C3_SCL
+#define I2C3_SCL NONE
+#endif
+#ifndef I2C3_SDA
+#define I2C3_SDA NONE
+#endif
+#endif
+
+#if I2CDEV_COUNT >= 4
+#ifndef I2C4_SCL
+#define I2C4_SCL NONE
+#endif
+#ifndef I2C4_SDA
+#define I2C4_SDA NONE
+#endif
+#endif
+
+#else // I2C_FULL_RECONFIGURABILITY
+
+// Backward compatibility for exisiting targets
+
+#ifdef STM32F1
+#ifndef I2C1_SCL
+#define I2C1_SCL PB8
+#endif
+#ifndef I2C1_SDA
+#define I2C1_SDA PB9
+#endif
+#ifndef I2C2_SCL
+#define I2C2_SCL PB10
+#endif
+#ifndef I2C2_SDA
+#define I2C2_SDA PB11
+#endif
+#endif // STM32F1
+
+#ifdef STM32F3
+#ifndef I2C1_SCL
+#define I2C1_SCL PB6
+#endif
+#ifndef I2C1_SDA
+#define I2C1_SDA PB7
+#endif
+#ifndef I2C2_SCL
+#define I2C2_SCL PA9
+#endif
+#ifndef I2C2_SDA
+#define I2C2_SDA PA10
+#endif
+#endif // STM32F3
+
+#ifdef STM32F4
+#ifndef I2C1_SCL
+#define I2C1_SCL PB6
+#endif
+#ifndef I2C1_SDA
+#define I2C1_SDA PB7
+#endif
+#ifndef I2C2_SCL
+#define I2C2_SCL PB10
+#endif
+#ifndef I2C2_SDA
+#define I2C2_SDA PB11
+#endif
+#ifndef I2C3_SCL
+#define I2C3_SCL PA8
+#endif
+#ifndef I2C3_SDA
+#define I2C3_SDA PC9
+#endif
+#endif // STM32F4
+
+#ifdef STM32F7
+#ifndef I2C1_SCL
+#define I2C1_SCL PB6
+#endif
+#ifndef I2C1_SDA
+#define I2C1_SDA PB7
+#endif
+#ifndef I2C2_SCL
+#define I2C2_SCL PB10
+#endif
+#ifndef I2C2_SDA
+#define I2C2_SDA PB11
+#endif
+#ifndef I2C3_SCL
+#define I2C3_SCL PA8
+#endif
+#ifndef I2C3_SDA
+#define I2C3_SDA PB4
+#endif
+#ifndef I2C4_SCL
+#define I2C4_SCL PD12
+#endif
+#ifndef I2C4_SDA
+#define I2C4_SDA PD13
+#endif
+#endif // STM32F7
+
+#endif // I2C_FULL_RECONFIGURABILITY
+
+// Default values for internal pullup
+
+#if defined(USE_I2C_PULLUP)
+#define I2C1_PULLUP true
+#define I2C2_PULLUP true
+#define I2C3_PULLUP true
+#define I2C4_PULLUP true
+#else
+#define I2C1_PULLUP false
+#define I2C2_PULLUP false
+#define I2C3_PULLUP false
+#define I2C4_PULLUP false
+#endif
+
+typedef struct i2cDefaultConfig_s {
+ I2CDevice device;
+ ioTag_t ioTagScl, ioTagSda;
+ bool overClock;
+ bool pullUp;
+} i2cDefaultConfig_t;
+
+static const i2cDefaultConfig_t i2cDefaultConfig[] = {
+#ifdef USE_I2C_DEVICE_1
+ { I2CDEV_1, IO_TAG(I2C1_SCL), IO_TAG(I2C1_SDA), I2C1_OVERCLOCK, I2C1_PULLUP },
+#endif
+#ifdef USE_I2C_DEVICE_2
+ { I2CDEV_2, IO_TAG(I2C2_SCL), IO_TAG(I2C2_SDA), I2C2_OVERCLOCK, I2C2_PULLUP },
+#endif
+#ifdef USE_I2C_DEVICE_3
+ { I2CDEV_3, IO_TAG(I2C3_SCL), IO_TAG(I2C3_SDA), I2C3_OVERCLOCK, I2C3_PULLUP },
+#endif
+#ifdef USE_I2C_DEVICE_4
+ { I2CDEV_4, IO_TAG(I2C4_SCL), IO_TAG(I2C4_SDA), I2C4_OVERCLOCK, I2C4_PULLUP },
+#endif
+};
+
+void pgResetFn_i2cConfig(i2cConfig_t *i2cConfig)
+{
+ memset(i2cConfig, 0, sizeof(*i2cConfig));
+
+ for (size_t index = 0 ; index < ARRAYLEN(i2cDefaultConfig) ; index++) {
+ const i2cDefaultConfig_t *defconf = &i2cDefaultConfig[index];
+ i2cConfig->ioTagScl[defconf->device] = defconf->ioTagScl;
+ i2cConfig->ioTagSda[defconf->device] = defconf->ioTagSda;
+ i2cConfig->overClock[defconf->device] = defconf->overClock;
+ i2cConfig->pullUp[defconf->device] = defconf->pullUp;
+ }
+}
+
+PG_REGISTER_WITH_RESET_FN(i2cConfig_t, i2cConfig, PG_I2C_CONFIG, 0);
+
+#endif // defined(USE_I2C) && !defined(USE_SOFT_I2C)
diff --git a/src/main/pg/bus_i2c.h b/src/main/pg/bus_i2c.h
new file mode 100644
index 0000000000..c3a14e921a
--- /dev/null
+++ b/src/main/pg/bus_i2c.h
@@ -0,0 +1,35 @@
+/*
+ * 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
+
+#include "platform.h"
+
+#include "drivers/bus_i2c.h"
+#include "drivers/io_types.h"
+#include "drivers/rcc_types.h"
+
+#include "pg/pg.h"
+
+typedef struct i2cConfig_s {
+ ioTag_t ioTagScl[I2CDEV_COUNT];
+ ioTag_t ioTagSda[I2CDEV_COUNT];
+ bool overClock[I2CDEV_COUNT];
+ bool pullUp[I2CDEV_COUNT];
+} i2cConfig_t;
+
+PG_DECLARE(i2cConfig_t, i2cConfig);
diff --git a/src/main/target/COLIBRI_RACE/target.c b/src/main/target/COLIBRI_RACE/target.c
index cc38369b05..f2475af02a 100644
--- a/src/main/target/COLIBRI_RACE/target.c
+++ b/src/main/target/COLIBRI_RACE/target.c
@@ -27,6 +27,8 @@
#include "drivers/timer.h"
#include "drivers/timer_def.h"
+#include "pg/bus_i2c.h"
+
#ifdef USE_BST
#include "bus_bst.h"
#endif
@@ -61,7 +63,7 @@ void targetBusInit(void)
#endif
#endif
- i2cHardwareConfigure();
+ i2cHardwareConfigure(i2cConfig());
i2cInit(I2CDEV_2);
bstInit(BST_DEVICE);