diff --git a/flight/PiOS/Common/pios_bmp085.c b/flight/PiOS/Common/pios_bmp085.c
index 7e5d160c3..5ccd79272 100644
--- a/flight/PiOS/Common/pios_bmp085.c
+++ b/flight/PiOS/Common/pios_bmp085.c
@@ -196,7 +196,7 @@ int32_t PIOS_BMP085_GetPressure(void)
 int32_t PIOS_BMP085_Read(uint8_t address, uint8_t *buffer, uint8_t len)
 {
 	/* Try to get the I2C peripheral */
-	if(PIOS_I2C_LockDevice(I2C_Non_Blocking) < 0) {
+	if(PIOS_I2C_LockDevice(0) < 0) {
 		/* Request a retry */
 		return -2;
 	}
@@ -230,7 +230,7 @@ int32_t PIOS_BMP085_Read(uint8_t address, uint8_t *buffer, uint8_t len)
 int32_t PIOS_BMP085_Write(uint8_t address, uint8_t buffer)
 {
 	/* Try to get the IIC peripheral */
-	if(PIOS_I2C_LockDevice(I2C_Non_Blocking) < 0) {
+	if(PIOS_I2C_LockDevice(0) < 0) {
 		/* Request a retry */
 		return -2;
 	}
diff --git a/flight/PiOS/STM32F10x/pios_i2c.c b/flight/PiOS/STM32F10x/pios_i2c.c
index 5fce2c491..f0c1bf16c 100644
--- a/flight/PiOS/STM32F10x/pios_i2c.c
+++ b/flight/PiOS/STM32F10x/pios_i2c.c
@@ -77,9 +77,9 @@ typedef struct {
 #ifdef USE_FREERTOS
 	xSemaphoreHandle sem_readySignal;
 	portBASE_TYPE xHigherPriorityTaskWoken;
+	xSemaphoreHandle xBusyMutex;
 #endif
 
-	volatile uint8_t i2c_semaphore;
 } I2CRecTypeDef;
 
 /* Local Prototypes */
@@ -131,11 +131,11 @@ int32_t PIOS_I2C_Init(void)
 
 	PIOS_I2C_InitPeripheral();
 
-	/* Now accessible for other tasks */
-	I2CRec.i2c_semaphore = 0;
+
 #ifdef USE_FREERTOS
 	vSemaphoreCreateBinary(I2CRec.sem_readySignal);
-#endif
+	I2CRec.xBusyMutex = xSemaphoreCreateMutex();
+#endif // USE_FREERTOS
 
 	TransferEnd(&I2CRec);
 
@@ -197,45 +197,38 @@ static void PIOS_I2C_InitPeripheral(void)
 }
 
 
+#ifdef USE_FREERTOS
 /**
 * Semaphore handling: requests the IIC interface
-* \param[in] semaphore_type is either IIC_Blocking or IIC_Non_Blocking
-* \return Non_Blocking: returns -1 to request a retry
-* \return 0 if IIC interface free
+* \param[in] timeout Timeout in ticks, 0 for no delay
+* \return TRUE when the lock to the device was obtained
 */
-int32_t PIOS_I2C_LockDevice(I2CSemaphoreTypeDef semaphore_type)
+bool PIOS_I2C_LockDevice(portTickType timeout)
 {
-	volatile I2CRecTypeDef *i2cx = &I2CRec;
-	int32_t status = -1;
-
-	do {
-		PIOS_IRQ_Disable();
-		if(!i2cx->i2c_semaphore) {
-			i2cx->i2c_semaphore = 1;
-			status = 0;
-		}
-		PIOS_IRQ_Enable();
-	} while(semaphore_type == I2C_Blocking && status != 0);
-
-	/* Clear transfer errors of last transmission */
-	i2cx->transfer_error = 0;
-
-	return status;
+	if (xSemaphoreTake(I2CRec.xBusyMutex, timeout) == pdTRUE)
+	{
+		// Ok, got device
+		return TRUE;
+	}
+	else
+	{
+		return FALSE;
+	}
 }
 
 
+
 /**
 * Semaphore handling: releases the IIC interface for other tasks
 * \return < 0 on errors
 */
-int32_t PIOS_I2C_UnlockDevice(void)
+void PIOS_I2C_UnlockDevice(void)
 {
-	I2CRec.i2c_semaphore = 0;
-
-	/* No error */
-	return 0;
+	xSemaphoreGive(I2CRec.xBusyMutex);
 }
 
+#endif // USE_FREERTOS
+
 
 /**
  * Internal function called at the start of a transfer
diff --git a/flight/PiOS/inc/pios_i2c.h b/flight/PiOS/inc/pios_i2c.h
index 56782d28d..af535bc1b 100644
--- a/flight/PiOS/inc/pios_i2c.h
+++ b/flight/PiOS/inc/pios_i2c.h
@@ -41,10 +41,6 @@
 
 
 /* Global Types */
-typedef enum {
-	I2C_Blocking,
-	I2C_Non_Blocking
-} I2CSemaphoreTypeDef;
 
 typedef enum {
 	I2C_Read,
@@ -54,11 +50,14 @@ typedef enum {
 
 /* Public Functions */
 extern int32_t PIOS_I2C_Init(void);
-extern int32_t PIOS_I2C_LockDevice(I2CSemaphoreTypeDef semaphore_type);
-extern int32_t PIOS_I2C_UnlockDevice(void);
 extern int32_t PIOS_I2C_TransferCheck(void);
 extern int32_t PIOS_I2C_TransferWait(void);
 extern int32_t PIOS_I2C_Transfer(I2CTransferTypeDef transfer, uint8_t address, uint8_t *buffer, uint16_t len);
 extern void PIOS_I2C_StartTransfer(I2CTransferTypeDef transfer, uint8_t address, uint8_t *buffer, uint16_t len);
 
+#ifdef PIOS_INCLUDE_FREERTOS
+extern bool PIOS_I2C_LockDevice(portTickType timeout);
+extern void PIOS_I2C_UnlockDevice(void);
+#endif // PIOS_INCLUDE_FREERTOS
+
 #endif /* PIOS_I2C_H */