diff -r 7dc9003f86a8 -r 7de246feba5f main/co2meter.c
--- a/main/co2meter.c	Sun Oct 13 19:27:12 2019 +0200
+++ b/main/co2meter.c	Fri Oct 25 21:53:43 2019 +0200
@@ -30,6 +30,7 @@
 static TaskHandle_t			xTaskWifi = NULL;
 static TaskHandle_t			xTaskMQTT = NULL;
 const esp_app_desc_t			*app_desc = NULL;
+u8g2_t					u8g2;				///<  A structure which will contain all the data for one display
 
 
 extern unit_t				units[3];			///< Pressure test units
@@ -41,13 +42,60 @@
 extern int				count_pub;
 
 
+void screen_main(float t, float p1, float p2, float p3)
+{
+    char	buf[65];
+
+    u8g2_ClearBuffer(&u8g2);
+    u8g2_DrawHLine(&u8g2, 0, 14, 128);
+    u8g2_SetFont(&u8g2, u8g2_font_t0_15_tr);
+    sprintf(buf, "CO2 meter %s", app_desc->version);
+    u8g2_uint_t w = u8g2_GetStrWidth(&u8g2, buf);
+    u8g2_DrawStr(&u8g2, (128 - w) / 2,12, buf);
+
+    u8g2_SetFont(&u8g2, u8g2_font_t0_22b_tf);
+    sprintf(buf, "%.1f °C", t);
+    w = u8g2_GetUTF8Width(&u8g2, buf);
+    u8g2_DrawUTF8(&u8g2, (128 - w) / 2,40, buf);
+    u8g2_SetFont(&u8g2, u8g2_font_t0_18b_tf);
+
+    sprintf(buf, "%.1f", p1);
+    w = u8g2_GetUTF8Width(&u8g2, buf);
+    u8g2_DrawUTF8(&u8g2, ((42 - w) / 2),63, buf);
+
+    sprintf(buf, "%.1f", p2);
+    w = u8g2_GetUTF8Width(&u8g2, buf);
+    u8g2_DrawUTF8(&u8g2, ((42 - w) / 2) + 43,63, buf);
+
+    sprintf(buf, "%.1f", p3);
+    w = u8g2_GetUTF8Width(&u8g2, buf);
+    u8g2_DrawUTF8(&u8g2, ((42 - w) / 2) + 86,63, buf);
+
+    u8g2_SendBuffer(&u8g2);
+    u8g2_SetPowerSave(&u8g2, 0); // wake up display
+}
+
+
+
+void screen_fatal(char *e1, char *e2)
+{
+    u8g2_SetFont(&u8g2, u8g2_font_t0_15_tr);
+    u8g2_DrawStr(&u8g2,2,12,e1);
+    u8g2_DrawStr(&u8g2,2,24,e2);
+    u8g2_SendBuffer(&u8g2);
+    u8g2_SetPowerSave(&u8g2, 0);
+
+}
+
+
+
+
 void app_main()
 {
     struct timeval	now;
     gettimeofday(&now, NULL);
     int			sleep_time_ms = (now.tv_sec - sleep_enter_time.tv_sec) * 1000 + (now.tv_usec - sleep_enter_time.tv_usec) / 1000;
     int			New_Loop2 = MAIN_LOOP2_INIT;
-    char		buf[65];
     esp_err_t           ret;
 
     Main_Loop1 = MAIN_LOOP1_INIT;
@@ -109,7 +157,6 @@
     u8g2_esp32_hal.scl = PIN_SCL;
     u8g2_esp32_hal_init(u8g2_esp32_hal);
 
-    u8g2_t u8g2; // a structure which will contain all the data for one display
     u8g2_Setup_sh1106_i2c_128x64_noname_f(&u8g2, U8G2_R0, u8g2_esp32_i2c_byte_cb, u8g2_esp32_gpio_and_delay_cb);  // init u8g2 structure
     u8x8_SetI2CAddress(&u8g2.u8x8, 0x78);
     ESP_LOGI(TAG, "u8g2_InitDisplay");
@@ -140,11 +187,7 @@
         } else {
             ESP_LOGE(TAG, "Failed to initialize SPIFFS (%d)", ret);
         }
-	u8g2_SetFont(&u8g2, u8g2_font_t0_15_tr);
-	u8g2_DrawStr(&u8g2,2,12,"SPIFFS:");
-	u8g2_DrawStr(&u8g2,2,24,"init error");
-	u8g2_SendBuffer(&u8g2);
-        u8g2_SetPowerSave(&u8g2, 0);
+	screen_fatal("SPIFFS:", "init error");
         return; // Stop application.
     }
 
@@ -152,11 +195,7 @@
     ret = esp_spiffs_info(NULL, &total, &used);
     if (ret != ESP_OK) {
         ESP_LOGE(TAG, "Failed to get SPIFFS partition information");
-	u8g2_SetFont(&u8g2, u8g2_font_t0_15_tr);
-        u8g2_DrawStr(&u8g2,2,12,"SPIFFS:");
-	u8g2_DrawStr(&u8g2,2,24,"partition error");
-        u8g2_SendBuffer(&u8g2);
-        u8g2_SetPowerSave(&u8g2, 0);
+	screen_fatal("SPIFFS:", "partition error");
         return; // Stop application.
     } else {
         ESP_LOGI(TAG, "Partition size: %d, used: %d - %d%%", total, used, (used * 100) / total);
@@ -239,14 +278,6 @@
 		    requestWiFi_system(true);
 		    request_ds18b20();
 		    request_adc();
-
-                    u8g2_ClearBuffer(&u8g2);
-		    u8g2_DrawHLine(&u8g2, 0, 14, 128);
-                    u8g2_SetFont(&u8g2, u8g2_font_t0_15_tr);
-		    sprintf(buf, "CO2 meter %s", app_desc->version);
-		    u8g2_uint_t w = u8g2_GetStrWidth(&u8g2, buf);
-                    u8g2_DrawStr(&u8g2, (128 - w) / 2,12, buf);
-
 		    break;
 
 		case MAIN_LOOP1_CONNECT:
@@ -264,6 +295,7 @@
 			uint32_t temp = 0;
 			int state = 0;
 			char rom_code[17];
+			float t = 0, p1, p2, p3;
 			if (xSemaphoreTake(xSemaphoreDS18B20, 10) == pdTRUE) {
 			    temp = (ds18b20_state->sensor[0].temperature * 1000);
 			    state = (ds18b20_state->sensor[0].error == 0) ? 0:1;
@@ -271,12 +303,7 @@
 			    rom_code[16] = '\0';
         		    xSemaphoreGive(xSemaphoreDS18B20);
     			}
-
-			u8g2_SetFont(&u8g2, u8g2_font_t0_22b_tf);
-			sprintf(buf, "%.1f °C", temp / 1000.0);
-			u8g2_uint_t w = u8g2_GetUTF8Width(&u8g2, buf);
-			u8g2_DrawUTF8(&u8g2, (128 - w) / 2,40, buf);
-			u8g2_SetFont(&u8g2, u8g2_font_t0_18b_tf);
+			t = temp / 1000.0;
 
 			/* Copy measured data and calculate results */
 			if (xSemaphoreTake(xSemaphoreUnits, 25) == pdTRUE) {
@@ -300,9 +327,12 @@
 				    if (P < 0)
 				    	P = 0;
 				    units[i].pressure = P;
-				    sprintf(buf, "%.1f", P / 1000.0);
-				    w = u8g2_GetUTF8Width(&u8g2, buf);
-				    u8g2_DrawUTF8(&u8g2, ((42 - w) / 2) + i * 43,63, buf);
+				    if (i == 0)
+					p1 = P / 1000.0;
+				    else if (i == 1)
+					p2 = P / 1000.0;
+				    else if (i == 2)
+					p3 = P / 1000.0;
 printf("%d volt: %d batt: %d scale: %d  bar: %d\n", i, units[i].pressure_voltage, adc_state->Batt_voltage, 
 		units[i].pressure_voltage / (adc_state->Batt_voltage / 1000) - units[i].pressure_zero, P);
 // Moet die echt op 5 volt?
@@ -313,8 +343,7 @@
 			    }
 			    write_units();
 			    xSemaphoreGive(xSemaphoreUnits);
-			    u8g2_SendBuffer(&u8g2);
-			    u8g2_SetPowerSave(&u8g2, 0); // wake up display
+			    screen_main(t, p1, p2, p3);
 			}
 		    }
 		    break;