diff --git a/firmware/src/src.ino b/firmware/src/src.ino
index f0da5a6..c406c3d 100644
--- a/firmware/src/src.ino
+++ b/firmware/src/src.ino
@@ -30,7 +30,7 @@
   31	- Special allocation in JeeLib RFM12 driver - Node31 can communicate with nodes on any network group
   -------------------------------------------------------------------------------------------------------------
   Change log:
-  V3.0   - (15/09/16) Add support for SI7021 sensor instead of DHT22 (emonTH V2.0 hardware)
+  V3.0   - Oct16 Add support for SI7021 sensor instead of DHT22 (emonTH V2.0 hardware)
   ^^^ emonTH V2.0 hardware ^^^
   V2.7   - (15/09/16) Serial print serial pairs for emonesp compatiable e.g. temp:210,humidity:56
   V2.6   - (24/10/15) Tweek RF transmission timmng to help reduce RF packet loss
@@ -128,7 +128,7 @@ unsigned long WDT_number;
 boolean  p;
 
 unsigned long now = 0;
-
+const byte SLAVE_ADDRESS = 42;
 //################################################################################################################################
 //################################################################################################################################
 #ifndef UNIT_TEST // IMPORTANT LINE! // http://docs.platformio.org/en/stable/plus/unit-testing.html
@@ -203,14 +203,23 @@ void setup() {
   // Setup and for presence of si7201
   //################################################################################################################################
   if (debug==1) Serial.println("Int SI7201..");
-  SI7021_sensor.begin();
-  int deviceid = SI7021_sensor.getDeviceId();
-  if (deviceid!=0) {
-    Serial.print("SI7021 Started, ID: "); Serial.println(deviceid);
-    SI7021_status=1;
-    si7021_env data = SI7021_sensor.getHumidityAndTemperature();
-    Serial.print("SI7021 t: "); Serial.println(data.celsiusHundredths/100.0);
-    Serial.print("SI7021 h: "); Serial.println(data.humidityBasisPoints/100.0);
+
+  // check if the I2C lines are HIGH
+  if (digitalRead(SDA) == HIGH || digitalRead(SCL) == HIGH)
+  {
+    SI7021_sensor.begin();
+    int deviceid = SI7021_sensor.getDeviceId();
+    if (deviceid!=0) {
+      Serial.print("SI7021 Started, ID: "); Serial.println(deviceid);
+      SI7021_status=1;
+      si7021_env data = SI7021_sensor.getHumidityAndTemperature();
+      Serial.print("SI7021 t: "); Serial.println(data.celsiusHundredths/100.0);
+      Serial.print("SI7021 h: "); Serial.println(data.humidityBasisPoints/100.0);
+    }
+    else {
+      SI7021_status=0;
+      Serial.println("SI7021 Error");
+    }
   }
   else {
     SI7021_status=0;
diff --git a/firmware/test/test_main.cpp b/firmware/test/test_main.cpp
index e11b235..4ed8a19 100644
--- a/firmware/test/test_main.cpp
+++ b/firmware/test/test_main.cpp
@@ -7,12 +7,12 @@
 #include <Arduino.h>
 #include <unity.h>
 #include <SI7021.h>
-SI7021 SI7021_sensor_test;
+SI7021 sensor;
 
 #ifdef UNIT_TEST
 
 // void setUp(void) {
-// // set stuff up here
+  // wire.begin();
 // }
 
 // void tearDown(void) {
@@ -20,24 +20,65 @@ SI7021 SI7021_sensor_test;
 // }
 
 const byte LED =9;
+const byte si7021_address=40;
 
-void test_9_pin_number(void) {
-    TEST_ASSERT_EQUAL(LED, LED);
-}
 
-void test_led_state_high(void) {
+void led_state_high(void) {
     digitalWrite(LED, HIGH);
     TEST_ASSERT_EQUAL(digitalRead(LED), HIGH);
 }
 
-void test_led_state_low(void) {
+void led_state_low(void) {
     digitalWrite(LED, LOW);
     TEST_ASSERT_EQUAL(digitalRead(LED), LOW);
 }
 
+void scl_line_high(void){
+  TEST_ASSERT_EQUAL(digitalRead(SCL), HIGH);
+}
 
-void test_si7021(void) {
-    TEST_ASSERT_EQUAL(SI7021_sensor_test.begin(), 1);
+void sda_line_high(void){
+  TEST_ASSERT_EQUAL(digitalRead(SDA), HIGH);
+}
+
+void si7021_exists(void){
+    // Scan I2C bus for Si7021
+    byte found_address =0;
+    byte count = 0;
+    for (byte i = 1; i < 120; i++)
+    {
+      Wire.beginTransmission (i);
+      if (Wire.endTransmission () == 0)
+        {
+        Serial.print (i, HEX);
+        found_address = (i);
+        count++;
+        delay (1);
+        }
+    }
+    TEST_ASSERT_EQUAL_HEX(found_address, si7021_address);
+}
+
+void si7021_start(void) {
+    TEST_ASSERT_EQUAL(sensor.begin(), 1);
+}
+
+void si7021_read_temp(void) {
+    sensor.begin();
+    si7021_env data = sensor.getHumidityAndTemperature();
+    int temp = data.celsiusHundredths;
+    // 21.00 degC = 2100
+    // delta, expected, actual
+    TEST_ASSERT_INT_WITHIN(6000, 2100, temp);
+}
+
+void si7021_read_humidity(void) {
+    sensor.begin();
+    si7021_env data = sensor.getHumidityAndTemperature();
+    int humidity = data.humidityBasisPoints;
+    // 50.00 RH = 5000
+    // delta, expected, actual
+    TEST_ASSERT_INT_WITHIN(5000, 5000, humidity);
 }
 
 
@@ -45,26 +86,30 @@ void test_si7021(void) {
 
 void setup() {
     UNITY_BEGIN();    // IMPORTANT LINE!
-    RUN_TEST(test_9_pin_number);
+
+    // Test LED
     pinMode(LED, OUTPUT);
-    RUN_TEST(test_si7021);
+    RUN_TEST(led_state_high);
+    RUN_TEST(led_state_low);
+
+    // Test Sensor
+    RUN_TEST(scl_line_high);
+    RUN_TEST(sda_line_high);
+
+    // I2C lines need to be pulled high for I2C to work
+    if (digitalRead(SDA) == HIGH && digitalRead(SCL) == HIGH){
+      RUN_TEST(si7021_start);
+      RUN_TEST(si7021_read_temp);
+      RUN_TEST(si7021_read_humidity);
+    }
+
 }
 
-uint8_t i = 0;
-uint8_t max_blinks = 5;
 
 void loop() {
-    if (i < max_blinks)
-    {
-        RUN_TEST(test_led_state_high);
-        delay(500);
-        RUN_TEST(test_led_state_low);
-        delay(500);
-        i++;
-    }
-    else if (i == max_blinks) {
+
       UNITY_END(); // stop unit testing
-    }
 }
 
+
 #endif