diff --git a/ZPAElectricity.cpp b/ZPAElectricity.cpp
new file mode 100644
index 0000000..e5a2feb
--- /dev/null
+++ b/ZPAElectricity.cpp
@@ -0,0 +1,118 @@
+#include "ZPAElectricity.h"
+
+// BCC is calculated as follows (source https://github.com/lvzon/dsmr-p1-parser/blob/master/doc/IEC-62056-21-notes.md):
+// The BCC is calculated over the bytes after STX up to and including the ETX byte
+// To calculate this BCC, take the first byte XOR 0xff
+// XOR this value with the second byte, and so forth up to and including the last byte (include 0x03 ETX !!),
+// and XOR the final value with 0xff.
+
+// State flags
+bool stateZPAreading = false;
+uint32_t lastZPAcharRead = 0;
+
+uint8_t localBCC;
+String ZPAreadData = "";
+
+bool ZPAreading() {
+  return stateZPAreading;
+}
+
+void ZPArequestData() {
+  Serial2.updateBaudRate(300);
+  Serial.println(F("Requesting data"));
+  Serial2.print(F("/?!\r\n"));
+
+  // Start reading, set up actual time
+  stateZPAreading = true;
+  lastZPAcharRead = millis();
+}
+
+void ZPAchangeBaudrate(char baud) {
+  int baudrate = 300;
+
+  switch (baud) {
+    case '1':
+      baudrate = 300;
+      break;
+
+    case '2':
+      baudrate = 1200;
+      break;
+
+    case '3':
+      baudrate = 2400;
+      break;
+
+    case '4':
+      baudrate = 4800;
+      break;
+  }
+
+  Serial.print(F("Change baudrate to "));
+  Serial.print(baud);
+  Serial.print(F(" ("));
+  Serial.print(baudrate);
+  Serial.println(F(")"));
+
+  Serial2.write(0x06);
+  Serial2.print(F("0"));
+  Serial2.print(baud);
+  Serial2.print(F("0\r\n"));
+  Serial2.flush();
+
+  Serial2.updateBaudRate(baudrate);
+}
+
+void handleZPASerial() {
+  if (!stateZPAreading)
+    return;
+
+  if (Serial2.available()) {
+    char r = Serial2.read();
+    lastZPAcharRead = millis();
+
+    if (r == 0x02) {
+      Serial.println(F("<STX>"));
+      localBCC = 0xFF;
+      ZPAreadData = "";
+      return;
+    }
+
+    if (r == 0x03) {
+      Serial.println(F("<ETX>"));
+      Serial.print(F("Checksum <BCC>: "));
+      uint8_t remoteBCC = Serial2.read();
+      Serial.print(remoteBCC, HEX);
+      Serial.println();
+      Serial.print(F("Calculated <BCC>: "));
+      localBCC = localBCC ^ r;
+      localBCC = localBCC ^ 0xFF;
+      Serial.print(localBCC, HEX);
+      Serial.println();
+      Serial.println();
+      Serial.println(F("Changing back baudrate to 300"));
+      Serial2.updateBaudRate(300);
+
+      stateZPAreading = false;
+      return;
+    }
+
+    // Calculate BCC checksum
+    localBCC = localBCC ^ r;
+
+    ZPAreadData += r;
+    Serial.write(r);
+  }
+}
+
+void handleZPA() {
+  handleZPASerial();
+
+  if (stateZPAreading && (millis() > lastZPAcharRead + ZPA_READ_TIMEOUT)) {
+    Serial.println(F("ZPA Read TIMEOUT, stopping, setting baudrate to 300"));
+
+    stateZPAreading = false;
+    ZPAreadData = "";
+    Serial2.updateBaudRate(300);
+  }
+}
diff --git a/ZPAElectricity.h b/ZPAElectricity.h
new file mode 100644
index 0000000..a453de0
--- /dev/null
+++ b/ZPAElectricity.h
@@ -0,0 +1,15 @@
+#ifndef ZPAELECTRICITY_H
+#define ZPAELECTRICITY_H
+
+#include <Arduino.h>
+
+#define ZPA_READ_TIMEOUT 10000 // 10 seconds timeout to read from ZPA serial line
+
+
+void ZPArequestData();
+bool ZPAreading();
+void ZPAchangeBaudrate(char baud);
+void handleZPASerial();
+void handleZPA();
+
+#endif
diff --git a/ZPA_elektromer_UARTtest.ino b/ZPA_elektromer_UARTtest.ino
index d51d0ee..235ac4d 100644
--- a/ZPA_elektromer_UARTtest.ino
+++ b/ZPA_elektromer_UARTtest.ino
@@ -2,6 +2,8 @@
 #include <WiFiClient.h>
 #include <ArduinoOTA.h>
 
+#include "ZPAElectricity.h"
+
 const char* host = "zpaesp32";
 const char* ssid = "...";
 const char* password = "...";
@@ -77,97 +79,62 @@ void setup() {
   Serial.println("Ready");
   Serial.print("IP address: ");
   Serial.println(WiFi.localIP());
-
 }
 
-// BCC is calculated as follows (source https://github.com/lvzon/dsmr-p1-parser/blob/master/doc/IEC-62056-21-notes.md):
-// The BCC is calculated over the bytes after STX up to and including the ETX byte
-// To calculate this BCC, take the first byte XOR 0xff
-// XOR this value with the second byte, and so forth up to and including the last byte (include 0x03 ETX !!),
-// and XOR the final value with 0xff.
-
-uint8_t localBCC;
-
-void loop() {
-
-  ArduinoOTA.handle();
-
-
-  if (Serial2.available()) {
-    char r = Serial2.read();
-
-    if (r == 0x02) {
-      Serial.print("<STX>");
-      localBCC = 0xFF; // Set checksum to 0xFF (first byte is XORed with 0xFF)
-      return;
-    }
-
-    if (r == 0x03) {
-      Serial.println("<ETX>");
-      Serial.print("Checksum <BCC>: ");
-      Serial.print(Serial2.read());
-      Serial.println();
-      Serial.print("Calculated <BCC>: ");
-      localBCC = localBCC ^ r;    // Include last byte (ETX) to checksum
-      localBCC = localBCC ^ 0xFF; // Final value XORed with 0xFF
-      Serial.print(localBCC);
-      Serial.println();
-      Serial.println();
-      Serial.println("Changing back baudrate to 300");
-      Serial2.updateBaudRate(300);
-      return;
-    }
-
-    // Calculate BCC checksum
-    localBCC = localBCC ^ r;
-    Serial.write(r);
-  }
-
-
+void handlePCSerial() {
   if (Serial.available()) {
     char r = Serial.read();
 
     if (r == 'R') {
-      Serial.println("Requesting data");
-      Serial2.print("/?!\r\n");
+      if (ZPAreading()) {
+        Serial.println(F("Another reading already running"));
+        return;
+      }
+
+      ZPArequestData();
       return;
     }
 
     if (r == '1') {
-      Serial.println("Baudrate to 300");
+      Serial.println(F("Baudrate to 300"));
       Serial2.updateBaudRate(300);
     }
 
     if (r == '2') {
-      Serial.println("Baudrate to 1200");
+      Serial.println(F("Baudrate to 1200"));
       Serial2.updateBaudRate(1200);
     }
 
     if (r == '3') {
-      Serial.println("Baudrate to 2400");
+      Serial.println(F("Baudrate to 2400"));
       Serial2.updateBaudRate(2400);
     }
 
     if (r == '4') {
-      Serial.println("Baudrate to 4800");
+      Serial.println(F("Baudrate to 4800"));
       Serial2.updateBaudRate(4800);
     }
 
     if (r == '5') {
-      Serial.println("Baudrate to 9600");
+      Serial.println(F("Baudrate to 9600"));
       Serial2.updateBaudRate(9600);
     }
 
     if (r == 'C') {
-      Serial.println("Change baudrate");
-      Serial2.write(0x06);
-      Serial2.print("040\r\n");
-      Serial2.flush();
-      Serial2.updateBaudRate(4800);
+      ZPAchangeBaudrate('4');
       return;
     }
 
     Serial2.write(r);
   }
+}
+
+void loop() {
+
+  ArduinoOTA.handle();
+
+  handleZPA();
+
+  handlePCSerial();
 
 }