diff --git a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp
index a4316e7a0f4..fc2239f8232 100644
--- a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp
+++ b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp
@@ -109,6 +109,9 @@ static THD_FUNCTION(main_loop,arg)
 {
     daemon_task = chThdGetSelfX();
 
+    //Clear all I2C Buses
+    ChibiOS::I2CBus::clear_all();
+
     ChibiOS::Shared_DMA::init();
     
     hal.uartA->begin(115200);
diff --git a/libraries/AP_HAL_ChibiOS/I2CDevice.cpp b/libraries/AP_HAL_ChibiOS/I2CDevice.cpp
index 8a656e0f916..15ab6298b97 100644
--- a/libraries/AP_HAL_ChibiOS/I2CDevice.cpp
+++ b/libraries/AP_HAL_ChibiOS/I2CDevice.cpp
@@ -51,6 +51,38 @@ void I2CBus::dma_init(void)
                                 FUNCTOR_BIND_MEMBER(&I2CBus::dma_deallocate, void));    
 }
 
+// Clear Bus to avoid bus lockup
+void I2CBus::clear_all()
+{
+#if defined(HAL_GPIO_PIN_I2C1_SCL) && defined(HAL_I2C1_SCL_AF)
+    clear_bus(HAL_GPIO_PIN_I2C1_SCL, HAL_I2C1_SCL_AF);
+#endif
+
+#if defined(HAL_GPIO_PIN_I2C2_SCL) && defined(HAL_I2C2_SCL_AF)
+    clear_bus(HAL_GPIO_PIN_I2C1_SCL, HAL_I2C1_SCL_AF);
+#endif
+
+#if defined(HAL_GPIO_PIN_I2C3_SCL) && defined(HAL_I2C3_SCL_AF)
+    clear_bus(HAL_GPIO_PIN_I2C1_SCL, HAL_I2C1_SCL_AF);
+#endif
+
+#if defined(HAL_GPIO_PIN_I2C4_SCL) && defined(HAL_I2C4_SCL_AF)
+    clear_bus(HAL_GPIO_PIN_I2C1_SCL, HAL_I2C1_SCL_AF);
+#endif
+}
+
+//This code blocks!
+void I2CBus::clear_bus(ioline_t scl_line, uint8_t scl_af)
+{
+    //send dummy clock
+    palSetLineMode(scl_line, PAL_MODE_OUTPUT_PUSHPULL);
+    for(int i = 0; i < 20; i++) {
+        palToggleLine(scl_line);
+        hal.scheduler->delay_microseconds(200);
+    }
+    palSetLineMode(scl_line, PAL_MODE_ALTERNATE(scl_af) | PAL_STM32_OSPEED_MID2 | PAL_STM32_OTYPE_OPENDRAIN);
+}
+
 // setup I2C buses
 I2CDeviceManager::I2CDeviceManager(void)
 {
diff --git a/libraries/AP_HAL_ChibiOS/I2CDevice.h b/libraries/AP_HAL_ChibiOS/I2CDevice.h
index b838cd39bab..661e17ffeaa 100644
--- a/libraries/AP_HAL_ChibiOS/I2CDevice.h
+++ b/libraries/AP_HAL_ChibiOS/I2CDevice.h
@@ -40,6 +40,8 @@ public:
     void dma_allocate(void);
     void dma_deallocate(void);    
     void dma_init(void);
+    static void clear_all(void);
+    static void clear_bus(ioline_t scl_line, uint8_t scl_af);
 };
     
 class I2CDevice : public AP_HAL::I2CDevice {
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py
index 7df65a319bb..21871dd9ecc 100755
--- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py
+++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py
@@ -509,6 +509,11 @@ def write_I2C_config(f):
     for dev in i2c_list:
         if not dev.startswith('I2C') or dev[3] not in "1234":
             error("Bad I2C_ORDER element %s" % dev)
+        if dev + "_SCL" in bylabel:
+            p = bylabel[dev + "_SCL"]
+            f.write(
+                '#define HAL_%s_SCL_AF %d\n' % (dev, p.af)
+            )
         n = int(dev[3:])
         devlist.append('HAL_I2C%u_CONFIG' % n)
         f.write(