diff --git a/well_control.ino b/well_control.ino
index f3aebde..63fd53b 100644
--- a/well_control.ino
+++ b/well_control.ino
@@ -7,6 +7,9 @@
 #define TCP_PORT 15000
 
 #define SENSOR_UPDATE_INTERVAL 500
+#define SCREEN_UPDATE_INTERVAL 200
+// Interval between allowed switch-states of the pump
+#define PUMP_SWITCH_INTERVAL 1000
 
 #define MODE_AUTO 0
 #define MODE_MANUAL 1
@@ -49,6 +52,7 @@ void setup()
 	Serial.begin(115200);
 	Serial.println("Initialized...");
 
+	WiFi.mode(WIFI_STA);
 	WiFi.begin(WIFI_NAME, WIFI_PASS);
 	Serial.print("Connecting");
 	while (WiFi.status() != WL_CONNECTED)
@@ -78,16 +82,24 @@ void setup()
 }
 
 
-void UpdateSensorData()
+void UpdateDHTData()
 {
 	float h = dht.readHumidity();
 	float t = dht.readTemperature();
-	Serial.print("read h ");
-	Serial.print(h);
-	Serial.print(" read t ");
-	Serial.println(t);
 	cur_hum = (int)(h * 100);
 	cur_t = (int)(t * 100);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////
+// Workings with pressure sensor
+///////////////////////////////////////////////////////////////////////////////////////////////////
+
+const int MEASURES_AMOUNT = 100;
+int measures[MEASURES_AMOUNT];
+int measure_pos = 0;
+
+void UpdatePressure()
+{
 	int p_int = analogRead(A0);
 	// we have 10-bit ADC, so get value between 0 and 1 and then multiply by max voltage
 	// (it is 3.3)
@@ -95,29 +107,83 @@ void UpdateSensorData()
 	// divide by maximal input voltage from sensor (it is 2.5)
 	// and then multiply by sensor max value (10 bar) - we will get pressure in bar
 	// finally multiply by 100 as we have int registers
-	cur_p = (int)(((p_volt / 2.5f) * 10) * 100);
+	measures[measure_pos] = (int)(((p_volt / 2.5f) * 10) * 100);
+	measure_pos++;
+	// We will use average of MEASURES_AMOUNT last sensor readings
+	// excluding minimal and maximal value (to get rid of peaks)
+	if (measure_pos >= MEASURES_AMOUNT) measure_pos = 0;
+	int min_pos = 0;
+	int max_pos = 0;
+	for (int i = 0; i < MEASURES_AMOUNT; i++)
+	{
+		if (measures[i] > measures[max_pos]) max_pos = i;
+		if (measures[i] < measures[min_pos]) min_pos = i;
+	}
+	int sum = 0;
+	for (int i = 0; i < MEASURES_AMOUNT; i++)
+	{
+		if (i == min_pos || i == max_pos) continue;
+		sum += measures[i];
+	}
+	cur_p = sum / (MEASURES_AMOUNT - 2);
 }
 
+///////////////////////////////////////////////////////////////////////////////////////////////////
+
+unsigned long last_screen_update = 0;
+
 void UpdateDisplay()
 {
-	lcd.setCursor(0, 0);
-	lcd.print("P: ");
-	lcd.print((cur_p / 100.0), 2);
-	lcd.print(" T: ");
-	lcd.print((cur_t / 100.0), 1);
-	lcd.setCursor(0, 1);
-	lcd.print("H: ");
-	lcd.print(cur_hum / 100);
-	// print current error
-	//
-	if (cur_state)
+	unsigned long current = millis();
+	if (current < last_screen_update || current - last_screen_update >= SCREEN_UPDATE_INTERVAL)
 	{
-		lcd.setRGB(0, 0, 255);
+		lcd.setCursor(0, 0);
+		lcd.print("P: ");
+		lcd.print((cur_p / 100.0), 2);
+		lcd.print(" T: ");
+		lcd.print((cur_t / 100.0), 1);
+		lcd.setCursor(0, 1);
+		lcd.print("H: ");
+		lcd.print(cur_hum / 100);
+		lcd.print(" WiFi: ");
+		lcd.print(WiFi.status());
+		// print current error
+		//
+		if (cur_state)
+		{
+			lcd.setRGB(0, 0, 255);
+		}
+		else
+		{
+			lcd.setRGB(0, 255, 0);
+		}
+		last_screen_update = current;
+	}
+}
+
+unsigned long last_pump_switch = 0;
+
+void UpdatePump()
+{
+	unsigned long current = millis();
+	int new_state = 0;
+	if (cur_mode == MODE_AUTO)
+	{
+		if (cur_p < p_on) new_state = 1;
+		else if (cur_p > p_off) new_state = 0;
+		if ((current < last_pump_switch || current - last_pump_switch > PUMP_SWITCH_INTERVAL)
+			&& new_state != cur_state)
+		{
+			cur_state = new_state;
+			mb.Hreg(REGISTER_CUR_STATE, cur_state);
+			last_pump_switch = current;
+		}
 	}
 	else
 	{
-		lcd.setRGB(0, 255, 0);
+		cur_state = mb.Hreg(REGISTER_CUR_STATE);
 	}
+	digitalWrite(D4, cur_state);
 }
 
 void loop() 
@@ -126,25 +192,16 @@ void loop()
 	int u = 0;
 	if (current < last_update || current - last_update >= SENSOR_UPDATE_INTERVAL)
 	{
-		UpdateSensorData();
+		UpdateDHTData();
 		last_update = current;
 		u = 1;
 	}
+	UpdatePressure();
 	mb.task();
 	p_on = mb.Hreg(REGISTER_P_ON);
 	p_off = mb.Hreg(REGISTER_P_OFF);
 	cur_mode = mb.Hreg(REGISTER_MODE);
-	if (cur_mode == MODE_AUTO)
-	{
-		if (cur_p < p_on) cur_state = 1;
-		else if (cur_p > p_off) cur_state = 0;
-		mb.Hreg(REGISTER_CUR_STATE, cur_state);
-	}
-	else
-	{
-		cur_state = mb.Hreg(REGISTER_CUR_STATE);
-	}
-	digitalWrite(D4, cur_state);
+	UpdatePump();
 	if (u) 
 	{
 		Serial.print("Current pressure: ");
@@ -165,8 +222,5 @@ void loop()
 	UpdateDisplay();
 	if (WiFi.status() != WL_CONNECTED)
 	{
-		// no wifi
-		Serial.println("No WiFi connection");
-		ESP.restart();
 	}
 }