diff --git a/firmware/cup.c b/firmware/cup.c
index 108ccf3..a19db2c 100644
--- a/firmware/cup.c
+++ b/firmware/cup.c
@@ -10,7 +10,7 @@
 #define FET0_PIN 0
 #define FET1_PIN 1
 
-#define TIMEOUT 255
+#define TIMEOUT 51 /* 2 seconds */
 
 const uint16_t TARGET_TEMP_MAX = 1520;
 
@@ -162,8 +162,9 @@ void adc_init(void)
 
 void heat_init(void)
 {
-	TCCR1B = (1 << CS12) | (1 << CS10);/* prescaler / 1024 */
+	TCCR1B = (1 << WGM12) | (1 << CS10);/* some mode allowing to set timer TOP, no prescaler */
 	TIMSK1 = (1 << TOIE1);/* interupt on overflow */
+	OCR1A = 39063;
 	/* insert loading of heat setting here */
 }
 
@@ -196,7 +197,18 @@ int main(void)
 				adc_sum = 0;
 				adc_pos = 0;
 				temperature = linearize_temp(temperature_adc);
-				heat_output = control_output(temperature, target);
+				if(dev_state)
+				{
+					heat_output = control_output(temperature, target);
+					heat_on[0] = 0;
+					heat_on[1] = 128;
+					heat_off[0] = heat_output;
+					heat_off[1] = heat_output + 128;
+				}
+				else
+				{
+					heat_on[0] = heat_on[1] = heat_off[0] = heat_off[1] = 0;
+				}
 			}
 		}