diff --git a/src/main/drivers/bus_i2c.h b/src/main/drivers/bus_i2c.h
index 33fb4100cc..df539434af 100755
--- a/src/main/drivers/bus_i2c.h
+++ b/src/main/drivers/bus_i2c.h
@@ -17,7 +17,13 @@
 
 #pragma once
 
-void i2cInit(I2C_TypeDef *I2Cx);
+typedef enum I2CDevice {
+    I2CDEV_1,
+    I2CDEV_2,
+    I2CDEV_MAX = I2CDEV_2,
+} I2CDevice;
+
+void i2cInit(I2CDevice index);
 bool i2cWriteBuffer(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data);
 bool i2cWrite(uint8_t addr_, uint8_t reg, uint8_t data);
 bool i2cRead(uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* buf);
diff --git a/src/main/drivers/bus_i2c_soft.c b/src/main/drivers/bus_i2c_soft.c
index 58620bba01..4a73671344 100644
--- a/src/main/drivers/bus_i2c_soft.c
+++ b/src/main/drivers/bus_i2c_soft.c
@@ -25,19 +25,39 @@
 #include "gpio.h"
 
 // Software I2C driver, using same pins as hardware I2C, with hw i2c module disabled.
-// SCL  PB10
-// SDA  PB11
+// Can be configured for I2C2 pinout (SCL: PB10, SDA: PB11) or I2C1 pinout (SCL: PB6, SDA: PB7)
 
 #ifdef SOFT_I2C
 
-#define SCL_H         GPIOB->BSRR = Pin_10 /* GPIO_SetBits(GPIOB , GPIO_Pin_10)   */
-#define SCL_L         GPIOB->BRR  = Pin_10 /* GPIO_ResetBits(GPIOB , GPIO_Pin_10) */
+#ifdef SOFT_I2C_PB1011
+#define SCL_H         GPIOB->BSRR = Pin_10
+#define SCL_L         GPIOB->BRR  = Pin_10
 
-#define SDA_H         GPIOB->BSRR = Pin_11 /* GPIO_SetBits(GPIOB , GPIO_Pin_11)   */
-#define SDA_L         GPIOB->BRR  = Pin_11 /* GPIO_ResetBits(GPIOB , GPIO_Pin_11) */
+#define SDA_H         GPIOB->BSRR = Pin_11
+#define SDA_L         GPIOB->BRR  = Pin_11
 
-#define SCL_read      (GPIOB->IDR & Pin_10) /* GPIO_ReadInputDataBit(GPIOB , GPIO_Pin_10) */
-#define SDA_read      (GPIOB->IDR & Pin_11) /* GPIO_ReadInputDataBit(GPIOB , GPIO_Pin_11) */
+#define SCL_read      (GPIOB->IDR & Pin_10)
+#define SDA_read      (GPIOB->IDR & Pin_11)
+#define I2C_PINS      (Pin_10 | Pin_11)
+#define I2C_GPIO      GPIOB
+#endif
+
+#ifdef SOFT_I2C_PB67
+#define SCL_H         GPIOB->BSRR = Pin_6
+#define SCL_L         GPIOB->BRR  = Pin_6
+
+#define SDA_H         GPIOB->BSRR = Pin_7
+#define SDA_L         GPIOB->BRR  = Pin_7
+
+#define SCL_read      (GPIOB->IDR & Pin_6)
+#define SDA_read      (GPIOB->IDR & Pin_7)
+#define I2C_PINS      (Pin_6 | Pin_7)
+#define I2C_GPIO      GPIOB
+#endif
+
+#ifndef SCL_H
+#error Need to define SOFT_I2C_PB1011 or SOFT_I2C_PB67 (see board.h)
+#endif
 
 static void I2C_delay(void)
 {
@@ -160,10 +180,10 @@ void i2cInit(I2C_TypeDef * I2C)
 {
     gpio_config_t gpio;
 
-    gpio.pin = Pin_10 | Pin_11;
+    gpio.pin = I2C_PINS;
     gpio.speed = Speed_2MHz;
     gpio.mode = Mode_Out_OD;
-    gpioInit(GPIOB, &gpio);
+    gpioInit(I2C_GPIO, &gpio);
 }
 
 bool i2cWriteBuffer(uint8_t addr, uint8_t reg, uint8_t len, uint8_t * data)
diff --git a/src/main/drivers/bus_i2c_stm32f10x.c b/src/main/drivers/bus_i2c_stm32f10x.c
index 2648e79903..194d79508c 100755
--- a/src/main/drivers/bus_i2c_stm32f10x.c
+++ b/src/main/drivers/bus_i2c_stm32f10x.c
@@ -32,12 +32,34 @@
 // I2C2
 // SCL  PB10
 // SDA  PB11
+// I2C1
+// SCL  PB6
+// SDA  PB7
 
-static I2C_TypeDef *I2Cx;
 static void i2c_er_handler(void);
 static void i2c_ev_handler(void);
 static void i2cUnstick(void);
 
+typedef struct i2cDevice_t {
+    I2C_TypeDef *dev;
+    GPIO_TypeDef *gpio;
+    uint16_t scl;
+    uint16_t sda;
+    uint8_t ev_irq;
+    uint8_t er_irq;
+    uint32_t peripheral;
+} i2cDevice_t;
+
+static const i2cDevice_t i2cHardwareMap[] = {
+    { I2C1, GPIOB, Pin_6, Pin_7, I2C1_EV_IRQn, I2C1_ER_IRQn, RCC_APB1Periph_I2C1 },
+    { I2C2, GPIOB, Pin_10, Pin_11, I2C2_EV_IRQn, I2C2_ER_IRQn, RCC_APB1Periph_I2C2 },
+};
+
+// Copy of peripheral address for IRQ routines
+static I2C_TypeDef *I2Cx;
+// Copy of device index for reinit, etc purposes
+static I2CDevice I2Cx_index;
+
 void I2C1_ER_IRQHandler(void)
 {
     i2c_er_handler();
@@ -76,7 +98,7 @@ static bool i2cHandleHardwareFailure(void)
 {
     i2cErrorCount++;
     // reinit peripheral + clock out garbage
-    i2cInit(I2Cx);
+    i2cInit(I2Cx_index);
     return false;
 }
 
@@ -166,7 +188,7 @@ static void i2c_er_handler(void)
                 while (I2Cx->CR1 & 0x0100) { ; }                        // wait for any start to finish sending
                 I2C_GenerateSTOP(I2Cx, ENABLE);                         // send stop to finalise bus transaction
                 while (I2Cx->CR1 & 0x0200) { ; }                        // wait for stop to finish sending
-                i2cInit(I2Cx);                                          // reset and configure the hardware
+                i2cInit(I2Cx_index);                                    // reset and configure the hardware
             } else {
                 I2C_GenerateSTOP(I2Cx, ENABLE);                         // stop to free up the bus
                 I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE);   // Disable EVT and ERR interrupts while bus inactive
@@ -277,26 +299,24 @@ void i2c_ev_handler(void)
     }
 }
 
-void i2cInit(I2C_TypeDef *I2C)
+void i2cInit(I2CDevice index)
 {
     NVIC_InitTypeDef nvic;
     I2C_InitTypeDef i2c;
-    gpio_config_t gpio;
 
-    RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2 | RCC_APB2Periph_GPIOB, ENABLE);
+    if (index > I2CDEV_MAX)
+        index = I2CDEV_MAX;
 
-    // Init pins
-    gpio.pin = Pin_10 | Pin_11;
-    gpio.speed = Speed_2MHz;
-    gpio.mode = Mode_AF_OD;
-    gpioInit(GPIOB, &gpio);
-
-    I2Cx = I2C;
+    // Turn on peripheral clock, save device and index
+    I2Cx = i2cHardwareMap[index].dev;
+    I2Cx_index = index;
+    RCC_APB1PeriphClockCmd(i2cHardwareMap[index].peripheral, ENABLE);
 
     // clock out stuff to make sure slaves arent stuck
+    // This will also configure GPIO as AF_OD at the end
     i2cUnstick();
 
-    // Init I2C
+    // Init I2C peripheral
     I2C_DeInit(I2Cx);
     I2C_StructInit(&i2c);
 
@@ -309,14 +329,14 @@ void i2cInit(I2C_TypeDef *I2C)
     I2C_Init(I2Cx, &i2c);
 
     // I2C ER Interrupt
-    nvic.NVIC_IRQChannel = I2C2_ER_IRQn;
+    nvic.NVIC_IRQChannel = i2cHardwareMap[index].er_irq;
     nvic.NVIC_IRQChannelPreemptionPriority = 0;
     nvic.NVIC_IRQChannelSubPriority = 0;
     nvic.NVIC_IRQChannelCmd = ENABLE;
     NVIC_Init(&nvic);
 
     // I2C EV Interrupt
-    nvic.NVIC_IRQChannel = I2C2_EV_IRQn;
+    nvic.NVIC_IRQChannel = i2cHardwareMap[index].ev_irq;
     nvic.NVIC_IRQChannelPreemptionPriority = 0;
     NVIC_Init(&nvic);
 }
@@ -328,44 +348,52 @@ uint16_t i2cGetErrorCounter(void)
 
 static void i2cUnstick(void)
 {
-    gpio_config_t gpio;
-    uint8_t i;
+    GPIO_TypeDef *gpio;
+    gpio_config_t cfg;
+    uint16_t scl, sda;
+    int i;
 
-    gpio.pin = Pin_10 | Pin_11;
-    gpio.speed = Speed_2MHz;
-    gpio.mode = Mode_Out_OD;
-    gpioInit(GPIOB, &gpio);
+    // prepare pins
+    gpio = i2cHardwareMap[I2Cx_index].gpio;
+    scl = i2cHardwareMap[I2Cx_index].scl;
+    sda = i2cHardwareMap[I2Cx_index].sda;
+
+    digitalHi(gpio, scl | sda);
+
+    cfg.pin = scl | sda;
+    cfg.speed = Speed_2MHz;
+    cfg.mode = Mode_Out_OD;
+    gpioInit(gpio, &cfg);
 
-    digitalHi(GPIOB, Pin_10 | Pin_11);
     for (i = 0; i < 8; i++) {
         // Wait for any clock stretching to finish
-        while (!digitalIn(GPIOB, Pin_10))
+        while (!digitalIn(gpio, scl))
             delayMicroseconds(10);
 
         // Pull low
-        digitalLo(GPIOB, Pin_10); // Set bus low
+        digitalLo(gpio, scl); // Set bus low
         delayMicroseconds(10);
         // Release high again
-        digitalHi(GPIOB, Pin_10); // Set bus high
+        digitalHi(gpio, scl); // Set bus high
         delayMicroseconds(10);
     }
 
     // Generate a start then stop condition
     // SCL  PB10
     // SDA  PB11
-    digitalLo(GPIOB, Pin_11); // Set bus data low
+    digitalLo(gpio, sda); // Set bus data low
     delayMicroseconds(10);
-    digitalLo(GPIOB, Pin_10); // Set bus scl low
+    digitalLo(gpio, scl); // Set bus scl low
     delayMicroseconds(10);
-    digitalHi(GPIOB, Pin_10); // Set bus scl high
+    digitalHi(gpio, scl); // Set bus scl high
     delayMicroseconds(10);
-    digitalHi(GPIOB, Pin_11); // Set bus sda high
+    digitalHi(gpio, sda); // Set bus sda high
 
     // Init pins
-    gpio.pin = Pin_10 | Pin_11;
-    gpio.speed = Speed_2MHz;
-    gpio.mode = Mode_AF_OD;
-    gpioInit(GPIOB, &gpio);
+    cfg.pin = scl | sda;
+    cfg.speed = Speed_2MHz;
+    cfg.mode = Mode_AF_OD;
+    gpioInit(gpio, &cfg);
 }
 
 #endif
diff --git a/src/main/drivers/bus_i2c_stm32f30x.c b/src/main/drivers/bus_i2c_stm32f30x.c
index 46c77c7e6a..a42490ce57 100644
--- a/src/main/drivers/bus_i2c_stm32f30x.c
+++ b/src/main/drivers/bus_i2c_stm32f30x.c
@@ -160,7 +160,7 @@ void i2cInitPort(I2C_TypeDef *I2Cx)
     }
 }
 
-void i2cInit(I2C_TypeDef *I2C)
+void i2cInit(I2CDevice index)
 {
     i2cInitPort(I2C1); // hard coded to use I2C1 for now
 }
diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c
index 93846b9278..bc3e58231f 100755
--- a/src/main/drivers/system.c
+++ b/src/main/drivers/system.c
@@ -126,7 +126,7 @@ void systemInit(bool overclock)
 
 #ifndef CC3D
     // Configure the rest of the stuff
-    i2cInit(I2C2);
+    i2cInit(I2CDEV_2);
 #endif
 
     // sleep for 100ms
diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h
index 30fd5e3a64..fe2025608b 100644
--- a/src/main/target/CC3D/target.h
+++ b/src/main/target/CC3D/target.h
@@ -23,4 +23,8 @@
 #define ACC
 #define GYRO
 
+// #define SOFT_I2C // enable to test software i2c
+// #define SOFT_I2C_PB1011 // If SOFT_I2C is enabled above, need to define pinout as well (I2C1 = PB67, I2C2 = PB1011)
+// #define SOFT_I2C_PB67
+
 #define SENSORS_SET (SENSOR_ACC)
diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h
index e3421c6e90..db8887985f 100644
--- a/src/main/target/NAZE/target.h
+++ b/src/main/target/NAZE/target.h
@@ -41,4 +41,8 @@
 #define LED0
 #define LED1
 
+// #define SOFT_I2C // enable to test software i2c
+// #define SOFT_I2C_PB1011 // If SOFT_I2C is enabled above, need to define pinout as well (I2C1 = PB67, I2C2 = PB1011)
+// #define SOFT_I2C_PB67
+
 #define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
diff --git a/src/main/target/OLIMEXINO/target.h b/src/main/target/OLIMEXINO/target.h
index 6b0976d7ad..e4bc4043b1 100644
--- a/src/main/target/OLIMEXINO/target.h
+++ b/src/main/target/OLIMEXINO/target.h
@@ -40,4 +40,8 @@
 #define GYRO
 #define MAG
 
+// #define SOFT_I2C // enable to test software i2c
+// #define SOFT_I2C_PB1011 // If SOFT_I2C is enabled above, need to define pinout as well (I2C1 = PB67, I2C2 = PB1011)
+// #define SOFT_I2C_PB67
+
 #define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)