diff --git a/firmware/weather_station.ino b/firmware/weather_station.ino
index 9d0a1c8..8cc67d4 100644
--- a/firmware/weather_station.ino
+++ b/firmware/weather_station.ino
@@ -155,7 +155,7 @@ char HeaterSi7021 (float humidity)
   // If Smart heater algorithm is disabled, reset the statemachine forever.
   // TempValid bit is also forced to 1, but it could be that we just came out of a heater period.
   // We assume that the client on the other side of the ModBus is smart enough to understand.
-  if (MeasuredData.StatusBits & 0x04)
+  if ( (MeasuredData.StatusBits & 0x04) == 0)
     state = 0;
 
   switch (state)
@@ -172,6 +172,7 @@ char HeaterSi7021 (float humidity)
     case 1:
       if (humidity >= 95) {
         if ( (millis() - StatemachineTimer) >= 3.6e+6 ) {
+        //if ( (millis() - StatemachineTimer) >= 300000 ) { // short delay for testing
           Heater = 1;
           TempValid = 0;
           StatemachineTimer = millis();
@@ -185,9 +186,9 @@ char HeaterSi7021 (float humidity)
       }      
       break;
 
-    // Heater is now on, let the sensor cook for 10 minutes
+    // Heater is now on, let the sensor cook for 5 minutes
     case 2:
-      if ( (millis() - StatemachineTimer) >= 600000 ) {
+      if ( (millis() - StatemachineTimer) >= 300000 ) {
         StatemachineTimer = millis();
         Heater = 0;
         state = 3;
@@ -196,9 +197,9 @@ char HeaterSi7021 (float humidity)
       }
       TempValid = 0;
       break;
-    // Heater is now off, let the sensor cool for 10 minutes
+    // Heater is now off, let the sensor cool for 15 minutes
     case 3:
-      if ( (millis() - StatemachineTimer) >= 600000 ) {
+      if ( (millis() - StatemachineTimer) >= 900000 ) {
         TempValid = 1;            // Sensor cooled, so we can take a valid temperature reading
         if (humidity >= 95) {
           // Humidity still above 95%, repeat heating/cooling
@@ -229,27 +230,31 @@ char HeaterSi7021 (float humidity)
 void ReadSi7021 (void)
 {
     char result=0x2;
+    float humidity;
 
     si7021.triggerMeasurement();
-    si7021.getHumidity(MeasuredData.Humidity);
+    si7021.getHumidity(humidity);
 
-    if (MeasuredData.Humidity>100 || MeasuredData.Humidity<0)
-      MeasuredData.Humidity = 100;
+    if (humidity>100 || humidity<0)
+      humidity = 100;
  
     //If humidity is larger than 95% switch on heater to get more acurate measurement and prevent memory offset
-    result = HeaterSi7021(MeasuredData.Humidity);
+    result = HeaterSi7021(humidity);
     MeasuredData.StatusBits &= 0xFFFC;  // Reset heater status bits to zero
     MeasuredData.StatusBits |= result;  // And set the proper bits to one if there are any. The result is we copied the status bits to the register
-  
-    // Scale for more decimal positions when converted to integer value for ModBus
-    MeasuredData.Humidity *= 100;
 
-    // Temperture readings are valid (as the sensor is not heated)
+    // Temperture and humidity readings are valid (as the sensor is not heated)
     if (result & 0x2) {
       si7021.getTemperature(MeasuredData.Temperature);
       // Scale for more decimal positions when converted to integer value for ModBus
       MeasuredData.Temperature *= 100;
       //Serial.print(F("Valid temp"));
+
+      si7021.getHumidity(MeasuredData.Humidity);
+      if (MeasuredData.Humidity>100 || MeasuredData.Humidity<0)
+        MeasuredData.Humidity = 100;
+      // Scale for more decimal positions when converted to integer value for ModBus
+      MeasuredData.Humidity *= 100;      
     }
     // Statemachine thinks it is time to switch on the heater
     if (result & 0x1) {