diff --git a/libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h b/libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h
index 0e40813843..2c1f16c0c9 100644
--- a/libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h
+++ b/libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h
@@ -14,6 +14,8 @@ namespace PX4 {
         class PX4GPIO;
         class PX4DigitalSource;
         class NSHShellStream;
+        class PX4I2CDriver;
+        class PX4_I2C;
 }
 
 #endif //__AP_HAL_PX4_NAMESPACE_H__
diff --git a/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp b/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp
index 7309a612c9..47b8e83063 100644
--- a/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp
+++ b/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp
@@ -15,6 +15,7 @@
 #include "AnalogIn.h"
 #include "Util.h"
 #include "GPIO.h"
+#include "I2CDriver.h"
 
 #include <AP_HAL_Empty/AP_HAL_Empty.h>
 #include <AP_HAL_Empty/AP_HAL_Empty_Private.h>
@@ -30,8 +31,7 @@
 
 using namespace PX4;
 
-static Empty::EmptySemaphore  i2cSemaphore;
-static Empty::EmptyI2CDriver  i2cDriver(&i2cSemaphore);
+static PX4I2CDriver i2cDriver;
 static Empty::EmptySPIDeviceManager spiDeviceManager;
 //static Empty::EmptyGPIO gpioDriver;
 
diff --git a/libraries/AP_HAL_PX4/I2CDriver.cpp b/libraries/AP_HAL_PX4/I2CDriver.cpp
new file mode 100644
index 0000000000..39e94edb9e
--- /dev/null
+++ b/libraries/AP_HAL_PX4/I2CDriver.cpp
@@ -0,0 +1,93 @@
+#include <AP_HAL/AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+
+#include "I2CDriver.h"
+#include <drivers/device/i2c.h>
+#include <arch/board/board.h>
+#include "board_config.h"
+
+extern const AP_HAL::HAL& hal;
+
+using namespace PX4;
+
+/*
+  wrapper class for I2C to expose protected functions from PX4Firmware
+ */
+class PX4::PX4_I2C : public device::I2C {
+public:
+    PX4_I2C(uint8_t bus);
+    bool do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len);
+};
+
+// constructor
+PX4_I2C::PX4_I2C(uint8_t bus) : 
+    I2C("AP_I2C", "/dev/api2c", bus, 0, 400000UL)
+{
+    /*
+      attach to the bus. Return value can be ignored as we have no probe function
+    */
+    init();
+}
+
+/*
+  main transfer function
+ */
+bool PX4_I2C::do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len)
+{
+    set_address(address);
+    return transfer(send, send_len, recv, recv_len) == OK;
+}
+
+
+// constructor for main i2c class
+PX4I2CDriver::PX4I2CDriver(void)
+{
+    px4_i2c = new PX4_I2C(PX4_I2C_BUS_EXPANSION);
+    if (px4_i2c == nullptr) {
+        hal.scheduler->panic("Unable to allocate PX4 I2C driver");
+    }
+}
+
+/* write: for i2c devices which do not obey register conventions */
+uint8_t PX4I2CDriver::write(uint8_t addr, uint8_t len, uint8_t* data)
+{
+    return px4_i2c->do_transfer(addr, data, len, nullptr, 0) ? 0:1;
+}
+
+/* writeRegister: write a single 8-bit value to a register */
+uint8_t PX4I2CDriver::writeRegister(uint8_t addr, uint8_t reg, uint8_t val)
+{
+    uint8_t d[2] = { reg, val };
+    return px4_i2c->do_transfer(addr, d, sizeof(d), nullptr, 0) ? 0:1;
+}
+
+/* writeRegisters: write bytes to contigious registers */
+uint8_t PX4I2CDriver::writeRegisters(uint8_t addr, uint8_t reg,
+                                     uint8_t len, uint8_t* data)
+{
+    return write(addr, 1, &reg) == 0 && write(addr, len, data) == 0;
+}
+
+/* read: for i2c devices which do not obey register conventions */
+uint8_t PX4I2CDriver::read(uint8_t addr, uint8_t len, uint8_t* data)
+{
+    return px4_i2c->do_transfer(addr, nullptr, 0, data, len) ? 0:1;
+}
+    
+/* readRegister: read from a device register - writes the register,
+ * then reads back an 8-bit value. */
+uint8_t PX4I2CDriver::readRegister(uint8_t addr, uint8_t reg, uint8_t* data)
+{
+    return px4_i2c->do_transfer(addr, &reg, 1, data, 1) ? 0:1;
+}
+
+/* readRegister: read contigious device registers - writes the first 
+ * register, then reads back multiple bytes */
+uint8_t PX4I2CDriver::readRegisters(uint8_t addr, uint8_t reg,
+                                    uint8_t len, uint8_t* data)
+{
+    return px4_i2c->do_transfer(addr, &reg, 1, data, len) ? 0:1;
+}
+
+#endif // CONFIG_HAL_BOARD
diff --git a/libraries/AP_HAL_PX4/I2CDriver.h b/libraries/AP_HAL_PX4/I2CDriver.h
new file mode 100644
index 0000000000..c08a4a7123
--- /dev/null
+++ b/libraries/AP_HAL_PX4/I2CDriver.h
@@ -0,0 +1,48 @@
+
+#ifndef __AP_HAL_PX4_I2CDRIVER_H__
+#define __AP_HAL_PX4_I2CDRIVER_H__
+
+#include "AP_HAL_PX4.h"
+#include <AP_HAL_Empty/AP_HAL_Empty.h>
+#include <AP_HAL_Empty/AP_HAL_Empty_Private.h>
+
+class PX4::PX4I2CDriver : public AP_HAL::I2CDriver {
+public:
+    PX4I2CDriver(void);
+
+    void begin() {}
+    void end() {}
+    void setTimeout(uint16_t ms) {}
+    void setHighSpeed(bool active) {}
+
+    /* write: for i2c devices which do not obey register conventions */
+    uint8_t write(uint8_t addr, uint8_t len, uint8_t* data);
+
+    /* writeRegister: write a single 8-bit value to a register */
+    uint8_t writeRegister(uint8_t addr, uint8_t reg, uint8_t val);
+
+    /* writeRegisters: write bytes to contigious registers */
+    uint8_t writeRegisters(uint8_t addr, uint8_t reg, uint8_t len, uint8_t* data);
+
+    /* read: for i2c devices which do not obey register conventions */
+    uint8_t read(uint8_t addr, uint8_t len, uint8_t* data);
+    
+    /* readRegister: read from a device register - writes the register,
+     * then reads back an 8-bit value. */
+    uint8_t readRegister(uint8_t addr, uint8_t reg, uint8_t* data);
+
+    /* readRegister: read contigious device registers - writes the first 
+     * register, then reads back multiple bytes */
+    uint8_t readRegisters(uint8_t addr, uint8_t reg, uint8_t len, uint8_t* data);
+    
+    uint8_t lockup_count() { return 0; }
+
+    AP_HAL::Semaphore* get_semaphore() { return &semaphore; }
+
+private:
+    // we use an empty semaphore as the underlying I2C class already has a semaphore
+    Empty::EmptySemaphore semaphore;
+    PX4_I2C *px4_i2c = nullptr;
+};
+
+#endif // __AP_HAL_PX4_I2CDRIVER_H__