diff --git a/src/main.cpp b/src/main.cpp
index f880482b238bdf2be9599ddad8acbaad5c6260ad..02c27c7fc5bfca893c84dcb244c687b319d5b771 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -1,24 +1,75 @@
 #include <Arduino.h>
 #include "BLEDevice.h"
 #include "Regexp.h"
-#include <SimpleMap.h>
+#include "SimpleMap.h"
+
+static int timerLog = 5;        // seconds
+static int factorMsToPpm = 700; // US: 500, EU: 640, AU: 700 (= device default)
 
 static BLEUUID serviceUUID("0000ffe0-0000-1000-8000-00805f9b34fb");
 static BLEUUID charUUID("0000ffe1-0000-1000-8000-00805f9b34fb");
 
-#define MS_TO_PPM 700 // US: 500, EU: 640, AU: 700 (= device default)
-
 static boolean doConnect = false;
-static boolean connected = false;
 static boolean doScan = true;
 static BLERemoteCharacteristic *pRemoteCharacteristic;
 static BLEAdvertisedDevice *myDevice;
 
 SimpleMap<String, double> *sensorData;
 
-void bleScan();
-void bleConnect();
-void bleParseData(uint8_t *bleData);
+void bleDataCallback(const char *match,
+                     const unsigned int length,
+                     const MatchState &ms)
+{
+  char cap[20];
+  String value = ms.GetCapture(cap, 0);
+  String unit = ms.GetCapture(cap, 1);
+  String temp = ms.GetCapture(cap, 2);
+
+  if (unit != "ppt")
+  {
+    sensorData->put("temperature", temp.toFloat());
+  }
+
+  //Serial.println(unit + " = " + value + " @ " + temp + " °C");
+  if (unit == "pH")
+  {
+    sensorData->put("ph", value.toFloat());
+  }
+  else if (unit == "mS")
+  {
+    sensorData->put("ec_ms", value.toFloat());
+    sensorData->put("ec_us", value.toFloat() * 1000);
+    sensorData->put("ec_ppm", value.toFloat() * factorMsToPpm);
+    sensorData->put("ec_ppt", value.toFloat() * factorMsToPpm / 1000);
+  }
+  else if (unit == "uS")
+  {
+    sensorData->put("ec_ms", value.toFloat() / 1000);
+    sensorData->put("ec_us", value.toFloat());
+    sensorData->put("ec_ppm", value.toFloat() / 1000 * factorMsToPpm);
+    sensorData->put("ec_ppt", value.toFloat() / 1000 * factorMsToPpm / 1000);
+  }
+  else if (unit == "ppm")
+  {
+    sensorData->put("ec_ms", value.toFloat() / factorMsToPpm);
+    sensorData->put("ec_us", value.toFloat() / factorMsToPpm * 1000);
+    sensorData->put("ec_ppm", value.toFloat());
+    sensorData->put("ec_ppt", value.toFloat() / 1000);
+  }
+  else if (unit == "ppt")
+  {
+    sensorData->put("ec_ms", value.toFloat() / factorMsToPpm * 1000);
+    sensorData->put("ec_us", value.toFloat() / factorMsToPpm * 1000 * 1000);
+    sensorData->put("ec_ppm", value.toFloat() * 1000);
+    sensorData->put("ec_ppt", value.toFloat());
+  }
+}
+
+void bleParseData(uint8_t *bleData)
+{
+  MatchState ms((char *)bleData);
+  ms.GlobalMatch("([%d%.?]+)%s+(%a+)%c+([%d%.]+)", bleDataCallback);
+}
 
 static void notifyCallback(
     BLERemoteCharacteristic *pBLERemoteCharacteristic,
@@ -35,15 +86,13 @@ class MyClientCallback : public BLEClientCallbacks
 {
   void onConnect(BLEClient *pclient)
   {
+    doConnect = false;
   }
 
   void onDisconnect(BLEClient *pclient)
   {
-    Serial.println("[BLE] Disconnected, reconnecting...");
-    bleConnect();
-    connected = false;
-    doScan = true;
-    doConnect = false;
+    Serial.println("[BLE] Disconnected");
+    ESP.restart();
   }
 };
 
@@ -58,6 +107,7 @@ bool connectToServer()
   }
   else
   {
+    Serial.println("[BLE] Could not connect");
     return false;
   }
 
@@ -108,43 +158,33 @@ class MyAdvertisedDeviceCallbacks : public BLEAdvertisedDeviceCallbacks
 
 void bleScan()
 {
-  Serial.println("[BLE] Something...");
-  BLEDevice::getScan()->start(0);
+  Serial.println("[BLE] Scanning...");
+  BLEScan *pBLEScan = BLEDevice::getScan();
+  pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks());
+  pBLEScan->setInterval(1349);
+  pBLEScan->setWindow(449);
+  pBLEScan->setActiveScan(true);
+  pBLEScan->start(5, false);
+  Serial.println("[BLE] Scan finished");
 }
 
-void bleConnect()
+void logSensorData()
 {
-  if (!connectToServer())
-  {
-    Serial.println("[BLE] Could not connect");
-    doScan = true;
-  }
-  doConnect = false;
-}
-
-void logSensorData() {
-    Serial.printf("Temp : %.1f °C\r\n", sensorData->get("temperature"));
-    Serial.printf("pH   : %.2f\r\n",    sensorData->get("ph"));
-    Serial.printf("mS/cm: %.2f\r\n",    sensorData->get("ec_ms"));
-    Serial.printf("uS/cm: %.0f\r\n",    sensorData->get("ec_us"));
-    Serial.printf("ppm  : %.0f\r\n",    sensorData->get("ec_ppm"));
-    Serial.printf("ppt  : %.2f\r\n",    sensorData->get("ec_ppt"));
-    Serial.println("---");
+  Serial.printf("Temp : %.1f °C\r\n", sensorData->get("temperature"));
+  Serial.printf("pH   : %.2f\r\n", sensorData->get("ph"));
+  Serial.printf("mS/cm: %.2f\r\n", sensorData->get("ec_ms"));
+  Serial.printf("uS/cm: %.0f\r\n", sensorData->get("ec_us"));
+  Serial.printf("ppm  : %.0f\r\n", sensorData->get("ec_ppm"));
+  Serial.printf("ppt  : %.2f\r\n", sensorData->get("ec_ppt"));
+  Serial.println("---");
 }
 
 void setup()
 {
   Serial.begin(115200);
   Serial.println();
-  Serial.println("[BLE] Setup and initial scan");
   BLEDevice::init("");
-  BLEScan *pBLEScan = BLEDevice::getScan();
-  pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks());
-  pBLEScan->setInterval(1349);
-  pBLEScan->setWindow(449);
-  pBLEScan->setActiveScan(true);
-  pBLEScan->start(5, false);
-
+  bleScan();
   sensorData = new SimpleMap<String, double>([](String &a, String &b) -> int
                                              {
                                                if (a == b)
@@ -158,64 +198,20 @@ void setup()
 void loop()
 {
 
-  if (doConnect)
+  if (doConnect && !connectToServer())
   {
-    bleConnect();
+    ESP.restart();
   }
 
   if (doScan)
   {
-    bleScan();
+    BLEDevice::getScan()->start(0);
   }
 
   static unsigned long logTimer = millis();
-  if (millis() - logTimer >= 1000)
+  if (millis() - logTimer >= timerLog * 1000)
   {
-      logSensorData();
-      logTimer = millis();
+    logSensorData();
+    logTimer = millis();
   }
 }
-
-void bleDataCallback(const char *match,
-                     const unsigned int length,
-                     const MatchState &ms)
-{
-  char cap[20];
-  String value = ms.GetCapture(cap, 0);
-  String unit = ms.GetCapture(cap, 1);
-  String temp = ms.GetCapture(cap, 2);
-  //Serial.println(unit + " = " + value + " @ " + temp + " °C");
-  if (unit == "pH") {
-    sensorData->put("ph",  value.toFloat());
-  } else if (unit == "mS") {
-    sensorData->put("ec_ms",  value.toFloat());
-    sensorData->put("ec_us",  value.toFloat() * 1000);
-    sensorData->put("ec_ppm", value.toFloat() * MS_TO_PPM);
-    sensorData->put("ec_ppt", value.toFloat() * MS_TO_PPM / 1000); 
-  } else if (unit == "uS") {
-    sensorData->put("ec_ms",  value.toFloat() / 1000);
-    sensorData->put("ec_us",  value.toFloat());
-    sensorData->put("ec_ppm", value.toFloat() / 1000 * MS_TO_PPM);
-    sensorData->put("ec_ppt", value.toFloat() / 1000 * MS_TO_PPM  / 1000);
-  } else if (unit == "ppm") {
-    sensorData->put("ec_ms",  value.toFloat() / MS_TO_PPM);
-    sensorData->put("ec_us",  value.toFloat() / MS_TO_PPM * 1000);
-    sensorData->put("ec_ppm", value.toFloat());
-    sensorData->put("ec_ppt", value.toFloat() / 1000);
-  } else if (unit == "ppt") {
-    sensorData->put("ec_ms",  value.toFloat() / MS_TO_PPM * 1000);
-    sensorData->put("ec_us",  value.toFloat() / MS_TO_PPM * 1000 * 1000);
-    sensorData->put("ec_ppm", value.toFloat() * 1000);
-    sensorData->put("ec_ppt", value.toFloat());    
-  }
-  sensorData->put(unit, value.toFloat());
-  if (unit != "ppt") {
-    sensorData->put("temperature", temp.toFloat());
-  }
-}
-
-void bleParseData(uint8_t *bleData)
-{
-  MatchState ms((char *)bleData);
-  ms.GlobalMatch("([%d%.?]+)%s+(%a+)%c+([%d%.]+)", bleDataCallback);
-}
\ No newline at end of file