diff --git a/Software/Software.ino b/Software/Software.ino
index 5d6026e4..b01af79e 100644
--- a/Software/Software.ino
+++ b/Software/Software.ino
@@ -17,6 +17,10 @@
 #include "src/lib/eModbus-eModbus/scripts/mbServerFCs.h"
 #include "src/lib/miwagner-ESP32-Arduino-CAN/CAN_config.h"
 #include "src/lib/miwagner-ESP32-Arduino-CAN/ESP32CAN.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+#include "esp_system.h"
+#include "esp_task_wdt.h"
 
 #ifdef WEBSERVER
 #include <ESPmDNS.h>
@@ -119,6 +123,8 @@ unsigned long timeSpentInFaultedMode = 0;
 bool batteryAllowsContactorClosing = false;
 bool inverterAllowsContactorClosing = true;
 
+TaskHandle_t mainLoopTask;
+
 // Initialization
 void setup() {
   init_serial();
@@ -149,60 +155,71 @@ void setup() {
 
   // BOOT button at runtime is used as an input for various things
   pinMode(0, INPUT_PULLUP);
+
+  esp_task_wdt_deinit();  // Disable watchdog
+
+  xTaskCreatePinnedToCore((TaskFunction_t)&mainLoop, "mainLoop", 4096, NULL, 8, &mainLoopTask, 1);
 }
 
 // Perform main program functions
 void loop() {
+  ;
+}
+
+void mainLoop(void* pvParameters) {
+  while (true) {
 
 #ifdef WEBSERVER
-  // Over-the-air updates by ElegantOTA
-  wifi_monitor();
-  ElegantOTA.loop();
+    // Over-the-air updates by ElegantOTA
+    wifi_monitor();
+    ElegantOTA.loop();
 #ifdef MQTT
-  mqtt_loop();
+    mqtt_loop();
 #endif
 #endif
 
-  // Input
-  receive_can();  // Receive CAN messages. Runs as fast as possible
+    // Input
+    receive_can();  // Receive CAN messages. Runs as fast as possible
 #ifdef DUAL_CAN
-  receive_can2();
+    receive_can2();
 #endif
 #if defined(SERIAL_LINK_RECEIVER) || defined(SERIAL_LINK_TRANSMITTER)
-  runSerialDataLink();
+    runSerialDataLink();
 #endif
 
-  // Process
-  if (millis() - previousMillis10ms >= INTERVAL_10_MS)  // Every 10ms
-  {
-    previousMillis10ms = millis();
-    handle_LED_state();  // Set the LED color according to state
+    // Process
+    if (millis() - previousMillis10ms >= INTERVAL_10_MS)  // Every 10ms
+    {
+      previousMillis10ms = millis();
+      handle_LED_state();  // Set the LED color according to state
 #ifdef CONTACTOR_CONTROL
-    handle_contactors();  // Take care of startup precharge/contactor closing
+      handle_contactors();  // Take care of startup precharge/contactor closing
 #endif
-  }
-
-  if (millis() - previousMillisUpdateVal >= intervalUpdateValues)  // Every 5s normally
-  {
-    previousMillisUpdateVal = millis();
-    update_SOC();     // Check if real or calculated SOC% value should be sent
-    update_values();  // Update values heading towards inverter. Prepare for sending on CAN, or write directly to Modbus.
-    if (DUMMY_EVENT_ENABLED) {
-      set_event(EVENT_DUMMY_ERROR, (uint8_t)millis());
     }
-  }
 
-  // Output
-  send_can();  // Send CAN messages
+    if (millis() - previousMillisUpdateVal >= intervalUpdateValues)  // Every 5s normally
+    {
+      previousMillisUpdateVal = millis();
+      update_SOC();     // Check if real or calculated SOC% value should be sent
+      update_values();  // Update values heading towards inverter. Prepare for sending on CAN, or write directly to Modbus.
+      if (DUMMY_EVENT_ENABLED) {
+        set_event(EVENT_DUMMY_ERROR, (uint8_t)millis());
+      }
+    }
+
+    // Output
+    send_can();  // Send CAN messages
 #ifdef DUAL_CAN
-  send_can2();
+    send_can2();
 #endif
-  run_event_handling();
+    run_event_handling();
 
-  if (digitalRead(0) == HIGH) {
-    test_all_colors = false;
-  } else {
-    test_all_colors = true;
+    if (digitalRead(0) == HIGH) {
+      test_all_colors = false;
+    } else {
+      test_all_colors = true;
+    }
+    delay(2);
   }
 }
 
@@ -347,7 +364,7 @@ void init_modbus() {
   MBserver.registerWorker(MBTCP_ID, WRITE_MULT_REGISTERS, &FC16);
   MBserver.registerWorker(MBTCP_ID, R_W_MULT_REGISTERS, &FC23);
   // Start ModbusRTU background task
-  MBserver.begin(Serial2);
+  MBserver.begin(Serial2, 0);
 #endif
 }