diff --git a/flight/OpenPilot/Tests/test_i2c_PCF8570.c b/flight/OpenPilot/Tests/test_i2c_PCF8570.c
index 2d0722859..516887836 100644
--- a/flight/OpenPilot/Tests/test_i2c_PCF8570.c
+++ b/flight/OpenPilot/Tests/test_i2c_PCF8570.c
@@ -109,7 +109,16 @@ static void Task1(void *pvParameters)
 	int i = 0;
 
 
-	PIOS_I2C_Transfer(I2C_Write, DEVICE_1_ADDRESS<<1, (uint8_t*)"\x20\xB0\xB1\xB2", 4);
+	if (PIOS_I2C_LockDevice(100))
+	{
+		if (PIOS_I2C_Transfer(I2C_Write, DEVICE_1_ADDRESS<<1, (uint8_t*)"\x20\xB0\xB1\xB2", 4) != 0)
+			OnError();
+		PIOS_I2C_UnlockDevice();
+	}
+	else
+	{
+		OnError();
+	}
 
 	for(;;)
 	{
@@ -179,10 +188,10 @@ static void Task2(void *pvParameters)
 
 		if (PIOS_I2C_LockDevice(100))
 		{
-			if (PIOS_I2C_Transfer(I2C_Write_WithoutStop, DEVICE_1_ADDRESS<<1, (uint8_t*)"\x10", 1) != 0)
+			if (PIOS_I2C_Transfer(I2C_Write_WithoutStop, DEVICE_2_ADDRESS<<1, (uint8_t*)"\x10", 1) != 0)
 				OnError();
 
-			if (PIOS_I2C_Transfer(I2C_Read, DEVICE_1_ADDRESS<<1, buf, 3) != 0)
+			if (PIOS_I2C_Transfer(I2C_Read, DEVICE_2_ADDRESS<<1, buf, 3) != 0)
 				OnError();
 
 			if (memcmp(buf, "\xF0\xF1\xF2",3) != 0)