diff --git a/Board320_240.cpp b/Board320_240.cpp
index bc6c895..3064358 100644
--- a/Board320_240.cpp
+++ b/Board320_240.cpp
@@ -1,7 +1,4 @@
-#ifndef BOARD320_240_CPP
-#define BOARD320_240_CPP
-
-#include <SD.h>
+//#include <SD.h>
 #include <FS.h>
 #include <analogWrite.h>
 #include <TFT_eSPI.h>
@@ -11,6 +8,10 @@
 #include "config.h"
 #include "BoardInterface.h"
 #include "Board320_240.h"
+#include <ArduinoJson.h>
+#include "SIM800L.h"
+
+RTC_DATA_ATTR unsigned int bootCount = 0;
 
 /**
    Init board
@@ -22,8 +23,42 @@ void Board320_240::initBoard() {
   pinMode(pinButtonLeft, INPUT);
   pinMode(pinButtonRight, INPUT);
 
+  // Init time library
+  struct timeval tv;
+  tv.tv_sec = 1589011873;
+  settimeofday(&tv, NULL);
+  struct tm now;
+  getLocalTime(&now, 0);
+  liveData->params.chargingStartTime = liveData->params.currentTime = mktime(&now);
+
+  ++bootCount;
+
+  syslog->print("Boot count: ");
+  syslog->println(bootCount);
+}
+
+/**
+   After setup device
+*/
+void Board320_240::afterSetup() {
+
+  if (digitalRead(pinButtonRight) == LOW) {
+    loadTestData();
+  }
+
+  bool afterSetup = false;
+
+  // Check if bard was sleeping
+  if (bootCount > 1) {
+    // Init comm device
+    afterSetup = true;
+    BoardInterface::afterSetup();
+    // Wake or continue with sleeping
+    afterSleep();
+  }
+
   // Init display
-  Serial.println("Init tft display");
+  syslog->println("Init tft display");
   tft.begin();
   tft.invertDisplay(invertDisplay);
   tft.setRotation(liveData->settings.displayRotation);
@@ -37,12 +72,6 @@ void Board320_240::initBoard() {
 #endif
   spr.setColorDepth((psramUsed) ? 16 : 8);
   spr.createSprite(320, 240);
-}
-
-/**
-   After setup device
-*/
-void Board320_240::afterSetup() {
 
   // Show test data on right button during boot device
   displayScreen = liveData->settings.defaultScreen;
@@ -54,18 +83,24 @@ void Board320_240::afterSetup() {
   // Starting Wifi after BLE prevents reboot loop
   if (liveData->settings.wifiEnabled == 1) {
 
-    /*Serial.print("memReport(): MALLOC_CAP_8BIT | MALLOC_CAP_SPIRAM bytes free. ");
-      Serial.println(heap_caps_get_free_size(MALLOC_CAP_8BIT | MALLOC_CAP_SPIRAM));
-
-      Serial.println("WiFi init...");
+    /*syslog->print("memReport(): MALLOC_CAP_8BIT | MALLOC_CAP_SPIRAM bytes free. ");
+        syslog->println(heap_caps_get_free_size(MALLOC_CAP_8BIT | MALLOC_CAP_SPIRAM));
+        syslog->println("WiFi init...");
       WiFi.enableSTA(true);
       WiFi.mode(WIFI_STA);
       WiFi.begin(liveData->settings.wifiSsid, liveData->settings.wifiPassword);
-      Serial.println("WiFi init completed...");*/
+        syslog->println("WiFi init completed...");*/
   }
 
   // Init GPS
   if (liveData->settings.gpsHwSerialPort <= 2) {
+    syslog->print("GPS initialization on hwUart: ");
+    syslog->println(liveData->settings.gpsHwSerialPort);
+    if (liveData->settings.gpsHwSerialPort == 0) {
+      syslog->println("hwUart0 collision with serial console! Disabling serial console");
+      syslog->flush();
+      syslog->end();
+    }
     gpsHwUart = new HardwareSerial(liveData->settings.gpsHwSerialPort);
     gpsHwUart->begin(9600);
   }
@@ -73,11 +108,101 @@ void Board320_240::afterSetup() {
   // SD card
   if (liveData->settings.sdcardEnabled == 1) {
     if (sdcardMount() && liveData->settings.sdcardAutstartLog == 1) {
+      syslog->println("Toggle recording on SD card");
       sdcardToggleRecording();
     }
   }
 
-  BoardInterface::afterSetup();
+  // Init SIM800L
+  if (liveData->settings.gprsHwSerialPort <= 2) {
+    sim800lSetup();
+  }
+
+  // Init comm device
+  if (!afterSetup) {
+    BoardInterface::afterSetup();
+  }
+}
+
+/**
+   Go to Sleep for TIME_TO_SLEEP seconds
+*/
+void Board320_240::goToSleep() {
+
+  //Sleep MCP2515
+  commInterface->disconnectDevice();
+
+  //Sleep SIM800L
+  if (liveData->params.sim800l_enabled) {
+    if (sim800l->isConnectedGPRS()) {
+      bool disconnected = sim800l->disconnectGPRS();
+      for (uint8_t i = 0; i < 5 && !disconnected; i++) {
+        delay(1000);
+        disconnected = sim800l->disconnectGPRS();
+      }
+    }
+
+    if (sim800l->getPowerMode() == NORMAL) {
+      sim800l->setPowerMode(SLEEP);
+      delay(1000);
+    }
+    sim800l->enterSleepMode();
+  }
+
+  syslog->println("Going to sleep for " + String(TIME_TO_SLEEP) + " seconds!");
+  syslog->flush();
+
+  delay(1000);
+
+  //Sleep ESP32
+  esp_sleep_enable_timer_wakeup(TIME_TO_SLEEP * 1000000ULL);
+  esp_deep_sleep_start();
+}
+
+/*
+  Wake up board from sleep
+  Iterate thru commands and determine if car is charging or ignition is on
+*/
+void Board320_240::afterSleep() {
+
+  syslog->println("Waking up from sleep mode!");
+
+  // Wakeup reason
+  esp_sleep_wakeup_cause_t wakeup_reason;
+  wakeup_reason = esp_sleep_get_wakeup_cause();
+  switch (wakeup_reason) {
+    case ESP_SLEEP_WAKEUP_EXT0 : syslog->println("Wakeup caused by external signal using RTC_IO"); break;
+    case ESP_SLEEP_WAKEUP_EXT1 : syslog->println("Wakeup caused by external signal using RTC_CNTL"); break;
+    case ESP_SLEEP_WAKEUP_TIMER : syslog->println("Wakeup caused by timer"); break;
+    case ESP_SLEEP_WAKEUP_TOUCHPAD : syslog->println("Wakeup caused by touchpad"); break;
+    case ESP_SLEEP_WAKEUP_ULP : syslog->println("Wakeup caused by ULP program"); break;
+    default: syslog->printf("Wakeup was not caused by deep sleep: %d\n", wakeup_reason); break;
+  }
+
+  //
+  bool firstRun = true;
+  while (liveData->commandQueueIndex - 1 > liveData->commandQueueLoopFrom || firstRun) {
+    if (liveData->commandQueueIndex - 1 == liveData->commandQueueLoopFrom) {
+      firstRun = false;
+    }
+
+    if (millis() > 5000) {
+      syslog->println("Time's up (5s timeout)...");
+      goToSleep();
+    }
+
+    commInterface->mainLoop();
+  }
+
+  if (liveData->params.auxVoltage > 5 && liveData->params.auxVoltage < 12) {
+    syslog->println("AuxBATT too low!");
+    goToSleep();
+  } else if (!liveData->params.ignitionOn && !liveData->params.chargingOn) {
+    syslog->println("Not started & Not charging.");
+    goToSleep();
+  } else {
+    syslog->println("Wake up conditions satisfied... Good morning!");
+  }
 }
 
 /**
@@ -273,9 +398,14 @@ void Board320_240::drawSceneMain() {
   sprintf(tmpStr1, ((liveData->settings.temperatureUnit == 'c') ? "%01.00f" : "%01.01f"), liveData->celsius2temperature(liveData->params.batHeaterC));
   drawBigCell(2, 3, 1, 1, tmpStr1, "BAT.HEAT", TFT_TEMP, TFT_WHITE);
 
-  // Aux perc
-  sprintf(tmpStr1, "%01.00f%%", liveData->params.auxPerc);
-  drawBigCell(3, 0, 1, 1, tmpStr1, "AUX BAT.", (liveData->params.auxPerc < 60 ? TFT_RED : TFT_DEFAULT_BK), TFT_WHITE);
+  // Aux perc / temp
+  if (liveData->settings.carType == CAR_BMW_I3_2014) { // TODO: use invalid auxPerc value as decision point here?
+    sprintf(tmpStr1, "%01.00f", liveData->params.auxTemperature);
+    drawBigCell(3, 0, 1, 1, tmpStr1, "AUX TEMP.", (liveData->params.auxTemperature < 5 ? TFT_RED : TFT_DEFAULT_BK), TFT_WHITE);
+  } else {
+    sprintf(tmpStr1, "%01.00f%%", liveData->params.auxPerc);
+    drawBigCell(3, 0, 1, 1, tmpStr1, "AUX BAT.", (liveData->params.auxPerc < 60 ? TFT_RED : TFT_DEFAULT_BK), TFT_WHITE);
+  }
 
   // Aux amp
   sprintf(tmpStr1, (abs(liveData->params.auxCurrentAmp) > 9.9 ? "%01.00f" : "%01.01f"), liveData->params.auxCurrentAmp);
@@ -814,41 +944,6 @@ void Board320_240::drawSceneSoc10Table() {
   spr.drawString(tmpStr1, 310, zeroY + (14 * 15), 2);
 }
 
-/**
-  DEBUG screen
-*/
-void Board320_240::drawSceneDebug() {
-
-  int32_t posx, posy;
-  String chHex, chHex2;
-  uint8_t chByte;
-
-  spr.setTextSize(1); // Size for small 5x7 font
-  spr.setTextColor(TFT_SILVER, TFT_TEMP);
-  spr.setTextDatum(TL_DATUM);
-  spr.drawString(debugAtshRequest, 0, 0, 2);
-  spr.drawString(debugCommandRequest, 128, 0, 2);
-  spr.drawString(liveData->commandRequest, 256, 0, 2);
-  spr.setTextDatum(TR_DATUM);
-
-  for (int i = 0; i < debugLastString.length() / 2; i++) {
-    chHex = debugLastString.substring(i * 2, (i * 2) + 2);
-    chHex2 = debugPreviousString.substring(i * 2, (i * 2) + 2);
-    spr.setTextColor(((chHex.equals(chHex2)) ?  TFT_SILVER : TFT_GREEN), TFT_TEMP);
-    chByte = liveData->hexToDec(chHex.c_str(), 1, false);
-    posx = (((i) % 10) * 32) + 24;
-    posy = ((floor((i) / 10)) * 32) + 24;
-    sprintf(tmpStr1, "%03d", chByte);
-    spr.drawString(tmpStr1, posx + 4, posy, 2);
-
-    spr.setTextColor(TFT_YELLOW, TFT_TEMP);
-    sprintf(tmpStr1, "%c", (char)chByte);
-    spr.drawString(tmpStr1, posx + 4, posy + 13, 2);
-  }
-
-  debugPreviousString = debugLastString;
-}
-
 /**
    Modify caption
 */
@@ -869,8 +964,12 @@ String Board320_240::menuItemCaption(int16_t menuItemId, String title) {
     case 105: prefix = (liveData->settings.carType == CAR_HYUNDAI_KONA_2020_39) ? ">" : ""; break;
     case 106: prefix = (liveData->settings.carType == CAR_RENAULT_ZOE) ? ">" : ""; break;
     case 107: prefix = (liveData->settings.carType == CAR_KIA_NIRO_PHEV) ? ">" : ""; break;
+    case 108: prefix = (liveData->settings.carType == CAR_BMW_I3_2014) ? ">" : ""; break;
     case 120: prefix = (liveData->settings.carType == CAR_DEBUG_OBD2_KIA) ? ">" : ""; break;
     //
+    case MENU_ADAPTER_BLE4: prefix = (liveData->settings.commType == COMM_TYPE_OBD2BLE4) ? ">" : ""; break;
+    case MENU_ADAPTER_CAN:  prefix = (liveData->settings.commType == COMM_TYPE_OBD2CAN) ? ">" : ""; break;
+    case MENU_ADAPTER_BT3:  prefix = (liveData->settings.commType == COMM_TYPE_OBD2BT3) ? ">" : ""; break;
     /*case MENU_WIFI:
       suffix = "n/a";
       switch (WiFi.status()) {
@@ -883,14 +982,23 @@ String Board320_240::menuItemCaption(int16_t menuItemId, String title) {
       WL_DISCONNECTED: suffix = "DISCONNECTED"; break;
       }
       break;*/
-    case MENU_GPRS:             sprintf(tmpStr1, "[%s] %s", (liveData->settings.gprsEnabled == 1) ? "on" : "off", liveData->settings.gprsApn); suffix = tmpStr1; break;
+    case MENU_GPRS:             sprintf(tmpStr1, "[HW UART=%d]", liveData->settings.gprsHwSerialPort);  suffix = (liveData->settings.gprsHwSerialPort == 255) ? "[off]" : tmpStr1; break;
     case MENU_SDCARD:           sprintf(tmpStr1, "[%d] %lluMB", SD.cardType(), SD.cardSize() / (1024 * 1024)); suffix = tmpStr1; break;
+    case MENU_SERIAL_CONSOLE:   suffix = (liveData->settings.serialConsolePort == 255) ? "[off]" : "[on]"; break;
+    case MENU_DEBUG_LEVEL:      switch (liveData->settings.debugLevel) {
+        case 0: suffix = "[all]" ; break;
+        case 1: suffix = "[comm]" ; break;
+        case 2: suffix = "[gsm]" ; break;
+        case 3: suffix = "[sdcard]" ; break;
+        default: suffix = "[unknown]";
+      }
+      break;
     case MENU_SCREEN_ROTATION:  suffix = (liveData->settings.displayRotation == 1) ? "[vertical]" : "[normal]"; break;
     case MENU_DEFAULT_SCREEN:   sprintf(tmpStr1, "[%d]", liveData->settings.defaultScreen); suffix = tmpStr1; break;
     case MENU_SCREEN_BRIGHTNESS: sprintf(tmpStr1, "[%d%%]", liveData->settings.lcdBrightness); suffix = (liveData->settings.lcdBrightness == 0) ? "[auto]" : tmpStr1; break;
     case MENU_PREDRAWN_GRAPHS:  suffix = (liveData->settings.predrawnChargingGraphs == 1) ? "[on]" : "[off]"; break;
     case MENU_HEADLIGHTS_REMINDER: suffix = (liveData->settings.headlightsReminder == 1) ? "[on]" : "[off]"; break;
-    case MENU_DEBUG_SCREEN:     suffix = (liveData->settings.debugScreen == 1) ? "[on]" : "[off]"; break;
+    case MENU_SLEEP_MODE:     suffix = (liveData->settings.sleepModeEnabled == 1) ? "[on]" : "[off]"; break;
     case MENU_GPS:              sprintf(tmpStr1, "[HW UART=%d]", liveData->settings.gpsHwSerialPort);  suffix = (liveData->settings.gpsHwSerialPort == 255) ? "[off]" : tmpStr1; break;
     //
     case MENU_SDCARD_ENABLED:   sprintf(tmpStr1, "[%s]", (liveData->settings.sdcardEnabled == 1) ? "on" : "off"); suffix = tmpStr1; break;
@@ -899,6 +1007,7 @@ String Board320_240::menuItemCaption(int16_t menuItemId, String title) {
                                              (strlen(liveData->params.sdcardFilename) != 0) ? liveData->params.sdcardFilename :
                                              (liveData->params.sdcardInit) ? "READY" : "MOUNT"); suffix = tmpStr1; break;
     case MENU_SDCARD_REC:       sprintf(tmpStr1, "[%s]", (liveData->settings.sdcardEnabled == 0) ? "n/a" : (liveData->params.sdcardRecording) ? "STOP" : "START"); suffix = tmpStr1; break;
+    case MENU_SDCARD_INTERVAL:   sprintf(tmpStr1, "[%d]", liveData->settings.sdcardLogIntervalSec); suffix = tmpStr1; break;
     //
     case MENU_WIFI_ENABLED:     suffix = (liveData->settings.wifiEnabled == 1) ? "[on]" : "[off]"; break;
     case MENU_WIFI_SSID:        sprintf(tmpStr1, "%s", liveData->settings.wifiSsid); suffix = tmpStr1; break;
@@ -1005,12 +1114,12 @@ void Board320_240::menuItemClick() {
   // Exit menu, parent level menu, open item
   bool showParentMenu = false;
   if (liveData->menuItemSelected > 0) {
-    Serial.println(tmpMenuItem->id);
+    syslog->println(tmpMenuItem->id);
     // Device list
     if (tmpMenuItem->id > 10000 && tmpMenuItem->id < 10100) {
       strlcpy((char*)liveData->settings.obdMacAddress, (char*)tmpMenuItem->obdMacAddress, 20);
-      Serial.print("Selected adapter MAC address ");
-      Serial.println(liveData->settings.obdMacAddress);
+      syslog->print("Selected adapter MAC address ");
+      syslog->println(liveData->settings.obdMacAddress);
       saveSettings();
       ESP.restart();
     }
@@ -1024,7 +1133,12 @@ void Board320_240::menuItemClick() {
       case 105: liveData->settings.carType = CAR_HYUNDAI_KONA_2020_39; showMenu(); return; break;
       case 106: liveData->settings.carType = CAR_RENAULT_ZOE; showMenu(); return; break;
       case 107: liveData->settings.carType = CAR_KIA_NIRO_PHEV; showMenu(); return; break;
+      case 108: liveData->settings.carType = CAR_BMW_I3_2014; showMenu(); return; break;
       case 120: liveData->settings.carType = CAR_DEBUG_OBD2_KIA; showMenu(); return; break;
+      // Comm type
+      case MENU_ADAPTER_BLE4: liveData->settings.commType = COMM_TYPE_OBD2BLE4; showMenu(); return; break;
+      case MENU_ADAPTER_CAN: liveData->settings.commType = COMM_TYPE_OBD2CAN; showMenu(); return; break;
+      case MENU_ADAPTER_BT3: liveData->settings.commType = COMM_TYPE_OBD2BT3; showMenu(); return; break;
       // Screen orientation
       case MENU_SCREEN_ROTATION: liveData->settings.displayRotation = (liveData->settings.displayRotation == 1) ? 3 : 1; tft.setRotation(liveData->settings.displayRotation); showMenu(); return; break;
       // Default screen
@@ -1033,23 +1147,26 @@ void Board320_240::menuItemClick() {
       case 3063: liveData->settings.defaultScreen = 3; showParentMenu = true; break;
       case 3064: liveData->settings.defaultScreen = 4; showParentMenu = true; break;
       case 3065: liveData->settings.defaultScreen = 5; showParentMenu = true; break;
-      // Debug screen off/on
-      case MENU_DEBUG_SCREEN: liveData->settings.debugScreen = (liveData->settings.debugScreen == 1) ? 0 : 1; showMenu(); return; break;
-      case MENU_SCREEN_BRIGHTNESS: liveData->settings.lcdBrightness += 20; if (liveData->settings.lcdBrightness > 100) liveData->settings.lcdBrightness = 0;
+      // SleepMode off/on
+      case MENU_SLEEP_MODE:           liveData->settings.sleepModeEnabled = (liveData->settings.sleepModeEnabled == 1) ? 0 : 1; showMenu(); return; break;
+      case MENU_SCREEN_BRIGHTNESS:    liveData->settings.lcdBrightness += 20; if (liveData->settings.lcdBrightness > 100) liveData->settings.lcdBrightness = 0;
         setBrightness((liveData->settings.lcdBrightness == 0) ? 100 : liveData->settings.lcdBrightness); showMenu(); return; break;
       // Pre-drawn charg.graphs off/on
-      case MENU_PREDRAWN_GRAPHS: liveData->settings.predrawnChargingGraphs = (liveData->settings.predrawnChargingGraphs == 1) ? 0 : 1; showMenu(); return; break;
-      case MENU_HEADLIGHTS_REMINDER: liveData->settings.headlightsReminder = (liveData->settings.headlightsReminder == 1) ? 0 : 1; showMenu(); return; break;
-      case MENU_GPS: liveData->settings.gpsHwSerialPort = (liveData->settings.gpsHwSerialPort == 2) ? 255 : liveData->settings.gpsHwSerialPort + 1; showMenu(); return; break;
+      case MENU_PREDRAWN_GRAPHS:      liveData->settings.predrawnChargingGraphs = (liveData->settings.predrawnChargingGraphs == 1) ? 0 : 1; showMenu(); return; break;
+      case MENU_HEADLIGHTS_REMINDER:  liveData->settings.headlightsReminder = (liveData->settings.headlightsReminder == 1) ? 0 : 1; showMenu(); return; break;
+      case MENU_GPRS:                 liveData->settings.gprsHwSerialPort = (liveData->settings.gprsHwSerialPort == 2) ? 255 : liveData->settings.gprsHwSerialPort + 1; showMenu(); return; break;
+      case MENU_GPS:                  liveData->settings.gpsHwSerialPort = (liveData->settings.gpsHwSerialPort == 2) ? 255 : liveData->settings.gpsHwSerialPort + 1; showMenu(); return; break;
+      case MENU_SERIAL_CONSOLE:       liveData->settings.serialConsolePort = (liveData->settings.serialConsolePort == 0) ? 255 : liveData->settings.serialConsolePort + 1; showMenu(); return; break;
+      case MENU_DEBUG_LEVEL:          liveData->settings.debugLevel = (liveData->settings.debugLevel == 3) ? 0 : liveData->settings.debugLevel + 1; syslog->setDebugLevel(liveData->settings.debugLevel); showMenu(); return; break;
       // Wifi menu
-      case MENU_WIFI_ENABLED: liveData->settings.wifiEnabled = (liveData->settings.wifiEnabled == 1) ? 0 : 1; showMenu(); return; break;
-      case MENU_WIFI_SSID: return; break;
-      case MENU_WIFI_PASSWORD: return; break;
+      case MENU_WIFI_ENABLED:         liveData->settings.wifiEnabled = (liveData->settings.wifiEnabled == 1) ? 0 : 1; showMenu(); return; break;
+      case MENU_WIFI_SSID:            return; break;
+      case MENU_WIFI_PASSWORD:        return; break;
       // Sdcard
-      case MENU_SDCARD_ENABLED:   liveData->settings.sdcardEnabled = (liveData->settings.sdcardEnabled == 1) ? 0 : 1; showMenu(); return; break;
+      case MENU_SDCARD_ENABLED:       liveData->settings.sdcardEnabled = (liveData->settings.sdcardEnabled == 1) ? 0 : 1; showMenu(); return; break;
       case MENU_SDCARD_AUTOSTARTLOG:  liveData->settings.sdcardAutstartLog = (liveData->settings.sdcardAutstartLog == 1) ? 0 : 1; showMenu(); return; break;
       case MENU_SDCARD_MOUNT_STATUS:  sdcardMount(); break;
-      case MENU_SDCARD_REC:      sdcardToggleRecording(); showMenu(); return; break;
+      case MENU_SDCARD_REC:           sdcardToggleRecording(); showMenu(); return; break;
       // Distance
       case 4011: liveData->settings.distanceUnit = 'k'; showParentMenu = true; break;
       case 4012: liveData->settings.distanceUnit = 'm'; showParentMenu = true; break;
@@ -1060,13 +1177,40 @@ void Board320_240::menuItemClick() {
       case 4031: liveData->settings.pressureUnit = 'b'; showParentMenu = true; break;
       case 4032: liveData->settings.pressureUnit = 'p'; showParentMenu = true; break;
       // Pair ble device
-      case 2: scanDevices = true; /*startBleScan(); */return;
+      case 2:
+        if (liveData->settings.commType == COMM_TYPE_OBD2CAN) {
+          displayMessage("Not supported", "in CAN mode");
+          delay(3000);
+          hideMenu();
+          return;
+        }
+        scanDevices = true;
+        liveData->menuCurrent = 9999;
+        commInterface->scanDevices();
+        return;
       // Reset settings
       case 8: resetSettings(); hideMenu(); return;
       // Save settings
       case 9: saveSettings(); break;
       // Version
-      case 10: hideMenu(); return;
+      case 10:
+        /*  commInterface->executeCommand("ATSH770");
+          delay(50);
+          commInterface->executeCommand("3E");
+          delay(50);
+          commInterface->executeCommand("1003");
+          delay(50);
+          commInterface->executeCommand("2FBC1003");
+          delay(5000);
+          commInterface->executeCommand("ATSH770");
+          delay(50);
+          commInterface->executeCommand("3E");
+          delay(50);
+          commInterface->executeCommand("1003");
+          delay(50);
+          commInterface->executeCommand("2FBC1103");
+          delay(5000);*/
+        hideMenu(); return;
       // Shutdown
       case 11: shutdownDevice(); return;
       default:
@@ -1094,7 +1238,7 @@ void Board320_240::menuItemClick() {
         }
       }
       liveData->menuCurrent = parentMenu;
-      Serial.println(liveData->menuCurrent);
+      syslog->println(liveData->menuCurrent);
       showMenu();
     }
     return;
@@ -1146,7 +1290,7 @@ void Board320_240::redrawScreen() {
       drawSceneMain();
     }
   } else {
-     displayScreenAutoMode = SCREEN_DASH;
+    displayScreenAutoMode = SCREEN_DASH;
   }
   // 2. Main screen
   if (displayScreen == SCREEN_DASH) {
@@ -1168,18 +1312,14 @@ void Board320_240::redrawScreen() {
   if (displayScreen == SCREEN_SOC10) {
     drawSceneSoc10Table();
   }
-  // 7. DEBUG SCREEN
-  if (displayScreen == SCREEN_DEBUG) {
-    drawSceneDebug();
-  }
 
   if (!displayScreenSpeedHud) {
 
     // SDCARD recording
     /*liveData->params.sdcardRecording*/
-    if (liveData->settings.sdcardEnabled == 1) {
-      spr.fillCircle((displayScreen == SCREEN_SPEED || displayScreenAutoMode == SCREEN_SPEED) ? 160 : 310, 10, 4, TFT_BLACK);
-      spr.fillCircle((displayScreen == SCREEN_SPEED || displayScreenAutoMode == SCREEN_SPEED) ? 160 : 310, 10, 3,
+    if (liveData->settings.sdcardEnabled == 1 && (liveData->params.mainLoopCounter & 1) == 1) {
+      spr.fillCircle((displayScreen == SCREEN_SPEED || displayScreenAutoMode == SCREEN_SPEED) ? 140 : 310, 10, 4, TFT_BLACK);
+      spr.fillCircle((displayScreen == SCREEN_SPEED || displayScreenAutoMode == SCREEN_SPEED) ? 140 : 310, 10, 3,
                      (liveData->params.sdcardInit == 1) ?
                      (liveData->params.sdcardRecording) ?
                      (strlen(liveData->params.sdcardFilename) != 0) ?
@@ -1189,18 +1329,32 @@ void Board320_240::redrawScreen() {
                      TFT_YELLOW /* failed to initialize sdcard */
                     );
     }
+    // GPS state
     if (gpsHwUart != NULL && (displayScreen == SCREEN_SPEED || displayScreenAutoMode == SCREEN_SPEED)) {
-      spr.drawCircle(180, 10, 5, (gps.location.isValid()) ? TFT_GREEN : TFT_RED);
+      spr.drawCircle(160, 10, 5, (gps.location.isValid()) ? TFT_GREEN : TFT_RED);
       spr.setTextSize(1);
       spr.setTextColor((gps.location.isValid()) ? TFT_GREEN : TFT_WHITE, TFT_BLACK);
       spr.setTextDatum(TL_DATUM);
       sprintf(tmpStr1, "%d", liveData->params.gpsSat);
-      spr.drawString(tmpStr1, 194, 2, 2);
-      
+      spr.drawString(tmpStr1, 174, 2, 2);
     }
 
+    // Door status
+    if (liveData->params.trunkDoorOpen)
+      spr.fillRect(20, 0, 320 - 40, 20, TFT_YELLOW);
+    if (liveData->params.leftFrontDoorOpen)
+      spr.fillRect(0, 20, 20, 98, TFT_YELLOW);
+    if (liveData->params.rightFrontDoorOpen)
+      spr.fillRect(0, 122, 20, 98, TFT_YELLOW);
+    if (liveData->params.leftRearDoorOpen)
+      spr.fillRect(320 - 20, 20, 20, 98, TFT_YELLOW);
+    if (liveData->params.rightRearDoorOpen)
+      spr.fillRect(320 - 20, 122, 20, 98, TFT_YELLOW);
+    if (liveData->params.hoodDoorOpen)
+      spr.fillRect(20, 240 - 20, 320 - 40, 20, TFT_YELLOW);
+
     // BLE not connected
-    if (!liveData->bleConnected && liveData->bleConnect) {
+    if (!liveData->commConnected && liveData->bleConnect && liveData->tmpSettings.commType == COMM_TYPE_OBD2BLE4) {
       // Print message
       spr.setTextSize(1);
       spr.setTextColor(TFT_WHITE, TFT_BLACK);
@@ -1209,7 +1363,7 @@ void Board320_240::redrawScreen() {
       spr.drawString("Press middle button to menu.", 0, 200, 2);
       spr.drawString(APP_VERSION, 0, 220, 2);
     }
-    
+
     spr.pushSprite(0, 0);
   }
 }
@@ -1219,6 +1373,8 @@ void Board320_240::redrawScreen() {
 */
 void Board320_240::loadTestData() {
 
+  syslog->println("Loading test data");
+
   testDataMode = true; // skip lights off message
   carInterface->loadTestData();
   redrawScreen();
@@ -1229,6 +1385,8 @@ void Board320_240::loadTestData() {
 */
 void Board320_240::mainLoop() {
 
+  liveData->params.mainLoopCounter++;
+
   ///////////////////////////////////////////////////////////////////////
   // Handle buttons
   // MIDDLE - menu select
@@ -1257,7 +1415,7 @@ void Board320_240::mainLoop() {
         menuMove(false);
       } else {
         displayScreen++;
-        if (displayScreen > displayScreenCount - (liveData->settings.debugScreen == 0) ? 1 : 0)
+        if (displayScreen > displayScreenCount - 1)
           displayScreen = 0; // rotate screens
         // Turn off display on screen 0
         setBrightness((displayScreen == SCREEN_BLANK) ? 0 : (liveData->settings.lcdBrightness == 0) ? 100 : liveData->settings.lcdBrightness);
@@ -1281,11 +1439,6 @@ void Board320_240::mainLoop() {
           displayScreenSpeedHud = !displayScreenSpeedHud;
           redrawScreen();
         }
-        if (liveData->settings.debugScreen == 1 && displayScreen == SCREEN_DEBUG) {
-          debugCommandIndex = (debugCommandIndex >= liveData->commandQueueCount) ? liveData->commandQueueLoopFrom : debugCommandIndex + 1;
-          redrawScreen();
-        }
-
       }
     }
   }
@@ -1305,6 +1458,12 @@ void Board320_240::mainLoop() {
     syncGPS();
   }
 
+  // SIM800L
+  if (liveData->params.lastDataSent + SIM800L_TIMER < liveData->params.currentTime && liveData->params.sim800l_enabled) {
+    sendDataViaGPRS();
+    liveData->params.lastDataSent = liveData->params.currentTime;
+  }
+
   // currentTime
   struct tm now;
   getLocalTime(&now, 0);
@@ -1312,20 +1471,20 @@ void Board320_240::mainLoop() {
 
   // SD card recording
   if (liveData->params.sdcardInit && liveData->params.sdcardRecording && liveData->params.sdcardCanNotify &&
-    (liveData->params.odoKm != -1 && liveData->params.socPerc != -1)) {
+      (liveData->params.odoKm != -1 && liveData->params.socPerc != -1)) {
+
+    //syslog->println(&now, "%y%m%d%H%M");
 
-    //Serial.println(&now, "%y%m%d%H%M");
-      
     // create filename
     if (liveData->params.operationTimeSec > 0 && strlen(liveData->params.sdcardFilename) == 0) {
       sprintf(liveData->params.sdcardFilename, "/%llu.json", uint64_t(liveData->params.operationTimeSec / 60));
-      Serial.print("Log filename by opTimeSec: ");
-      Serial.println(liveData->params.sdcardFilename);
+      syslog->print("Log filename by opTimeSec: ");
+      syslog->println(liveData->params.sdcardFilename);
     }
     if (liveData->params.currTimeSyncWithGps && strlen(liveData->params.sdcardFilename) < 15) {
       strftime(liveData->params.sdcardFilename, sizeof(liveData->params.sdcardFilename), "/%y%m%d%H%M.json", &now);
-      Serial.print("Log filename by GPS: ");
-      Serial.println(liveData->params.sdcardFilename);
+      syslog->print("Log filename by GPS: ");
+      syslog->println(liveData->params.sdcardFilename);
     }
 
     // append buffer, clear buffer & notify state
@@ -1333,14 +1492,14 @@ void Board320_240::mainLoop() {
       liveData->params.sdcardCanNotify = false;
       File file = SD.open(liveData->params.sdcardFilename, FILE_APPEND);
       if (!file) {
-        Serial.println("Failed to open file for appending");
+        syslog->println("Failed to open file for appending");
         File file = SD.open(liveData->params.sdcardFilename, FILE_WRITE);
       }
       if (!file) {
-        Serial.println("Failed to create file");
+        syslog->println("Failed to create file");
       }
       if (file) {
-        Serial.println("Save buffer to SD card");
+        syslog->println("Save buffer to SD card");
         serializeParamsToJson(file);
         file.print(",\n");
         file.close();
@@ -1348,9 +1507,24 @@ void Board320_240::mainLoop() {
     }
   }
 
-  // Shutdown when car is off
-  if (liveData->params.automaticShutdownTimer != 0 && liveData->params.currentTime - liveData->params.automaticShutdownTimer > 5)
-    shutdownDevice();  
+  // Turn off display if Ignition is off for more than 10s, less than month (prevent sleep when gps time is synchronized)
+  if (liveData->params.currentTime - liveData->params.lastIgnitionOnTime > 10 && liveData->params.currentTime - liveData->params.lastIgnitionOnTime < MONTH_SEC
+      && liveData->params.lastIgnitionOnTime != 0
+      && liveData->settings.sleepModeEnabled) {
+    setBrightness(0);
+  } else {
+    setBrightness((liveData->settings.lcdBrightness == 0) ? 100 : liveData->settings.lcdBrightness);
+  }
+
+  // Go to sleep when car is off for more than 30s and not charging (AC charger is disabled for few seconds when ignition is turned off)
+  if (liveData->params.currentTime - liveData->params.lastIgnitionOnTime > 30 && liveData->params.currentTime - liveData->params.lastIgnitionOnTime < MONTH_SEC
+      && !liveData->params.chargingOn
+      && liveData->params.lastIgnitionOnTime != 0
+      && liveData->settings.sleepModeEnabled)
+    goToSleep();
+
+  // Read data from BLE/CAN
+  commInterface->mainLoop();
 }
 
 /**
@@ -1366,43 +1540,53 @@ bool Board320_240::skipAdapterScan() {
 bool Board320_240::sdcardMount() {
 
   if (liveData->params.sdcardInit) {
-    Serial.print("SD card already mounted...");
+    syslog->print("SD card already mounted...");
     return true;
   }
 
   int8_t countdown = 3;
-  while (1) {
-    Serial.print("Initializing SD card...");
+  bool SdState = false;
 
-    if (SD.begin(pinSdcardCs)) {
+  while (1) {
+    syslog->print("Initializing SD card...");
+
+    /*    syslog->print(" TTGO-T4 ");
+        SPIClass * hspi = new SPIClass(HSPI);
+        spiSD.begin(pinSdcardSclk, pinSdcardMiso, pinSdcardMosi, pinSdcardCs); //SCK,MISO,MOSI,ss
+        SdState = SD.begin(pinSdcardCs, *hspi, SPI_FREQUENCY);*/
+
+    syslog->print(" M5STACK ");
+    SdState = SD.begin(pinSdcardCs);
+
+    if (SdState) {
 
       uint8_t cardType = SD.cardType();
       if (cardType == CARD_NONE) {
-        Serial.println("No SD card attached");
+        syslog->println("No SD card attached");
         return false;
       }
 
-      Serial.println("SD card found.");
+      syslog->println("SD card found.");
       liveData->params.sdcardInit = true;
 
-      Serial.print("SD Card Type: ");
+      syslog->print("SD Card Type: ");
       if (cardType == CARD_MMC) {
-        Serial.println("MMC");
+        syslog->println("MMC");
       } else if (cardType == CARD_SD) {
-        Serial.println("SDSC");
+        syslog->println("SDSC");
       } else if (cardType == CARD_SDHC) {
-        Serial.println("SDHC");
+        syslog->println("SDHC");
       } else {
-        Serial.println("UNKNOWN");
+        syslog->println("UNKNOWN");
       }
 
       uint64_t cardSize = SD.cardSize() / (1024 * 1024);
-      Serial.printf("SD Card Size: %lluMB\n", cardSize);
+      syslog->printf("SD Card Size: %lluMB\n", cardSize);
 
       return true;
     }
 
-    Serial.println("Initialization failed!");
+    syslog->println("Initialization failed!");
     countdown--;
     if (countdown <= 0) {
       break;
@@ -1421,7 +1605,7 @@ void Board320_240::sdcardToggleRecording() {
   if (!liveData->params.sdcardInit)
     return;
 
-  Serial.println("Toggle SD card recording...");
+  syslog->println("Toggle SD card recording...");
   liveData->params.sdcardRecording = !liveData->params.sdcardRecording;
   if (liveData->params.sdcardRecording) {
     liveData->params.sdcardCanNotify = true;
@@ -1444,8 +1628,8 @@ void Board320_240::syncGPS() {
   }
   if (gps.satellites.isValid()) {
     liveData->params.gpsSat = gps.satellites.value();
-    //Serial.print("GPS satellites: ");
-    //Serial.println(liveData->params.gpsSat);
+    //syslog->print("GPS satellites: ");
+    //syslog->println(liveData->params.gpsSat);
   }
   if (!liveData->params.currTimeSyncWithGps && gps.date.isValid() && gps.time.isValid()) {
     liveData->params.currTimeSyncWithGps = true;
@@ -1464,4 +1648,139 @@ void Board320_240::syncGPS() {
   }
 }
 
-#endif // BOARD320_240_CPP
+
+/**
+  SIM800L
+*/
+bool Board320_240::sim800lSetup() {
+  syslog->print("Setting SIM800L module. HW port: ");
+  syslog->println(liveData->settings.gprsHwSerialPort);
+
+  gprsHwUart = new HardwareSerial(liveData->settings.gprsHwSerialPort);
+  gprsHwUart->begin(9600);
+
+  sim800l = new SIM800L((Stream *)gprsHwUart, SIM800L_RST, 768 , 128);
+  // SIM800L DebugMode:
+  //sim800l = new SIM800L((Stream *)gprsHwUart, SIM800L_RST, 768 , 128, (Stream *)&Serial);
+
+  bool sim800l_ready = sim800l->isReady();
+  for (uint8_t i = 0; i < 5 && !sim800l_ready; i++) {
+    syslog->println("Problem to initialize SIM800L module, retry in 1 sec");
+    delay(1000);
+    sim800l_ready = sim800l->isReady();
+  }
+
+  if (!sim800l_ready) {
+    syslog->println("Problem to initialize SIM800L module");
+  } else {
+    syslog->println("SIM800L module initialized");
+
+    sim800l->exitSleepMode();
+
+    if (sim800l->getPowerMode() != NORMAL) {
+      syslog->println("SIM800L module in sleep mode - Waking up");
+      if (sim800l->setPowerMode(NORMAL)) {
+        syslog->println("SIM800L in normal power mode");
+      } else {
+        syslog->println("Failed to switch SIM800L to normal power mode");
+      }
+    }
+
+    syslog->print("Setting GPRS APN to: ");
+    syslog->println(liveData->settings.gprsApn);
+
+    bool sim800l_gprs = sim800l->setupGPRS(liveData->settings.gprsApn);
+    for (uint8_t i = 0; i < 5 && !sim800l_gprs; i++) {
+      syslog->println("Problem to set GPRS APN, retry in 1 sec");
+      delay(1000);
+      sim800l_gprs = sim800l->setupGPRS(liveData->settings.gprsApn);
+    }
+
+    if (sim800l_gprs) {
+      liveData->params.sim800l_enabled = true;
+      syslog->println("GPRS APN set OK");
+    } else {
+      syslog->println("Problem to set GPRS APN");
+    }
+  }
+
+  return true;
+}
+
+bool Board320_240::sendDataViaGPRS() {
+  syslog->println("Sending data via GPRS");
+
+  if (liveData->params.socPerc < 0) {
+    syslog->println("No valid data, skipping data send");
+    return false;
+  }
+
+  NetworkRegistration network = sim800l->getRegistrationStatus();
+  if (network != REGISTERED_HOME && network != REGISTERED_ROAMING) {
+    syslog->println("SIM800L module not connected to network, skipping data send");
+    return false;
+  }
+
+  if (!sim800l->isConnectedGPRS()) {
+    syslog->println("GPRS not connected... Connecting");
+    bool connected = sim800l->connectGPRS();
+    for (uint8_t i = 0; i < 5 && !connected; i++) {
+      syslog->println("Problem to connect GPRS, retry in 1 sec");
+      delay(1000);
+      connected = sim800l->connectGPRS();
+    }
+    if (connected) {
+      syslog->println("GPRS connected!");
+    } else {
+      syslog->println("GPRS not connected! Reseting SIM800L module!");
+      sim800l->reset();
+      sim800lSetup();
+
+      return false;
+    }
+  }
+
+  syslog->println("Start HTTP POST...");
+
+  StaticJsonDocument<768> jsonData;
+
+  jsonData["apikey"] = liveData->settings.remoteApiKey;
+  jsonData["carType"] = liveData->settings.carType;
+  jsonData["ignitionOn"] = liveData->params.ignitionOn;
+  jsonData["chargingOn"] = liveData->params.chargingOn;
+  jsonData["socPerc"] = liveData->params.socPerc;
+  jsonData["sohPerc"] = liveData->params.sohPerc;
+  jsonData["batPowerKw"] = liveData->params.batPowerKw;
+  jsonData["batPowerAmp"] = liveData->params.batPowerAmp;
+  jsonData["batVoltage"] = liveData->params.batVoltage;
+  jsonData["auxVoltage"] = liveData->params.auxVoltage;
+  jsonData["auxAmp"] = liveData->params.auxCurrentAmp;
+  jsonData["batMinC"] = liveData->params.batMinC;
+  jsonData["batMaxC"] = liveData->params.batMaxC;
+  jsonData["batInletC"] = liveData->params.batInletC;
+  jsonData["batFanStatus"] = liveData->params.batFanStatus;
+  jsonData["speedKmh"] = liveData->params.speedKmh;
+  jsonData["odoKm"] = liveData->params.odoKm;
+  jsonData["cumulativeEnergyChargedKWh"] = liveData->params.cumulativeEnergyChargedKWh;
+  jsonData["cumulativeEnergyDischargedKWh"] = liveData->params.cumulativeEnergyDischargedKWh;
+
+  char payload[768];
+  serializeJson(jsonData, payload);
+
+  syslog->print("Sending payload: ");
+  syslog->println(payload);
+
+  syslog->print("Remote API server: ");
+  syslog->println(liveData->settings.remoteApiUrl);
+
+  uint16_t rc = sim800l->doPost(liveData->settings.remoteApiUrl, "application/json", payload, 10000, 10000);
+  if (rc == 200) {
+    syslog->println("HTTP POST successful");
+  } else {
+    // Failed...
+    syslog->print("HTTP POST error: ");
+    syslog->println(rc);
+  }
+
+  return true;
+}
diff --git a/Board320_240.h b/Board320_240.h
index 0dc4f27..3cd563b 100644
--- a/Board320_240.h
+++ b/Board320_240.h
@@ -1,5 +1,4 @@
-#ifndef BOARD320_240_H
-#define BOARD320_240_H
+#pragma once
 
 // TFT COMMON
 #define LOAD_GLCD   // Font 1. Original Adafruit 8 pixel font needs ~1820 bytes in FLASH
@@ -12,10 +11,16 @@
 #define SMOOTH_FONT
 #define GFXFF 1  // TFT FOnts
 
+// DEEP SLEEP
+#define TIME_TO_SLEEP 60 // Sleep time in secs
+
 //
 #include <TFT_eSPI.h>
 #include <TinyGPS++.h>
 #include "BoardInterface.h"
+#include <SD.h>
+#include <SPI.h>
+#include "SIM800L.h"
 
 class Board320_240 : public BoardInterface {
 
@@ -23,8 +28,9 @@ class Board320_240 : public BoardInterface {
     // TFT, SD SPI
     TFT_eSPI tft = TFT_eSPI();
     TFT_eSprite spr = TFT_eSprite(&tft);
-    //SPIClass spiSD(HSPI);
     HardwareSerial* gpsHwUart = NULL;
+    HardwareSerial* gprsHwUart = NULL;
+    SIM800L* sim800l;
     TinyGPSPlus gps;
     char tmpStr1[20];
     char tmpStr2[20];
@@ -46,11 +52,16 @@ class Board320_240 : public BoardInterface {
     void afterSetup() override;
     void mainLoop() override;
     bool skipAdapterScan() override;
+    void goToSleep();
+    void afterSleep();
     // SD card
     bool sdcardMount() override;
     void sdcardToggleRecording() override;
     // GPS
     void syncGPS();
+    // SIM800L
+    bool sim800lSetup();
+    bool sendDataViaGPRS();
     // Basic GUI
     void setBrightness(byte lcdBrightnessPerc) override;
     void displayMessage(const char* row1, const char* row2) override;
@@ -76,5 +87,3 @@ class Board320_240 : public BoardInterface {
     void loadTestData();
     //
 };
-
-#endif // BOARD320_240_H
diff --git a/BoardInterface.cpp b/BoardInterface.cpp
index b9ba737..b7198c4 100644
--- a/BoardInterface.cpp
+++ b/BoardInterface.cpp
@@ -1,6 +1,3 @@
-#ifndef BOARDINTERFACE_CPP
-#define BOARDINTERFACE_CPP
-
 #define ARDUINOJSON_USE_LONG_LONG 1
 
 #include <ArduinoJson.h>
@@ -31,7 +28,7 @@ void BoardInterface::attachCar(CarInterface* pCarInterface) {
 */
 void BoardInterface::shutdownDevice() {
 
-  Serial.println("Shutdown.");
+  syslog->println("Shutdown.");
 
   char msg[20];
   for (int i = 3; i >= 1; i--) {
@@ -41,7 +38,7 @@ void BoardInterface::shutdownDevice() {
   }
 
 #ifdef SIM800L_ENABLED
-  if(sim800l->isConnectedGPRS()) {
+  if (sim800l->isConnectedGPRS()) {
     sim800l->disconnectGPRS();
   }
   sim800l->setPowerMode(MINIMUM);
@@ -51,7 +48,8 @@ void BoardInterface::shutdownDevice() {
   setBrightness(0);
   //WiFi.disconnect(true);
   //WiFi.mode(WIFI_OFF);
-  btStop();
+
+  commInterface->disconnectDevice();
   //adc_power_off();
   //esp_wifi_stop();
   esp_bt_controller_disable();
@@ -67,7 +65,7 @@ void BoardInterface::shutdownDevice() {
 void BoardInterface::saveSettings() {
 
   // Flash to memory
-  Serial.println("Settings saved to eeprom.");
+  syslog->println("Settings saved to eeprom.");
   EEPROM.put(0, liveData->settings);
   EEPROM.commit();
 }
@@ -78,7 +76,7 @@ void BoardInterface::saveSettings() {
 void BoardInterface::resetSettings() {
 
   // Flash to memory
-  Serial.println("Factory reset.");
+  syslog->println("Factory reset.");
   liveData->settings.initFlag = 1;
   EEPROM.put(0, liveData->settings);
   EEPROM.commit();
@@ -98,7 +96,7 @@ void BoardInterface::loadSettings() {
 
   // Default settings
   liveData->settings.initFlag = 183;
-  liveData->settings.settingsVersion = 5;
+  liveData->settings.settingsVersion = 6;
   liveData->settings.carType = CAR_KIA_ENIRO_2020_64;
   tmpStr = "00:00:00:00:00:00"; // Pair via menu (middle button)
   tmpStr.toCharArray(liveData->settings.obdMacAddress, tmpStr.length() + 1);
@@ -114,9 +112,9 @@ void BoardInterface::loadSettings() {
   liveData->settings.pressureUnit = 'b';
   liveData->settings.defaultScreen = 1;
   liveData->settings.lcdBrightness = 0;
-  liveData->settings.debugScreen = 0;
+  liveData->settings.sleepModeEnabled = 0;
   liveData->settings.predrawnChargingGraphs = 1;
-  liveData->settings.commType = 0; // BLE4
+  liveData->settings.commType = COMM_TYPE_OBD2BLE4; // BLE4
   liveData->settings.wifiEnabled = 0;
   tmpStr = "empty";
   tmpStr.toCharArray(liveData->settings.wifiSsid, tmpStr.length() + 1);
@@ -138,19 +136,24 @@ void BoardInterface::loadSettings() {
   tmpStr.toCharArray(liveData->settings.remoteApiKey, tmpStr.length() + 1);
   liveData->settings.headlightsReminder = 0;
   liveData->settings.gpsHwSerialPort = 255; // off
+  liveData->settings.gprsHwSerialPort = 255; // off
+  liveData->settings.serialConsolePort = 0; // hwuart0
+  liveData->settings.debugLevel = 1; // 0 - info only, 1 - debug communication (BLE/CAN), 2 - debug GSM, 3 - debug SDcard
+  liveData->settings.sdcardLogIntervalSec = 2;
+  liveData->settings.gprsLogIntervalSec = 60;
 
   // Load settings and replace default values
-  Serial.println("Reading settings from eeprom.");
+  syslog->println("Reading settings from eeprom.");
   EEPROM.begin(sizeof(SETTINGS_STRUC));
   EEPROM.get(0, liveData->tmpSettings);
 
   // Init flash with default settings
   if (liveData->tmpSettings.initFlag != 183) {
-    Serial.println("Settings not found. Initialization.");
+    syslog->println("Settings not found. Initialization.");
     saveSettings();
   } else {
-    Serial.print("Loaded settings ver.: ");
-    Serial.println(liveData->tmpSettings.settingsVersion);
+    syslog->print("Loaded settings ver.: ");
+    syslog->println(liveData->tmpSettings.settingsVersion);
 
     // Upgrade structure
     if (liveData->settings.settingsVersion != liveData->tmpSettings.settingsVersion) {
@@ -158,7 +161,7 @@ void BoardInterface::loadSettings() {
         liveData->tmpSettings.settingsVersion = 2;
         liveData->tmpSettings.defaultScreen = liveData->settings.defaultScreen;
         liveData->tmpSettings.lcdBrightness = liveData->settings.lcdBrightness;
-        liveData->tmpSettings.debugScreen = liveData->settings.debugScreen;
+        liveData->tmpSettings.sleepModeEnabled = liveData->settings.sleepModeEnabled;
       }
       if (liveData->tmpSettings.settingsVersion == 2) {
         liveData->tmpSettings.settingsVersion = 3;
@@ -166,7 +169,7 @@ void BoardInterface::loadSettings() {
       }
       if (liveData->tmpSettings.settingsVersion == 3) {
         liveData->tmpSettings.settingsVersion = 4;
-        liveData->tmpSettings.commType = 0; // BLE4
+        liveData->tmpSettings.commType = COMM_TYPE_OBD2BLE4; // BLE4
         liveData->tmpSettings.wifiEnabled = 0;
         tmpStr = "empty";
         tmpStr.toCharArray(liveData->tmpSettings.wifiSsid, tmpStr.length() + 1);
@@ -192,6 +195,13 @@ void BoardInterface::loadSettings() {
         liveData->tmpSettings.settingsVersion = 5;
         liveData->tmpSettings.gpsHwSerialPort = 255; // off
       }
+      if (liveData->tmpSettings.settingsVersion == 5) {
+        liveData->tmpSettings.settingsVersion = 6;
+        liveData->tmpSettings.serialConsolePort = 0; // hwuart0
+        liveData->tmpSettings.debugLevel = 0; // show all
+        liveData->tmpSettings.sdcardLogIntervalSec = 2;
+        liveData->tmpSettings.gprsLogIntervalSec = 60;
+      }
 
       // Save upgraded structure
       liveData->settings = liveData->tmpSettings;
@@ -201,6 +211,8 @@ void BoardInterface::loadSettings() {
     // Apply settings from flash if needed
     liveData->settings = liveData->tmpSettings;
   }
+
+  syslog->setDebugLevel(liveData->settings.debugLevel);
 }
 
 /**
@@ -208,14 +220,22 @@ void BoardInterface::loadSettings() {
 */
 void BoardInterface::afterSetup() {
 
+  syslog->println("BoardInterface::afterSetup");
+
   // Init Comm iterface
+  syslog->print("Init communication device: ");
+  syslog->println(liveData->settings.commType);
+
   if (liveData->settings.commType == COMM_TYPE_OBD2BLE4) {
     commInterface = new CommObd2Ble4();
   } else if (liveData->settings.commType == COMM_TYPE_OBD2CAN) {
-    commInterface = new CommObd2Ble4();
-    //commInterface = new CommObd2Can();
+    commInterface = new CommObd2Can();
+  } else if (liveData->settings.commType == COMM_TYPE_OBD2BT3) {
+    //commInterface = new CommObd2Bt3();
+    syslog->println("BT3 not implemented");
   }
-  //commInterface->initComm(liveData, NULL);
+
+  commInterface->initComm(liveData, this);
   commInterface->connectDevice();
 }
 
@@ -241,6 +261,15 @@ void BoardInterface::customConsoleCommand(String cmd) {
   if (key == "remoteApiKey") value.toCharArray(liveData->settings.remoteApiKey, value.length() + 1);
 }
 
+/**
+   Parser response from obd2/can
+*/
+void BoardInterface::parseRowMerged() {
+
+  carInterface->parseRowMerged();
+}
+
+
 /**
    Serialize parameters
 */
@@ -313,5 +342,3 @@ bool BoardInterface::serializeParamsToJson(File file, bool inclApiKey) {
   serializeJson(jsonData, Serial);
   serializeJson(jsonData, file);
 }
-
-#endif // BOARDINTERFACE_CPP
diff --git a/BoardInterface.h b/BoardInterface.h
index 3a2db28..1602014 100644
--- a/BoardInterface.h
+++ b/BoardInterface.h
@@ -1,5 +1,4 @@
-#ifndef BOARDINTERFACE_H
-#define BOARDINTERFACE_H
+#pragma once
 
 #include <FS.h>
 #include "LiveData.h"
@@ -24,12 +23,6 @@ class BoardInterface {
     bool testDataMode = false;
     bool scanDevices = false;
     String sdcardRecordBuffer = "";
-    // Debug screen - next command with right button
-    uint16_t debugCommandIndex = 0;
-    String debugAtshRequest = "ATSH7E4";
-    String debugCommandRequest = "220101";
-    String debugLastString = "620101FFF7E7FF99000000000300B10EFE120F11100F12000018C438C30B00008400003864000035850000153A00001374000647010D017F0BDA0BDA03E8";
-    String debugPreviousString = "620101FFF7E7FFB3000000000300120F9B111011101011000014CC38CB3B00009100003A510000367C000015FB000013D3000690250D018E0000000003E8";
     //
     void setLiveData(LiveData* pLiveData);
     void attachCar(CarInterface* pCarInterface);
@@ -41,6 +34,7 @@ class BoardInterface {
     virtual void displayMessage(const char* row1, const char* row2)=0;
     virtual void setBrightness(byte lcdBrightnessPerc)=0;
     virtual void redrawScreen()=0;
+    void parseRowMerged();
     // Menu
     virtual void showMenu()=0;
     virtual void hideMenu()=0;
@@ -55,5 +49,3 @@ class BoardInterface {
     virtual void sdcardToggleRecording()=0;
     bool serializeParamsToJson(File file, bool inclApiKey = false);
 };
-
-#endif // BOARDINTERFACE_H
diff --git a/BoardM5stackCore.cpp b/BoardM5stackCore.cpp
index 12bf0d9..523a067 100644
--- a/BoardM5stackCore.cpp
+++ b/BoardM5stackCore.cpp
@@ -1,6 +1,3 @@
-#ifndef BOARDM5STACKCORE_CPP
-#define BOARDM5STACKCORE_CPP
-
 #include "BoardInterface.h"
 #include "Board320_240.h"
 #include "BoardM5stackCore.h"
@@ -24,7 +21,7 @@ void BoardM5stackCore::initBoard() {
 
   // Mute speaker
   //ledcWriteTone(TONE_PIN_CHANNEL, 0);
-  digitalWrite(SPEAKER_PIN, 0);
+  dacWrite(SPEAKER_PIN, 0);
 
   //
   Board320_240::initBoard();
@@ -34,5 +31,3 @@ void BoardM5stackCore::mainLoop() {
 
   Board320_240::mainLoop();
 }
-
-#endif // BOARDM5STACKCORE_CPP
diff --git a/BoardM5stackCore.h b/BoardM5stackCore.h
index 93078f8..71732d1 100644
--- a/BoardM5stackCore.h
+++ b/BoardM5stackCore.h
@@ -1,5 +1,4 @@
-#ifndef BOARDM5STACKCORE_H
-#define BOARDM5STACKCORE_H
+#pragma once
 
 // Setup for m5stack core
 #define USER_SETUP_LOADED 1
@@ -42,5 +41,3 @@ class BoardM5stackCore : public Board320_240 {
     void initBoard() override;
     void mainLoop() override;
 };
-
-#endif // BOARDM5STACKCORE_H
diff --git a/BoardTtgoT4v13.cpp b/BoardTtgoT4v13.cpp
index 4057a96..97bf82b 100644
--- a/BoardTtgoT4v13.cpp
+++ b/BoardTtgoT4v13.cpp
@@ -1,6 +1,3 @@
-#ifndef BOARDTTGOT4V13_CPP
-#define BOARDTTGOT4V13_CPP
-
 #include "BoardInterface.h"
 #include "Board320_240.h"
 #include "BoardTtgoT4v13.h"
@@ -22,5 +19,3 @@ void BoardTtgoT4v13::initBoard() {
 
   Board320_240::initBoard();
 }
-
-#endif // BOARDTTGOT4V13_CPP
diff --git a/CarBmwI3.cpp b/CarBmwI3.cpp
new file mode 100644
index 0000000..6dfe532
--- /dev/null
+++ b/CarBmwI3.cpp
@@ -0,0 +1,412 @@
+#include "CarBmwI3.h"
+#include <vector>
+#include <algorithm>
+
+/**
+   activateliveData->commandQueue
+*/
+void CarBmwI3::activateCommandQueue() {
+	const uint16_t commandQueueLoopFrom = 18;
+	
+//	const std::vector<String> commandQueue = {
+   const std::vector<LiveData::Command_t> commandQueue = {
+		{0, "ATZ"},		// Reset all
+		{0, "ATD"},		// All to defaults
+		{0, "ATI"},		// Print the version ID
+		{0, "ATE0"},	// Echo off
+		{0, "ATPP2COFF"}, // Disable prog parameter 2C
+		//{0, "ATSH6F1"},	// Set header to 6F1
+		{0, "ATCF600"}, //	Set the ID filter to 600
+		{0, "ATCM700"}, //	Set the ID mask to 700
+		{0, "ATPBC001"}, //	 Protocol B options and baudrate (div 1 = 500k)
+		{0, "ATSPB"}, //	Set protocol to B and save it (USER1 11bit, 125kbaud)
+		{0, "ATAT0"}, //	Adaptive timing off
+		{0, "ATSTFF"}, //	Set timeout to ff x 4ms
+		{0, "ATAL"}, //	Allow long messages ( > 7 Bytes)
+		{0, "ATH1"}, //	Additional headers on
+		{0, "ATS0"}, //	Printing of spaces off
+		{0, "ATL0"}, //	Linefeeds off
+		{0, "ATCSM0"}, //	Silent monitoring off
+		{0, "ATCTM5"}, //	Set timer multiplier to 5
+		{0, "ATJE"}, //	Use J1939 SAE data format
+
+		// Loop from (BMW i3)
+		// BMS
+		{0, "ATSH6F1"},
+
+    {0x12, "22402B"}, // STATUS_MESSWERTE_IBS - 12V Bat
+    //////{0x12, "22F101"}, // STATUS_A_T_ELUE ???
+    {0x60, "22D107"}, // Speed km/h (ELM CAN send: '600322D107000000')
+    {0x60, "22D10D"}, // Total km without offset (ELM CAN send: '600322D10D000000')
+    {0x60, "22D114"}, // Total km - offset (ELM CAN send: '600322D114000000')
+    {0x78, "22D85C"}, // Calculated indoor temperature (ELM CAN send: '780322D85C000000')
+    {0x78, "22D96B"}, // Outdoor temperature
+    //{0, "22DC61"}, // BREMSLICHT_SCHALTER
+    {0x07, "22DD7B"}, // ALTERUNG_KAPAZITAET Aging of kapacity
+    {0x07, "22DD7C"}, // GW_INFO - should contain kWh but in some strange form
+    {0x07, "22DDBF"}, // Min and Max cell voltage
+    {0x07, "22DDC0"}, // TEMPERATUREN
+    {0x07, "22DD69"}, // HV_STORM
+    {0x07, "22DD6C"}, // KUEHLKREISLAUF_TEMP
+    {0x07, "22DDB4"}, // HV_SPANNUNG
+    {0x07, "22DDBC"}  // SOC
+		
+		
+	};
+
+	// 60Ah / 22kWh version
+	liveData->params.batteryTotalAvailableKWh = 18.8;
+	liveData->params.batModuleTempCount = 4; //?
+  
+  // init params which are currently not filled from parsed data
+  liveData->params.tireFrontLeftPressureBar = 0;
+  liveData->params.tireFrontLeftTempC = 0;
+  liveData->params.tireRearLeftPressureBar = 0;
+  liveData->params.tireRearLeftTempC = 0;
+  liveData->params.tireFrontRightPressureBar = 0;
+  liveData->params.tireFrontRightTempC = 0;
+  liveData->params.tireRearRightPressureBar = 0;
+  liveData->params.tireRearRightTempC = 0;
+
+	//  Empty and fill command queue
+  liveData->commandQueue.clear(); // probably not needed before assign
+  liveData->commandQueue.assign(commandQueue.begin(), commandQueue.end());
+
+	liveData->commandQueueLoopFrom = commandQueueLoopFrom;
+	liveData->commandQueueCount = commandQueue.size();
+  liveData->bAdditionalStartingChar = true; // there is one additional byte in received packets compared to other cars
+  liveData->expectedMinimalPacketLength = 6;  // to filter occasional 5-bytes long packets
+  liveData->rxTimeoutMs = 500; // timeout for receiving of CAN response
+  liveData->delayBetweenCommandsMs = 100; // delay between commands, set to 0 if no delay is needed 
+}
+
+/**
+   parseRowMerged
+*/
+void CarBmwI3::parseRowMerged() 
+{
+  syslog->print("--mergedVectorLength: ");   syslog->println(liveData->vResponseRowMerged.size());
+
+  struct Header_t
+  {
+    uint8_t startChar;
+    uint8_t pid[2];
+    uint8_t pData[];
+
+    uint16_t getPid() { return 256 * pid[0] + pid[1]; };
+  };
+
+  Header_t* pHeader = (Header_t*)liveData->vResponseRowMerged.data();
+  
+  
+  const uint16_t payloadLength = liveData->vResponseRowMerged.size() - sizeof(Header_t);
+  
+  // create reversed payload to get little endian order of data
+  std::vector<uint8_t> payloadReversed(pHeader->pData, pHeader->pData + payloadLength);
+  std::reverse(payloadReversed.begin(), payloadReversed.end());
+  
+  //syslog->print("--extracted PID: "); syslog->println(pHeader->getPid());
+  //syslog->print("--payload length: "); syslog->println(payloadLength);
+  
+  #pragma pack(push, 1)
+      
+  // BMS
+  if (liveData->currentAtshRequest.equals("ATSH6F1")) {
+
+    switch (pHeader->getPid()) {
+    case 0x402B:
+    {
+      struct s402B_t {
+        int16_t unknown[13];
+        uint16_t auxRawCurrent;
+        uint16_t auxRawVoltage;
+        int16_t auxTemp;
+      };
+
+      if (payloadLength == sizeof(s402B_t)) {
+        s402B_t* ptr = (s402B_t*)payloadReversed.data();
+
+        liveData->params.auxTemperature = ptr->auxTemp / 10.0;
+        liveData->params.auxVoltage = ptr->auxRawVoltage / 4000.0 + 6;
+        liveData->params.auxCurrentAmp = - (ptr->auxRawCurrent / 12.5 - 200);
+      }
+        
+    }
+    break;
+
+    case 0xD107: // Speed km/h
+    {
+      struct D107_t {
+        uint16_t speedKmh;
+      };
+
+      if (payloadLength == sizeof(D107_t)) { 
+        D107_t* ptr = (D107_t*)payloadReversed.data();
+        liveData->params.speedKmh = ptr->speedKmh / 10.0;
+
+        syslog->print("----speed km/h: ");   syslog->println(liveData->params.speedKmh);
+      }
+    }
+    break;
+
+    case 0xD10D: // Total km wihtout offset
+    {
+      struct D10D_t {
+        uint32_t totalKm1; // one of those two is RAM and other is EEPROM value
+        uint32_t totalKm2;
+      };
+
+      if (payloadLength == sizeof(D10D_t)) { 
+        D10D_t* ptr = (D10D_t*)payloadReversed.data();
+        liveData->params.odoKm = ptr->totalKm1 - totalDistanceKmOffset;
+
+        syslog->print("----total km1: ");   syslog->println(ptr->totalKm1);
+        syslog->print("----total km2: ");   syslog->println(ptr->totalKm2);
+        syslog->print("----total km:  ");   syslog->println(liveData->params.odoKm);
+      }
+    }
+    break;
+
+    case 0xD114: // Offset of total km
+    {
+      struct D114_t {
+        uint8_t offsetKm;
+      };
+
+      if (payloadLength == sizeof(D114_t)) { 
+        D114_t* ptr = (D114_t*)payloadReversed.data();
+        totalDistanceKmOffset = ptr->offsetKm;
+
+        syslog->print("----total off: ");   syslog->println(totalDistanceKmOffset);
+      }
+    }
+    break;
+    
+    case 0xD85C:
+    {
+      struct D85C_t {
+        int8_t indoorTemp;
+      };
+
+      if (payloadLength == sizeof(D85C_t)) { 
+        D85C_t* ptr = (D85C_t*)payloadReversed.data();
+        liveData->params.indoorTemperature = ptr->indoorTemp;
+      }
+    }
+    break;
+
+    case 0xD96B:
+    {
+      struct D96B_t {
+        uint16_t outdoorTempRaw;
+      };
+
+      if (payloadLength == sizeof(D96B_t)) { 
+        D96B_t* ptr = (D96B_t*)payloadReversed.data();
+        liveData->params.outdoorTemperature = (ptr->outdoorTempRaw / 2.0) - 40.0;
+      }
+    }
+    break;
+    
+    case 0xDD69:
+    {
+      struct DD69_t {
+        uint8_t unknown[4];
+        int32_t batAmp;
+      };
+
+      if (payloadLength == sizeof(DD69_t)) { 
+        DD69_t* ptr = (DD69_t*)payloadReversed.data();
+      
+        liveData->params.batPowerAmp =  ptr->batAmp / 100.0; //liveData->hexToDecFromResponse(6, 14, 4, true) / 100.0;
+      
+        liveData->params.batPowerKw = (liveData->params.batPowerAmp * liveData->params.batVoltage) / 1000.0;
+        if (liveData->params.batPowerKw < 0) // Reset charging start time
+          liveData->params.chargingStartTime = liveData->params.currentTime;
+
+        // calculate kWh/100
+        liveData->params.batPowerKwh100 = liveData->params.batPowerKw / liveData->params.speedKmh * 100;
+
+        // update charging graph data if car is charging
+        if (liveData->params.speedKmh < 10 && liveData->params.batPowerKw >= 1 && liveData->params.socPerc > 0 && liveData->params.socPerc <= 100) {
+          if ( liveData->params.chargingGraphMinKw[int(liveData->params.socPerc)] < 0 || liveData->params.batPowerKw < liveData->params.chargingGraphMinKw[int(liveData->params.socPerc)])
+            liveData->params.chargingGraphMinKw[int(liveData->params.socPerc)] = liveData->params.batPowerKw;
+          if ( liveData->params.chargingGraphMaxKw[int(liveData->params.socPerc)] < 0 || liveData->params.batPowerKw > liveData->params.chargingGraphMaxKw[int(liveData->params.socPerc)])
+            liveData->params.chargingGraphMaxKw[int(liveData->params.socPerc)] = liveData->params.batPowerKw;
+        }
+      }
+    }
+    break;
+
+    case 0xDD6C:
+    {
+      struct DD6C_t {
+        int16_t tempCoolant;
+      };
+
+      if (payloadLength == sizeof(DD6C_t)) {
+        DD6C_t* ptr = (DD6C_t*)payloadReversed.data();
+
+        liveData->params.coolingWaterTempC = ptr->tempCoolant / 10.0;
+        liveData->params.coolantTemp1C = ptr->tempCoolant / 10.0;
+        liveData->params.coolantTemp2C = ptr->tempCoolant / 10.0;
+
+        // update charging graph data if car is charging
+        if (liveData->params.speedKmh < 10 && liveData->params.batPowerKw >= 1 && liveData->params.socPerc > 0 && liveData->params.socPerc <= 100) {
+          liveData->params.chargingGraphWaterCoolantTempC[int(liveData->params.socPerc)] = liveData->params.coolingWaterTempC;
+        }
+      }
+      
+    }
+    break;
+
+    case 0xDD7B:
+    {
+      struct DD7B_t {
+        uint8_t agingOfCapacity;
+      };
+
+      if (payloadLength == sizeof(DD7B_t)) {
+        DD7B_t* ptr = (DD7B_t*)payloadReversed.data();
+
+        liveData->params.sohPerc = ptr->agingOfCapacity;
+      }
+      
+    }
+    break;
+
+    case 0xDD7C:
+    {
+      struct DD7C_t {
+        //uint8_t unused1;
+        uint32_t discharged;
+        uint32_t charged;
+        uint8_t unknown[];
+      };
+
+      Serial.print("DD7C received, struct sizeof is "); Serial.println(sizeof(DD7C_t));
+      if (payloadLength >= sizeof(DD7C_t)) {
+        
+        DD7C_t* ptr = (DD7C_t*)(payloadReversed.data() + 1); // skip one charcter on beginning (TODO: fix when pragma push/pack is done)
+        liveData->params.cumulativeEnergyDischargedKWh = ptr->discharged / 100000.0;
+        if (liveData->params.cumulativeEnergyDischargedKWhStart == -1)
+          liveData->params.cumulativeEnergyDischargedKWhStart = liveData->params.cumulativeEnergyDischargedKWh;
+
+        liveData->params.cumulativeEnergyChargedKWh = ptr->charged / 100000.0;
+        if (liveData->params.cumulativeEnergyChargedKWhStart == -1)
+          liveData->params.cumulativeEnergyChargedKWhStart = liveData->params.cumulativeEnergyChargedKWh;
+      }
+      
+    }
+    break;
+
+    case 0xDDB4:
+    {
+      struct DDB4_t {
+        uint16_t batVoltage;
+      };
+      
+      if (payloadLength == sizeof(DDB4_t)) { // HV_SPANNUNG_BATTERIE
+        DDB4_t* ptr = (DDB4_t*)payloadReversed.data();
+        
+        liveData->params.batVoltage = ptr->batVoltage / 100.0;
+        liveData->params.batPowerKw = (liveData->params.batPowerAmp * liveData->params.batVoltage) / 1000.0;
+        if (liveData->params.batPowerKw < 0) // Reset charging start time
+          liveData->params.chargingStartTime = liveData->params.currentTime;
+      }
+    }
+    break;
+
+    case 0xDDBF:
+    {
+      struct DDBF_t {
+        uint16_t unused[2];
+        uint16_t ucellMax;
+        uint16_t ucellMin;
+      };
+      
+      if (payloadLength == sizeof(DDBF_t)) { // HV_SPANNUNG_BATTERIE
+        DDBF_t* ptr = (DDBF_t*)payloadReversed.data();
+        
+        liveData->params.batCellMaxV = ptr->ucellMax / 1000.0;
+        liveData->params.batCellMinV = ptr->ucellMin / 1000.0;
+      }
+    }
+    break;
+
+    case 0xDDC0:
+    {
+      struct DDC0_t {
+        uint8_t unknown[2];
+        int16_t tempAvg;
+        int16_t tempMax;
+        int16_t tempMin;
+      };
+      
+      if (payloadLength == sizeof(DDC0_t)) {
+        DDC0_t* ptr = (DDC0_t*)payloadReversed.data();
+        
+        liveData->params.batMinC = ptr->tempMin / 100.0;
+        liveData->params.batTempC = ptr->tempAvg / 100.0;
+        liveData->params.batMaxC = ptr->tempMax / 100.0;
+
+        liveData->params.batModuleTempC[0] = liveData->params.batTempC;
+        liveData->params.batModuleTempC[1] = liveData->params.batTempC;
+        liveData->params.batModuleTempC[2] = liveData->params.batTempC;
+        liveData->params.batModuleTempC[3] = liveData->params.batTempC;
+
+        // update charging graph data if car is charging
+        if (liveData->params.speedKmh < 10 && liveData->params.batPowerKw >= 1 && liveData->params.socPerc > 0 && liveData->params.socPerc <= 100) {
+          liveData->params.chargingGraphBatMinTempC[int(liveData->params.socPerc)] = liveData->params.batMinC;
+          liveData->params.chargingGraphBatMaxTempC[int(liveData->params.socPerc)] = liveData->params.batMaxC;
+          //liveData->params.chargingGraphHeaterTempC[int(liveData->params.socPerc)] = liveData->params.batHeaterC;
+         
+        }
+      }
+    }
+    break;
+
+    case 0xDDBC:
+    {
+      struct DDBC_t {
+        uint8_t unknown[2];
+        uint16_t socMin;
+        uint16_t socMax;
+        uint16_t soc;
+      };
+      
+      if (payloadLength == sizeof(DDBC_t)) {
+        DDBC_t* ptr = (DDBC_t*)payloadReversed.data();
+
+        liveData->params.socPercPrevious = liveData->params.socPerc;
+        liveData->params.socPerc = ptr->soc / 10.0;
+        
+        // Soc10ced table, record x0% CEC/CED table (ex. 90%->89%, 80%->79%)
+        if(liveData->params.socPercPrevious - liveData->params.socPerc > 0) {
+          byte index = (int(liveData->params.socPerc) == 4) ? 0 : (int)(liveData->params.socPerc / 10) + 1;
+          if ((int(liveData->params.socPerc) % 10 == 9 || int(liveData->params.socPerc) == 4) && liveData->params.soc10ced[index] == -1) {
+            liveData->params.soc10ced[index] = liveData->params.cumulativeEnergyDischargedKWh;
+            liveData->params.soc10cec[index] = liveData->params.cumulativeEnergyChargedKWh;
+            liveData->params.soc10odo[index] = liveData->params.odoKm;
+            liveData->params.soc10time[index] = liveData->params.currentTime;
+          }
+        }
+      }
+    }
+    break;
+
+    
+    } // switch
+    
+  } // ATSH6F1
+#pragma pack(pop)
+}
+
+/**
+   loadTestData
+*/
+void CarBmwI3::loadTestData() 
+{
+
+	
+}
diff --git a/CarBmwI3.h b/CarBmwI3.h
new file mode 100644
index 0000000..eaa3c3e
--- /dev/null
+++ b/CarBmwI3.h
@@ -0,0 +1,16 @@
+#pragma once
+
+#include "CarInterface.h"
+
+class CarBmwI3 : public CarInterface {
+  
+protected:
+    
+public:
+	void activateCommandQueue() override;
+	void parseRowMerged() override;
+	void loadTestData() override;  
+
+private:
+  uint8_t totalDistanceKmOffset = 0;
+};
diff --git a/CarHyundaiIoniq.cpp b/CarHyundaiIoniq.cpp
index 69f3e40..d2fad5e 100644
--- a/CarHyundaiIoniq.cpp
+++ b/CarHyundaiIoniq.cpp
@@ -1,9 +1,6 @@
-#ifndef CARHYUNDAIIONIQ_CPP
-#define CARHYUNDAIIONIQ_CPP
-
 #include "CarHyundaiIoniq.h"
+#include <vector>
 
-#define commandQueueCountHyundaiIoniq 25
 #define commandQueueLoopFromHyundaiIoniq 8
 
 /**
@@ -11,7 +8,7 @@
 */
 void CarHyundaiIoniq::activateCommandQueue() {
 
-  String commandQueueHyundaiIoniq[commandQueueCountHyundaiIoniq] = {
+  std::vector<String> commandQueueHyundaiIoniq = {
     "AT Z",      // Reset all
     "AT I",      // Print the version ID
     "AT E0",     // Echo off
@@ -34,13 +31,17 @@ void CarHyundaiIoniq::activateCommandQueue() {
     "2103",   // cell voltages, screen 3 only
     "2104",   // cell voltages, screen 3 only
     "2105",   // soh, soc, ..
-    "2106",   // cooling water temp
 
     // VMCU
     "ATSH7E2",
     "2101",     // speed, ...
     "2102",     // aux, ...
 
+    // IGPM
+    "ATSH770",
+    "22BC03",     // low beam
+    "22BC06",     // brake light
+
     //"ATSH7Df",
     //"2106",
     //"220106",
@@ -67,15 +68,13 @@ void CarHyundaiIoniq::activateCommandQueue() {
   liveData->params.batModuleTempCount = 12;
 
   //  Empty and fill command queue
-  for (int i = 0; i < 300; i++) {
-    liveData->commandQueue[i] = "";
-  }
-  for (int i = 0; i < commandQueueCountHyundaiIoniq; i++) {
-    liveData->commandQueue[i] = commandQueueHyundaiIoniq[i];
+  liveData->commandQueue.clear();
+  for (auto cmd : commandQueueHyundaiIoniq) {
+    liveData->commandQueue.push_back({ 0, cmd });
   }
 
   liveData->commandQueueLoopFrom = commandQueueLoopFromHyundaiIoniq;
-  liveData->commandQueueCount = commandQueueCountHyundaiIoniq;
+  liveData->commandQueueCount = commandQueueHyundaiIoniq.size();
 }
 
 /**
@@ -83,6 +82,10 @@ void CarHyundaiIoniq::activateCommandQueue() {
 */
 void CarHyundaiIoniq::parseRowMerged() {
 
+  uint8_t tempByte;
+  float tempFloat;
+  String tmpStr;
+
   // VMCU 7E2
   if (liveData->currentAtshRequest.equals("ATSH7E2")) {
     if (liveData->commandRequest.equals("2101")) {
@@ -91,11 +94,29 @@ void CarHyundaiIoniq::parseRowMerged() {
         liveData->params.speedKmh = 0;
     }
     if (liveData->commandRequest.equals("2102")) {
-      liveData->params.auxPerc = liveData->hexToDecFromResponse(50, 52, 1, false);
       liveData->params.auxCurrentAmp = - liveData->hexToDecFromResponse(46, 50, 2, true) / 1000.0;
     }
   }
 
+  // IGPM
+  if (liveData->currentAtshRequest.equals("ATSH770")) {
+    if (liveData->commandRequest.equals("22BC03")) {
+      tempByte = liveData->hexToDecFromResponse(16, 18, 1, false);
+      liveData->params.ignitionOn = (bitRead(tempByte, 5) == 1);
+      if (liveData->params.ignitionOn) {
+        liveData->params.lastIgnitionOnTime = liveData->params.currentTime;
+      }
+
+      tempByte = liveData->hexToDecFromResponse(18, 20, 1, false);
+      liveData->params.headLights = (bitRead(tempByte, 5) == 1);
+      liveData->params.dayLights = (bitRead(tempByte, 3) == 1);
+    }
+    if (liveData->commandRequest.equals("22BC06")) {
+      tempByte = liveData->hexToDecFromResponse(14, 16, 1, false);
+      liveData->params.brakeLights = (bitRead(tempByte, 5) == 1);
+    }
+  }
+
   // Cluster module 7c6
   if (liveData->currentAtshRequest.equals("ATSH7C6")) {
     if (liveData->commandRequest.equals("22B002")) {
@@ -126,10 +147,25 @@ void CarHyundaiIoniq::parseRowMerged() {
         liveData->params.cumulativeEnergyDischargedKWhStart = liveData->params.cumulativeEnergyDischargedKWh;
       liveData->params.availableChargePower = liveData->decFromResponse(16, 20) / 100.0;
       liveData->params.availableDischargePower = liveData->decFromResponse(20, 24) / 100.0;
-      liveData->params.isolationResistanceKOhm = liveData->hexToDecFromResponse(118, 122, 2, true);
-      liveData->params.batFanStatus = liveData->hexToDecFromResponse(58, 60, 2, true);
-      liveData->params.batFanFeedbackHz = liveData->hexToDecFromResponse(60, 62, 2, true);
-      liveData->params.auxVoltage = liveData->hexToDecFromResponse(62, 64, 2, true) / 10.0;
+      liveData->params.isolationResistanceKOhm = liveData->hexToDecFromResponse(118, 122, 2, false);
+      liveData->params.batFanStatus = liveData->hexToDecFromResponse(58, 60, 1, false);
+      liveData->params.batFanFeedbackHz = liveData->hexToDecFromResponse(60, 62, 1, false);
+      liveData->params.auxVoltage = liveData->hexToDecFromResponse(62, 64, 1, false) / 10.0;
+
+      float tmpAuxPerc;
+      if(liveData->params.ignitionOn) {
+        tmpAuxPerc = (float)(liveData->params.auxVoltage - 12.8) * 100 / (float)(14.8 - 12.8); //min: 12.8V; max: 14.8V 
+      } else {
+        tmpAuxPerc = (float)(liveData->params.auxVoltage - 11.6) * 100 / (float)(12.8 - 11.6); //min 11.6V; max: 12.8V
+      }
+      if(tmpAuxPerc > 100) {
+        liveData->params.auxPerc = 100;
+      } else if(tmpAuxPerc < 0) {
+        liveData->params.auxPerc = 0;
+      } else {
+        liveData->params.auxPerc = tmpAuxPerc;
+      }
+
       liveData->params.batPowerAmp = - liveData->hexToDecFromResponse(24, 28, 2, true) / 10.0;
       liveData->params.batVoltage = liveData->hexToDecFromResponse(28, 32, 2, false) / 10.0;
       liveData->params.batPowerKw = (liveData->params.batPowerAmp * liveData->params.batVoltage) / 1000.0;
@@ -147,6 +183,12 @@ void CarHyundaiIoniq::parseRowMerged() {
       //liveData->params.batMaxC = liveData->hexToDecFromResponse(32, 34, 1, true);
       //liveData->params.batMinC = liveData->hexToDecFromResponse(34, 36, 1, true);
 
+      tempByte = liveData->hexToDecFromResponse(22, 24, 1, false);
+      liveData->params.chargingOn = (bitRead(tempByte, 5) == 1 || bitRead(tempByte, 6) == 1); // bit 5 = AC; bit 6 = DC
+      if(liveData->params.chargingOn) {
+        liveData->params.lastChargingOnTime = liveData->params.currentTime;
+      }
+
       // This is more accurate than min/max from BMS. It's required to detect kona/eniro cold gates (min 15C is needed > 43kW charging, min 25C is needed > 58kW charging)
       liveData->params.batInletC = liveData->hexToDecFromResponse(48, 50, 1, true);
       if (liveData->params.speedKmh < 10 && liveData->params.batPowerKw >= 1 && liveData->params.socPerc > 0 && liveData->params.socPerc <= 100) {
@@ -249,11 +291,11 @@ void CarHyundaiIoniq::loadTestData() {
   liveData->currentAtshRequest = "ATSH7E2";
   // 2101
   liveData->commandRequest = "2101";
-  liveData->responseRowMerged = "6101FFE0000009211222062F03000000001D7734";
+  liveData->responseRowMerged = "6101FFE0000009215A09061803000000000E773404200000000000";
   parseRowMerged();
   // 2102
   liveData->commandRequest = "2102";
-  liveData->responseRowMerged = "6102FF80000001010000009315B2888D390B08618B683900000000";
+  liveData->responseRowMerged = "6102FF80000001010000009522C570273A0F0D9199953900000000";
   parseRowMerged();
 
   // "ATSH7DF",
@@ -263,52 +305,57 @@ void CarHyundaiIoniq::loadTestData() {
   liveData->currentAtshRequest = "ATSH7B3";
   // 220100
   liveData->commandRequest = "220100";
-  liveData->responseRowMerged = "6201007E5007C8FF8A876A011010FFFF10FF10FFFFFFFFFFFFFFFFFF2EEF767D00FFFF00FFFF000000";
+  liveData->responseRowMerged = "6201007E5007C8FF7A665D00A981FFFF81FF10FFFFFFFFFFFFFFFFFF44CAA7AD00FFFF01FFFF000000";
   parseRowMerged();
   // 220102
   liveData->commandRequest = "220102";
-  liveData->responseRowMerged = "620102FF800000A3950000000000002600000000";
+  liveData->responseRowMerged = "620102FF800000CA5E0101000101005100000000";
   parseRowMerged();
 
   // BMS ATSH7E4
   liveData->currentAtshRequest = "ATSH7E4";
   // 220101
   liveData->commandRequest = "2101";
-  liveData->responseRowMerged = "6101FFFFFFFF5026482648A3FFC30D9E181717171718170019B50FB501000090000142230001425F0000771B00007486007815D809015C0000000003E800";
+  liveData->responseRowMerged = "6101FFFFFFFFBD136826480300220F600B0B0B0B0B0B0B000CCD05CC0A00009100012C4A00012A1800006F37000069F700346CC30D01890000000003E800";
   parseRowMerged();
   // 220102
   liveData->commandRequest = "2102";
-  liveData->responseRowMerged = "6102FFFFFFFFB5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5000000";
+  liveData->responseRowMerged = "6102FFFFFFFFCDCDCDCDCDCDCDCDCDCCCDCDCDCDCDCDCDCDCDCDCDCDCDCDCDCDCDCDCCCDCDCD000000";
   parseRowMerged();
   // 220103
   liveData->commandRequest = "2103";
-  liveData->responseRowMerged = "6103FFFFFFFFB5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5000000";
+  liveData->responseRowMerged = "6103FFFFFFFFCDCDCDCDCDCDCCCDCDCDCDCDCDCDCDCDCCCDCDCCCDCDCDCDCDCDCDCCCDCDCDCC000000";
   parseRowMerged();
   // 220104
   liveData->commandRequest = "2104";
-  liveData->responseRowMerged = "6104FFFFFFFFB5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5B5000000";
+  liveData->responseRowMerged = "6104FFFFFFFFCDCDCDCDCDCDCDCDCDCDCCCCCDCDCCCDCDCDCDCDCDCDCDCDCDCDCDCCCCCCCDCD000000";
   parseRowMerged();
   // 220105
   liveData->commandRequest = "2105";
-  liveData->responseRowMerged = "6105FFFFFFFF00000000001717171817171726482648000150181703E81A03E801520029000000000000000000000000";
-  parseRowMerged();
-  // 220106
-  liveData->commandRequest = "2106";
-  liveData->responseRowMerged = "7F2112"; // n/a on ioniq
+  liveData->responseRowMerged = "6105FFFFFFFF00000000000B0B0B0B0B0B0B136826480001500B0B03E80203E831C60031000000000000000000000000";
   parseRowMerged();
 
   // BCM / TPMS ATSH7A0
   liveData->currentAtshRequest = "ATSH7A0";
   // 22c00b
   liveData->commandRequest = "22c00b";
-  liveData->responseRowMerged = "62C00BFFFF0000B9510100B9510100B84F0100B54F0100AAAAAAAA";
+  liveData->responseRowMerged = "62C00BFFFF0000B73D0100B63D0100B43D0100B53C0100AAAAAAAA";
   parseRowMerged();
 
   // ATSH7C6
   liveData->currentAtshRequest = "ATSH7C6";
   // 22b002
   liveData->commandRequest = "22b002";
-  liveData->responseRowMerged = "62B002E000000000AD003D2D0000000000000000";
+  liveData->responseRowMerged = "62B002E000000000AA003B0B0000000000000000";
+  parseRowMerged();
+
+  //ATSH770
+  liveData->currentAtshRequest = "ATSH770";
+  liveData->commandRequest = "22BC03";
+  liveData->responseRowMerged = "62BC03FDEE3C7300600000AAAA";
+  parseRowMerged();
+  liveData->commandRequest = "22BC06";
+  liveData->responseRowMerged = "62BC06B480000000000000AAAA";
   parseRowMerged();
 
   /*  liveData->params.batModule01TempC = 28;
@@ -377,5 +424,3 @@ void CarHyundaiIoniq::loadTestData() {
   */
 
 }
-
-#endif //CARHYUNDAIIONIQ_CPP
diff --git a/CarHyundaiIoniq.h b/CarHyundaiIoniq.h
index 6a4cb01..6b296f8 100644
--- a/CarHyundaiIoniq.h
+++ b/CarHyundaiIoniq.h
@@ -1,5 +1,4 @@
-#ifndef CARHYUNDAIIONIQ_H
-#define CARHYUNDAIIONIQ_H
+#pragma once
 
 #include "CarInterface.h"
 
@@ -12,5 +11,3 @@ class CarHyundaiIoniq : public CarInterface {
     void parseRowMerged() override;
     void loadTestData() override;   
 };
-
-#endif
diff --git a/CarInterface.cpp b/CarInterface.cpp
index da557df..3787cee 100644
--- a/CarInterface.cpp
+++ b/CarInterface.cpp
@@ -1,6 +1,3 @@
-#ifndef CARINTERFACE_CPP
-#define CARINTERFACE_CPP
-
 #include "CarInterface.h"
 #include "LiveData.h"
 
@@ -19,5 +16,3 @@ void CarInterface::parseRowMerged() {
 void CarInterface::loadTestData() {
   
 }
-
-#endif // CARINTERFACE_CPP
diff --git a/CarKiaDebugObd2.cpp b/CarKiaDebugObd2.cpp
index 8b76ae3..afa140e 100644
--- a/CarKiaDebugObd2.cpp
+++ b/CarKiaDebugObd2.cpp
@@ -1,9 +1,7 @@
-#ifndef CARKIADEBUGOBD2_CPP
-#define CARKIADEBUGOBD2_CPP
-
 #include "CarKiaDebugObd2.h"
+#include <vector>
 
-#define commandQueueCountDebugObd2Kia 256
+//#define commandQueueCountDebugObd2Kia 256
 #define commandQueueLoopFromDebugObd2Kia 8
 
 /**
@@ -11,7 +9,7 @@
 */
 void CarKiaDebugObd2::activateCommandQueue() {
 
-  String commandQueueDebugObd2Kia[commandQueueCountDebugObd2Kia] = {
+  std::vector<String> commandQueueDebugObd2Kia = {
     "AT Z",      // Reset all
     "AT I",      // Print the version ID
     "AT E0",     // Echo off
@@ -222,15 +220,13 @@ void CarKiaDebugObd2::activateCommandQueue() {
   liveData->params.batteryTotalAvailableKWh = 64;
 
   //  Empty and fill command queue
-  for (uint16_t i = 0; i < 300; i++) {
-    liveData->commandQueue[i] = "";
-  }
-  for (uint16_t i = 0; i < commandQueueCountDebugObd2Kia; i++) {
-    liveData->commandQueue[i] = commandQueueDebugObd2Kia[i];
+  liveData->commandQueue.clear();
+  for (auto cmd : commandQueueDebugObd2Kia) {
+    liveData->commandQueue.push_back({ 0, cmd });
   }
 
   liveData->commandQueueLoopFrom = commandQueueLoopFromDebugObd2Kia;
-  liveData->commandQueueCount = commandQueueCountDebugObd2Kia;
+  liveData->commandQueueCount = commandQueueDebugObd2Kia.size();
 }
 
 /**
@@ -516,5 +512,3 @@ void CarKiaDebugObd2::loadTestData() {
   liveData->params.soc10time[0] = liveData->params.soc10time[1] + 900;
 
 }
-
-#endif // CARKIADEBUGOBD2_CPP
diff --git a/CarKiaDebugObd2.h b/CarKiaDebugObd2.h
index 43b4282..b7a12c7 100644
--- a/CarKiaDebugObd2.h
+++ b/CarKiaDebugObd2.h
@@ -1,5 +1,4 @@
-#ifndef CARKIADEBUGOBD2_H
-#define CARKIADEBUGOBD2_H
+#pragma once
 
 #include "CarInterface.h"
 
@@ -12,5 +11,3 @@ class CarKiaDebugObd2 : public CarInterface {
     void parseRowMerged() override;
     void loadTestData() override;  
 };
-
-#endif // CARKIADEBUGOBD2_H
diff --git a/CarKiaEniro.cpp b/CarKiaEniro.cpp
index 012622d..d99e49d 100644
--- a/CarKiaEniro.cpp
+++ b/CarKiaEniro.cpp
@@ -1,15 +1,12 @@
-#ifndef CARKIAENIRO_CPP
-#define CARKIAENIRO_CPP
-
-/* 
- *    eNiro/Kona chargings limits depending on battery temperature (min.value of 01-04 battery module)
+/*
+      eNiro/Kona chargings limits depending on battery temperature (min.value of 01-04 battery module)
   >= 35°C BMS allows max 180A
   >= 25°C without limit (200A)
   >= 15°C BMS allows max 120A
   >= 5°C BMS allows max 90A
   >= 1°C BMS allows max 60A
   <= 0°C BMS allows max 40A
- */
+*/
 
 #include <Arduino.h>
 #include <stdint.h>
@@ -18,16 +15,16 @@
 #include <sys/time.h>
 #include "LiveData.h"
 #include "CarKiaEniro.h"
+#include <vector>
 
-#define commandQueueCountKiaENiro 30
-#define commandQueueLoopFromKiaENiro 10
+#define commandQueueLoopFromKiaENiro 8
 
 /**
- * activateCommandQueue
- */
+   activateCommandQueue
+*/
 void CarKiaEniro::activateCommandQueue() {
 
-  String commandQueueKiaENiro[commandQueueCountKiaENiro] = {
+  std::vector<String> commandQueueKiaENiro = {
     "AT Z",      // Reset all
     "AT I",      // Print the version ID
     "AT S0",     // Printing of spaces on
@@ -45,18 +42,31 @@ void CarKiaEniro::activateCommandQueue() {
 
     // Loop from (KIA ENIRO)
 
-    // ABS / ESP + AHB
-    "ATSH7D1",
-    "22C101",     // brake, park/drive mode
-
     // IGPM
     "ATSH770",
     "22BC03",     // low beam
     "22BC06",     // brake light
 
+    // ABS / ESP + AHB
+    "ATSH7D1",
+    "22C101",     // brake, park/drive mode
+
+    // BCM / TPMS
+    "ATSH7A0",
+    "22c00b",   // tire pressure/temp
+
+    // Aircondition
+    "ATSH7B3",
+    "220100",   // in/out temp
+    "220102",   // coolant temp1, 2
+
+    // CLUSTER MODULE
+    "ATSH7C6",
+    "22B002",   // odo
+
     // VMCU
     "ATSH7E2",
-    "2101",     // speed, ...
+//    "2101",     // speed, ...
     "2102",     // aux, ...
 
     // BMS
@@ -68,102 +78,94 @@ void CarKiaEniro::activateCommandQueue() {
     "220105",   // soh, soc, ..
     "220106",   // cooling water temp
 
-    // Aircondition
-    "ATSH7B3",
-    "220100",   // in/out temp
-    "220102",   // coolant temp1, 2
-
-    // BCM / TPMS
-    "ATSH7A0",
-    "22c00b",   // tire pressure/temp
-
-    // CLUSTER MODULE
-    "ATSH7C6",
-    "22B002",   // odo
-
   };
 
   // 39 or 64 kWh model?
   liveData->params.batModuleTempCount = 4;
   liveData->params.batteryTotalAvailableKWh = 64;
   // =(I18*0,615)*(1+(I18*0,0008)) soc to kwh niro ev 2020
+  // Calculates based on nick.n17 dashboard data
   if (liveData->settings.carType == CAR_KIA_ENIRO_2020_39 || liveData->settings.carType == CAR_HYUNDAI_KONA_2020_39) {
     liveData->params.batteryTotalAvailableKWh = 39.2;
   }
 
   //  Empty and fill command queue
-  for (int i = 0; i < 300; i++) {
-    liveData->commandQueue[i] = "";
-  }
-  for (int i = 0; i < commandQueueCountKiaENiro; i++) {
-    liveData->commandQueue[i] = commandQueueKiaENiro[i];
+  liveData->commandQueue.clear();
+  //for (int i = 0; i < commandQueueCountKiaENiro; i++) {
+  for (auto cmd : commandQueueKiaENiro) {
+    liveData->commandQueue.push_back({ 0, cmd }); // stxChar not used, keep it 0
   }
 
   liveData->commandQueueLoopFrom = commandQueueLoopFromKiaENiro;
-  liveData->commandQueueCount = commandQueueCountKiaENiro;
+  liveData->commandQueueCount = commandQueueKiaENiro.size();
 }
 
 /**
- * parseRowMerged
- */
+   parseRowMerged
+*/
 void CarKiaEniro::parseRowMerged() {
 
-  bool tempByte;
+  uint8_t tempByte;
   float tempFloat;
   String tmpStr;
 
+  // IGPM
+  // RESPONDING WHEN CAR IS OFF
+  if (liveData->currentAtshRequest.equals("ATSH770")) {
+    if (liveData->commandRequest.equals("22BC03")) {
+      // 
+      tempByte = liveData->hexToDecFromResponse(14, 16, 1, false);
+      liveData->params.hoodDoorOpen = (bitRead(tempByte, 7) == 1);
+      liveData->params.leftFrontDoorOpen = (bitRead(tempByte, 5) == 1);
+      liveData->params.rightFrontDoorOpen = (bitRead(tempByte, 0) == 1);
+      liveData->params.leftRearDoorOpen  = (bitRead(tempByte, 4) == 1);
+      liveData->params.rightRearDoorOpen = (bitRead(tempByte, 2) == 1);
+      //
+      tempByte = liveData->hexToDecFromResponse(16, 18, 1, false);
+      liveData->params.ignitionOn = (bitRead(tempByte, 5) == 1);
+      liveData->params.trunkDoorOpen  = (bitRead(tempByte, 0) == 1);
+      if (liveData->params.ignitionOn) {
+        liveData->params.lastIgnitionOnTime = liveData->params.currentTime;
+      }
+
+      tempByte = liveData->hexToDecFromResponse(18, 20, 1, false);
+      liveData->params.headLights = (bitRead(tempByte, 5) == 1);
+      liveData->params.dayLights = (bitRead(tempByte, 3) == 1);
+    }
+    if (liveData->commandRequest.equals("22BC06")) {
+      tempByte = liveData->hexToDecFromResponse(14, 16, 1, false);
+      liveData->params.brakeLights = (bitRead(tempByte, 5) == 1);
+    }
+  }
+
   // ABS / ESP + AHB 7D1
+  // RESPONDING WHEN CAR IS OFF
   if (liveData->currentAtshRequest.equals("ATSH7D1")) {
     if (liveData->commandRequest.equals("22C101")) {
       uint8_t driveMode = liveData->hexToDecFromResponse(22, 24, 1, false);
       liveData->params.forwardDriveMode = (driveMode == 4);
       liveData->params.reverseDriveMode = (driveMode == 2);
       liveData->params.parkModeOrNeutral  = (driveMode == 1);
+      // Speed
+      liveData->params.speedKmh = liveData->hexToDecFromResponse(18, 20, 2, false); 
     }
   }
 
-  // IGPM
-  if (liveData->currentAtshRequest.equals("ATSH770")) {
-    if (liveData->commandRequest.equals("22BC03")) {
-      tempByte = liveData->hexToDecFromResponse(16, 18, 1, false);
-      liveData->params.ignitionOnPrevious = liveData->params.ignitionOn;
-      liveData->params.ignitionOn = (bitRead(tempByte, 5) == 1);
-      if (liveData->params.ignitionOnPrevious && !liveData->params.ignitionOn)
-        liveData->params.automaticShutdownTimer = liveData->params.currentTime;
-
-      liveData->params.lightInfo = liveData->hexToDecFromResponse(18, 20, 1, false);
-      liveData->params.headLights = (bitRead(liveData->params.lightInfo, 5) == 1);
-      liveData->params.dayLights = (bitRead(liveData->params.lightInfo, 3) == 1);
-    }
-    if (liveData->commandRequest.equals("22BC06")) {
-      liveData->params.brakeLightInfo = liveData->hexToDecFromResponse(14, 16, 1, false);
-      liveData->params.brakeLights = (bitRead(liveData->params.brakeLightInfo, 5) == 1);
+  // TPMS 7A0
+  if (liveData->currentAtshRequest.equals("ATSH7A0")) {
+    if (liveData->commandRequest.equals("22c00b")) {
+      liveData->params.tireFrontLeftPressureBar = liveData->hexToDecFromResponse(14, 16, 2, false) / 72.51886900361;     // === OK Valid *0.2 / 14.503773800722
+      liveData->params.tireFrontRightPressureBar = liveData->hexToDecFromResponse(22, 24, 2, false) / 72.51886900361;     // === OK Valid *0.2 / 14.503773800722
+      liveData->params.tireRearRightPressureBar = liveData->hexToDecFromResponse(30, 32, 2, false) / 72.51886900361;    // === OK Valid *0.2 / 14.503773800722
+      liveData->params.tireRearLeftPressureBar = liveData->hexToDecFromResponse(38, 40, 2, false) / 72.51886900361;     // === OK Valid *0.2 / 14.503773800722
+      liveData->params.tireFrontLeftTempC = liveData->hexToDecFromResponse(16, 18, 2, false)  - 50;      // === OK Valid
+      liveData->params.tireFrontRightTempC = liveData->hexToDecFromResponse(24, 26, 2, false) - 50;      // === OK Valid
+      liveData->params.tireRearRightTempC = liveData->hexToDecFromResponse(32, 34, 2, false) - 50;     // === OK Valid
+      liveData->params.tireRearLeftTempC = liveData->hexToDecFromResponse(40, 42, 2, false) - 50;     // === OK Valid
     }
   }
 
-  // VMCU 7E2
-  if (liveData->currentAtshRequest.equals("ATSH7E2")) {
-    if (liveData->commandRequest.equals("2101")) {
-      liveData->params.speedKmh = liveData->hexToDecFromResponse(32, 36, 2, false) * 0.0155; // / 100.0 *1.609 = real to gps is 1.750
-      if (liveData->params.speedKmh < -99 || liveData->params.speedKmh > 200)
-        liveData->params.speedKmh = 0;
-    }
-    if (liveData->commandRequest.equals("2102")) {
-      liveData->params.auxPerc = liveData->hexToDecFromResponse(50, 52, 1, false);
-      liveData->params.auxCurrentAmp = - liveData->hexToDecFromResponse(46, 50, 2, true) / 1000.0;
-    }
-  }
-
-  // Cluster module 7c6
-  if (liveData->currentAtshRequest.equals("ATSH7C6")) {
-    if (liveData->commandRequest.equals("22B002")) {
-      tempFloat = liveData->params.odoKm;
-      liveData->params.odoKm = liveData->decFromResponse(18, 24);
-      //if (tempFloat != liveData->params.odoKm) liveData->params.sdcardCanNotify = true;
-    }
-  }
-
-  // Aircon 7b3
+  // Aircon 7B3
   if (liveData->currentAtshRequest.equals("ATSH7B3")) {
     if (liveData->commandRequest.equals("220100")) {
       liveData->params.indoorTemperature = (liveData->hexToDecFromResponse(16, 18, 1, false) / 2) - 40;
@@ -175,6 +177,28 @@ void CarKiaEniro::parseRowMerged() {
     }
   }
 
+  // Cluster module 7C6
+  if (liveData->currentAtshRequest.equals("ATSH7C6")) {
+    if (liveData->commandRequest.equals("22B002")) {
+      tempFloat = liveData->params.odoKm;
+      liveData->params.odoKm = liveData->decFromResponse(18, 24);
+      //if (tempFloat != liveData->params.odoKm) liveData->params.sdcardCanNotify = true;
+    }
+  }
+
+  // VMCU 7E2
+  if (liveData->currentAtshRequest.equals("ATSH7E2")) {
+    /*if (liveData->commandRequest.equals("2101")) {
+      liveData->params.speedKmh = liveData->hexToDecFromResponse(32, 36, 2, false) * 0.0155; // / 100.0 *1.609 = real to gps is 1.750
+      if (liveData->params.speedKmh < -99 || liveData->params.speedKmh > 200)
+        liveData->params.speedKmh = 0;
+    }*/
+    if (liveData->commandRequest.equals("2102")) {
+      liveData->params.auxCurrentAmp = - liveData->hexToDecFromResponse(46, 50, 2, true) / 1000.0;
+      liveData->params.auxPerc = liveData->hexToDecFromResponse(50, 52, 1, false);
+    }
+  }
+
   // BMS 7e4
   if (liveData->currentAtshRequest.equals("ATSH7E4")) {
     if (liveData->commandRequest.equals("220101")) {
@@ -188,15 +212,15 @@ void CarKiaEniro::parseRowMerged() {
       liveData->params.availableChargePower = liveData->decFromResponse(16, 20) / 100.0;
       liveData->params.availableDischargePower = liveData->decFromResponse(20, 24) / 100.0;
       //liveData->params.isolationResistanceKOhm = liveData->hexToDecFromResponse(118, 122, 2, true);
-      liveData->params.batFanStatus = liveData->hexToDecFromResponse(60, 62, 2, true);
-      liveData->params.batFanFeedbackHz = liveData->hexToDecFromResponse(62, 64, 2, true);
-      liveData->params.auxVoltage = liveData->hexToDecFromResponse(64, 66, 2, true) / 10.0;
+      liveData->params.batFanStatus = liveData->hexToDecFromResponse(60, 62, 1, false);
+      liveData->params.batFanFeedbackHz = liveData->hexToDecFromResponse(62, 64, 1, false);
       liveData->params.batPowerAmp = - liveData->hexToDecFromResponse(26, 30, 2, true) / 10.0;
       liveData->params.batVoltage = liveData->hexToDecFromResponse(30, 34, 2, false) / 10.0;
       liveData->params.batPowerKw = (liveData->params.batPowerAmp * liveData->params.batVoltage) / 1000.0;
       if (liveData->params.batPowerKw < 0) // Reset charging start time
         liveData->params.chargingStartTime = liveData->params.currentTime;
       liveData->params.batPowerKwh100 = liveData->params.batPowerKw / liveData->params.speedKmh * 100;
+      liveData->params.auxVoltage = liveData->hexToDecFromResponse(64, 66, 1, false) / 10.0;
       liveData->params.batCellMaxV = liveData->hexToDecFromResponse(52, 54, 1, false) / 50.0;
       liveData->params.batCellMinV = liveData->hexToDecFromResponse(56, 58, 1, false) / 50.0;
       liveData->params.batModuleTempC[0] = liveData->hexToDecFromResponse(38, 40, 1, true);
@@ -208,6 +232,15 @@ void CarKiaEniro::parseRowMerged() {
       //liveData->params.batMaxC = liveData->hexToDecFromResponse(34, 36, 1, true);
       //liveData->params.batMinC = liveData->hexToDecFromResponse(36, 38, 1, true);
 
+      // Ignition Off/on
+      // tempByte = liveData->hexToDecFromResponse(106, 108, 1, false);
+      // liveData->params.chargingOn = (bitRead(tempByte, 2) == 1);
+      tempByte = liveData->hexToDecFromResponse(24, 26, 1, false);
+      liveData->params.chargingOn = (bitRead(tempByte, 5) == 1 || bitRead(tempByte, 6) == 1); // bit 5 = AC; bit 6 = DC
+      if(liveData->params.chargingOn) {
+        liveData->params.lastChargingOnTime = liveData->params.currentTime;
+      }
+   
       // This is more accurate than min/max from BMS. It's required to detect kona/eniro cold gates (min 15C is needed > 43kW charging, min 25C is needed > 58kW charging)
       liveData->params.batMinC = liveData->params.batMaxC = liveData->params.batModuleTempC[0];
       for (uint16_t i = 1; i < liveData->params.batModuleTempCount; i++) {
@@ -278,7 +311,7 @@ void CarKiaEniro::parseRowMerged() {
     }
     // BMS 7e4
     if (liveData->commandRequest.equals("220106")) {
-      liveData->params.coolingWaterTempC = liveData->hexToDecFromResponse(14, 16, 1, false);
+      liveData->params.coolingWaterTempC = liveData->hexToDecFromResponse(14, 16, 1, true);
       liveData->params.bmsUnknownTempC = liveData->hexToDecFromResponse(18, 20, 1, true);
       liveData->params.bmsUnknownTempD = liveData->hexToDecFromResponse(46, 48, 1, true);
       // log 220106 to sdcard
@@ -286,25 +319,11 @@ void CarKiaEniro::parseRowMerged() {
       tmpStr.toCharArray(liveData->params.debugData2, tmpStr.length() + 1);
     }
   }
-
-  // TPMS 7a0
-  if (liveData->currentAtshRequest.equals("ATSH7A0")) {
-    if (liveData->commandRequest.equals("22c00b")) {
-      liveData->params.tireFrontLeftPressureBar = liveData->hexToDecFromResponse(14, 16, 2, false) / 72.51886900361;     // === OK Valid *0.2 / 14.503773800722
-      liveData->params.tireFrontRightPressureBar = liveData->hexToDecFromResponse(22, 24, 2, false) / 72.51886900361;     // === OK Valid *0.2 / 14.503773800722
-      liveData->params.tireRearRightPressureBar = liveData->hexToDecFromResponse(30, 32, 2, false) / 72.51886900361;    // === OK Valid *0.2 / 14.503773800722
-      liveData->params.tireRearLeftPressureBar = liveData->hexToDecFromResponse(38, 40, 2, false) / 72.51886900361;     // === OK Valid *0.2 / 14.503773800722
-      liveData->params.tireFrontLeftTempC = liveData->hexToDecFromResponse(16, 18, 2, false)  - 50;      // === OK Valid
-      liveData->params.tireFrontRightTempC = liveData->hexToDecFromResponse(24, 26, 2, false) - 50;      // === OK Valid
-      liveData->params.tireRearRightTempC = liveData->hexToDecFromResponse(32, 34, 2, false) - 50;     // === OK Valid
-      liveData->params.tireRearLeftTempC = liveData->hexToDecFromResponse(40, 42, 2, false) - 50;     // === OK Valid
-    }
-  }
 }
 
 /**
- * loadTestData
- */
+   loadTestData
+*/
 void CarKiaEniro::loadTestData() {
 
   // IGPM
@@ -454,5 +473,3 @@ void CarKiaEniro::loadTestData() {
   liveData->params.soc10time[0] = liveData->params.soc10time[1] + 900;
 
 }
-
-#endif // CARKIAENIRO_CPP
diff --git a/CarKiaEniro.h b/CarKiaEniro.h
index b7f8033..9b1b92f 100644
--- a/CarKiaEniro.h
+++ b/CarKiaEniro.h
@@ -1,5 +1,4 @@
-#ifndef CARKIAENIRO_H
-#define CARKIAENIRO_H
+#pragma once
 
 #include "CarInterface.h"
 
@@ -12,5 +11,3 @@ class CarKiaEniro : public CarInterface {
     void parseRowMerged() override;
     void loadTestData() override;  
 };
-
-#endif // CARKIAENIRO_H
diff --git a/CarKiaNiroPhev.cpp b/CarKiaNiroPhev.cpp
index 414f255..2308ef5 100644
--- a/CarKiaNiroPhev.cpp
+++ b/CarKiaNiroPhev.cpp
@@ -1,9 +1,6 @@
-#ifndef CARKIANIROPHEV_CPP
-#define CARKIANIROPHEV_CPP
-
 #include "CarKiaNiroPhev.h"
+#include <vector>
 
-#define commandQueueCountKiaNiroPhev 25
 #define commandQueueLoopFromKiaNiroPhev 8
 
 /**
@@ -11,7 +8,7 @@
 */
 void CarKiaNiroPhev::activateCommandQueue() {
 
-  String commandQueueKiaNiroPhev[commandQueueCountKiaNiroPhev] = {
+  std::vector<String> commandQueueKiaNiroPhev = {
     "AT Z",      // Reset all
     "AT I",      // Print the version ID
     "AT E0",     // Echo off
@@ -67,15 +64,13 @@ void CarKiaNiroPhev::activateCommandQueue() {
   liveData->params.batModuleTempCount = 5;
 
   //  Empty and fill command queue
-  for (int i = 0; i < 300; i++) {
-    liveData->commandQueue[i] = "";
-  }
-  for (int i = 0; i < commandQueueCountKiaNiroPhev; i++) {
-    liveData->commandQueue[i] = commandQueueKiaNiroPhev[i];
+  liveData->commandQueue.clear();
+  for (auto cmd : commandQueueKiaNiroPhev) {
+    liveData->commandQueue.push_back({ 0, cmd });
   }
 
   liveData->commandQueueLoopFrom = commandQueueLoopFromKiaNiroPhev;
-  liveData->commandQueueCount = commandQueueCountKiaNiroPhev;
+  liveData->commandQueueCount = commandQueueKiaNiroPhev.size();
 }
 
 /**
@@ -376,5 +371,3 @@ void CarKiaNiroPhev::loadTestData() {
   */
 
 }
-
-#endif //CARKIANIROPHEV_CPP
diff --git a/CarKiaNiroPhev.h b/CarKiaNiroPhev.h
index 7c580a9..6fd7a80 100644
--- a/CarKiaNiroPhev.h
+++ b/CarKiaNiroPhev.h
@@ -1,5 +1,4 @@
-#ifndef CARKIANIROPHEV_H
-#define CARKIANIROPHEV_H
+#pragma once
 
 #include "CarInterface.h"
 
@@ -12,5 +11,3 @@ class CarKiaNiroPhev: public CarInterface {
     void parseRowMerged() override;
     void loadTestData() override;   
 };
-
-#endif
diff --git a/CarRenaultZoe.cpp b/CarRenaultZoe.cpp
index e200a48..1e3311b 100644
--- a/CarRenaultZoe.cpp
+++ b/CarRenaultZoe.cpp
@@ -1,6 +1,3 @@
-#ifndef CARRENAULTZOE_CPP
-#define CARRENAULTZOE_CPP
-
 #include <Arduino.h>
 #include <stdint.h>
 #include <WString.h>
@@ -8,16 +5,16 @@
 #include <sys/time.h>
 #include "LiveData.h"
 #include "CarRenaultZoe.h"
+#include <vector>
 
-#define commandQueueCountRenaultZoe 18
-#define commandQueueLoopFromRenaultZoe 11
+#define commandQueueLoopFromRenaultZoe 8
 
 /**
    activateCommandQueue
 */
 void CarRenaultZoe::activateCommandQueue() {
 
-  String commandQueueRenaultZoe[commandQueueCountRenaultZoe] = {
+  std::vector<String> commandQueueRenaultZoe = {
     "AT Z",      // Reset all
     "AT I",      // Print the version ID
     "AT S0",     // Printing of spaces on
@@ -32,36 +29,97 @@ void CarRenaultZoe::activateCommandQueue() {
     ////"AT AT0",     // disabled adaptive timing
     "AT DP",
     "AT ST16",    // reduced timeout to 1, orig.16
-    "atfcsd300010",
-    "atfcsm1",    // Allow long messages
 
     // Loop from (RENAULT ZOE)
 
     // LBC Lithium battery controller
     "ATSH79B",
     "ATFCSH79B",
-    "2101",
-    "2103",
-    "2104",
-    "2141",
-    "2142",
-    "2161",
+    "atfcsd300010",
+    "atfcsm1",
+    "221415",
+    "2101", // 034 61011383138600000000000000000000000009970D620FC920D0000005420000000000000008D80500000B202927100000000000000000
+    "2103", // 01D 6103018516A717240000000001850185000000FFFF07D00516E60000030000000000
+    "2104", // 04D 6104099A37098D37098F3709903709AC3609BB3609A136098B37099737098A37098437099437FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF363637000000000000
+    "2141", // 07e 61410F360F380F380F360F380F380F380F380F390F390F3A0F390F3A0F390F390F380F380F390F380F380F380F390F390F380F380F3A0F390F380F380F380F390F390F350F380F360F380F380F380F380F360F380F360F350F380F360F380F360F360F350F360F360F380F380F380F3A0F380F3A0F3A0F390F390F380F36000000000000
+    "2142", // 04a 61420F3A0F390F390F390F380F380F390F390F390F390F360F360F380F380F360F380F360F380F390F390F3A0F390F390F3A0F3A0F3A0F380F390F390F3A0F390F390F380F38922091F20000
+    "2161", // 014 6161000AA820C8C8C8C2C20001545800004696FF
+
+    // CLUSTER Instrument panel
+    "ATSH743",
+    "ATFCSH743",
+    "atfcsd300010",
+    "atfcsm1",
+    //    "220201", // 62020175300168
+    //    "220202", // 62020274710123
+    //    "220203"- "220205", // 7F2212
+    "220206", // 62020600015459
+    //"222204", // temp ext.
+
+    // BCB 793 Battery Connection Box
+    //    "ATSH792",
+    //    "ATFCSH792",
+    //    "atfcsd300010",
+    //    "atfcsm1",
+    //    "223101", to     "223114", // all with negative 7F2212*/
+
+    // CLIM 764 CLIMATE CONTROL
+    "ATSH744",
+    "ATFCSH744",
+    "atfcsd300010",
+    "atfcsm1",
+    "2143",
+    //    "2180", // NO DATA
+    //    "2181", // NO DATA
+    //"2182", // 618038303139520430343239353031520602051523080201008815
+    //    "2125", // 6125000000000000000000000000000000000000
+    //    "2126", // NO DATA
+    //    "2128", // NO DATA
+
+    // EVC 7ec El vehicle controler
+    "ATSH7E4",
+    "ATFCSH7E4",
+    "atfcsd300010",
+    "atfcsm1",
+    //    "222001", // 62200136
+    //    "222002", // 6220020B3D
+    "222003", // 6220030000
+    //    "222004", // 62200402ED
+    //    "222005", // 6220050532
+    //    "222006", // 622006015459
+
+    // PEB 77e Power Electronics Bloc
+    //    "ATSH75A",
+    //    "ATFCSH75A",
+    //    "atfcsd300010",
+    //    "atfcsm1",
+    //    "223009", // 6230093640
+
+    // UBP 7bc Uncoupled Braking Pedal
+    //    "ATSH79C",
+    //    "ATFCSH79C",
+    //    "atfcsd300010",
+    //    "atfcsm1",
+    //    "21F0", // 61F0303235315204303337333733325215160C0400000101008800
+    //    "21F1", // 61F10000000000F000000000F0000000000012061400005C91F600
+    //    "21FE", // 61FE333731325204303337333733325215160C0400010201008800
+
   };
 
   //
   liveData->params.batModuleTempCount = 12; // 24, 12 is display limit
-  liveData->params.batteryTotalAvailableKWh = 28;
+  liveData->params.batteryTotalAvailableKWh = 22;
+  // usable 22, total 26
+  
 
   //  Empty and fill command queue
-  for (int i = 0; i < 300; i++) {
-    liveData->commandQueue[i] = "";
-  }
-  for (int i = 0; i < commandQueueCountRenaultZoe; i++) {
-    liveData->commandQueue[i] = commandQueueRenaultZoe[i];
+  liveData->commandQueue.clear();
+  for (auto cmd : commandQueueRenaultZoe) {
+    liveData->commandQueue.push_back({ 0, cmd });
   }
 
   liveData->commandQueueLoopFrom = commandQueueLoopFromRenaultZoe;
-  liveData->commandQueueCount = commandQueueCountRenaultZoe;
+  liveData->commandQueueCount = commandQueueRenaultZoe.size();
 }
 
 /**
@@ -69,13 +127,19 @@ void CarRenaultZoe::activateCommandQueue() {
 */
 void CarRenaultZoe::parseRowMerged() {
 
-  bool tempByte;
+  uint8_t tempByte;
 
   // LBC 79B
   if (liveData->currentAtshRequest.equals("ATSH79B")) {
+    if (liveData->commandRequest.equals("221415")) {
+      liveData->params.batVoltage = liveData->hexToDecFromResponse(6, 8, 2, false);
+    }
     if (liveData->commandRequest.equals("2101")) {
       liveData->params.batPowerAmp = liveData->hexToDecFromResponse(4, 8, 2, false) - 5000;
       liveData->params.batPowerKw = (liveData->params.batPowerAmp * liveData->params.batVoltage) / 1000.0;
+      if (liveData->params.batPowerKw < 0) // Reset charging start time
+        liveData->params.chargingStartTime = liveData->params.currentTime;
+      liveData->params.batPowerKwh100 = liveData->params.batPowerKw / liveData->params.speedKmh * 100;
       liveData->params.auxVoltage = liveData->hexToDecFromResponse(56, 60, 2, false) / 100.0;
       liveData->params.availableChargePower = liveData->hexToDecFromResponse(84, 88, 2, false) / 100.0;
       liveData->params.batCellMinV = liveData->hexToDecFromResponse(24, 28, 2, false) / 100.0;
@@ -92,6 +156,14 @@ void CarRenaultZoe::parseRowMerged() {
       for (uint16_t i = 12; i < 24; i++) {
         liveData->params.batModuleTempC[i] = liveData->hexToDecFromResponse(80 + ((i - 12) * 6), 82 + ((i - 12) * 6), 1, false) - 40;
       }
+      liveData->params.batMinC = liveData->params.batMaxC = liveData->params.batModuleTempC[0];
+      for (uint16_t i = 1; i < 24; i++) {
+        if (liveData->params.batModuleTempC[i] < liveData->params.batMinC)
+          liveData->params.batMinC = liveData->params.batModuleTempC[i];
+        if (liveData->params.batModuleTempC[i] > liveData->params.batMaxC)
+          liveData->params.batMaxC = liveData->params.batModuleTempC[i];
+      }
+      liveData->params.batTempC = liveData->params.batMinC;
     }
     if (liveData->commandRequest.equals("2141")) {
       for (int i = 0; i < 62; i++) {
@@ -108,178 +180,99 @@ void CarRenaultZoe::parseRowMerged() {
     }
   }
 
-
-  /* niro
-    // ABS / ESP + AHB 7D1
-    if (liveData->currentAtshRequest.equals("ATSH7D1")) {
-      if (liveData->commandRequest.equals("22C101")) {
-        uint8_t driveMode = liveData->hexToDecFromResponse(22, 24, 1, false);
-        liveData->params.forwardDriveMode = (driveMode == 4);
-        liveData->params.reverseDriveMode = (driveMode == 2);
-        liveData->params.parkModeOrNeutral  = (driveMode == 1);
-      }
+  // CLUSTER 743
+  if (liveData->currentAtshRequest.equals("ATSH743")) {
+    if (liveData->commandRequest.equals("220206")) {
+      liveData->params.odoKm = liveData->hexToDecFromResponse(6, 14, 4, false);
     }
+  }
 
-    // IGPM
-    if (liveData->currentAtshRequest.equals("ATSH770")) {
-      if (liveData->commandRequest.equals("22BC03")) {
-        tempByte = liveData->hexToDecFromResponse(16, 18, 1, false);
-        liveData->params.ignitionOnPrevious = liveData->params.ignitionOn;
-        liveData->params.ignitionOn = (bitRead(tempByte, 5) == 1);
-        if (liveData->params.ignitionOnPrevious && !liveData->params.ignitionOn)
-          liveData->params.automaticShutdownTimer = liveData->params.currentTime;
-
-        liveData->params.lightInfo = liveData->hexToDecFromResponse(18, 20, 1, false);
-        liveData->params.headLights = (bitRead(liveData->params.lightInfo, 5) == 1);
-        liveData->params.dayLights = (bitRead(liveData->params.lightInfo, 3) == 1);
-      }
-      if (liveData->commandRequest.equals("22BC06")) {
-        liveData->params.brakeLightInfo = liveData->hexToDecFromResponse(14, 16, 1, false);
-        liveData->params.brakeLights = (bitRead(liveData->params.brakeLightInfo, 5) == 1);
-      }
+  // CLUSTER ATSH7E4
+  if (liveData->currentAtshRequest.equals("ATSH7E4")) {
+    if (liveData->commandRequest.equals("222003")) {
+      liveData->params.speedKmh = liveData->hexToDecFromResponse(6, 8, 2, false) / 100;
+      if (liveData->params.speedKmh < -99 || liveData->params.speedKmh > 200)
+        liveData->params.speedKmh = 0;
     }
+  }
 
-    // VMCU 7E2
-    if (liveData->currentAtshRequest.equals("ATSH7E2")) {
-      if (liveData->commandRequest.equals("2101")) {
-        liveData->params.speedKmh = liveData->hexToDecFromResponse(32, 36, 2, false) * 0.0155; // / 100.0 *1.609 = real to gps is 1.750
-        if (liveData->params.speedKmh < -99 || liveData->params.speedKmh > 200)
-          liveData->params.speedKmh = 0;
-      }
-      if (liveData->commandRequest.equals("2102")) {
-        liveData->params.auxPerc = liveData->hexToDecFromResponse(50, 52, 1, false);
-        liveData->params.auxCurrentAmp = - liveData->hexToDecFromResponse(46, 50, 2, true) / 1000.0;
-      }
+  // CLIM 744 CLIMATE CONTROL
+  if (liveData->currentAtshRequest.equals("ATSH744")) {
+    if (liveData->commandRequest.equals("2143")) {
+      liveData->params.outdoorTemperature = (liveData->hexToDecFromResponse(26, 28, 1, false)) - 40;
+      //liveData->params.indoorTemperature = (liveData->hexToDecFromResponse(16, 18, 1, false) / 2) - 40;
+      //liveData->params.coolantTemp1C = (liveData->hexToDecFromResponse(14, 16, 1, false) / 2) - 40;
+      //liveData->params.coolantTemp2C = (liveData->hexToDecFromResponse(16, 18, 1, false) / 2) - 40;
     }
+  }
 
-    // Cluster module 7c6
-    if (liveData->currentAtshRequest.equals("ATSH7C6")) {
-      if (liveData->commandRequest.equals("22B002")) {
-        liveData->params.odoKm = liveData->decFromResponse(18, 24);
-      }
-    }
+  /*uint8_t driveMode = liveData->hexToDecFromResponse(22, 24, 1, false);
+             liveData->params.forwardDriveMode = (driveMode == 4);
+             liveData->params.reverseDriveMode = (driveMode == 2);
+             liveData->params.parkModeOrNeutral  = (driveMode == 1);
 
-    // Aircon 7b3
-    if (liveData->currentAtshRequest.equals("ATSH7B3")) {
-      if (liveData->commandRequest.equals("220100")) {
-        liveData->params.indoorTemperature = (liveData->hexToDecFromResponse(16, 18, 1, false) / 2) - 40;
-        liveData->params.outdoorTemperature = (liveData->hexToDecFromResponse(18, 20, 1, false) / 2) - 40;
-      }
-      if (liveData->commandRequest.equals("220102") && liveData->responseRowMerged.substring(12, 14) == "00") {
-        liveData->params.coolantTemp1C = (liveData->hexToDecFromResponse(14, 16, 1, false) / 2) - 40;
-        liveData->params.coolantTemp2C = (liveData->hexToDecFromResponse(16, 18, 1, false) / 2) - 40;
-      }
-    }
+         // IGPM
+             tempByte = liveData->hexToDecFromResponse(16, 18, 1, false);
+             liveData->params.ignitionOnPrevious = liveData->params.ignitionOn;
+             liveData->params.ignitionOn = (bitRead(tempByte, 5) == 1);
+             if (liveData->params.ignitionOnPrevious && !liveData->params.ignitionOn)
+               liveData->params.automaticShutdownTimer = liveData->params.currentTime;
+             liveData->params.lightInfo = liveData->hexToDecFromResponse(18, 20, 1, false);
+             liveData->params.headLights = (bitRead(liveData->params.lightInfo, 5) == 1);
+             liveData->params.dayLights = (bitRead(liveData->params.lightInfo, 3) == 1);
+             liveData->params.brakeLightInfo = liveData->hexToDecFromResponse(14, 16, 1, false);
+             liveData->params.brakeLights = (bitRead(liveData->params.brakeLightInfo, 5) == 1);
+             liveData->params.auxPerc = liveData->hexToDecFromResponse(50, 52, 1, false);
+             liveData->params.auxCurrentAmp = - liveData->hexToDecFromResponse(46, 50, 2, true) / 1000.0;
+             liveData->params.cumulativeEnergyChargedKWh = liveData->decFromResponse(82, 90) / 10.0;
+             if (liveData->params.cumulativeEnergyChargedKWhStart == -1)
+               liveData->params.cumulativeEnergyChargedKWhStart = liveData->params.cumulativeEnergyChargedKWh;
+             liveData->params.cumulativeEnergyDischargedKWh = liveData->decFromResponse(90, 98) / 10.0;
+             if (liveData->params.cumulativeEnergyDischargedKWhStart == -1)
+               liveData->params.cumulativeEnergyDischargedKWhStart = liveData->params.cumulativeEnergyDischargedKWh;
+             liveData->params.availableDischargePower = liveData->decFromResponse(20, 24) / 100.0;
+             //liveData->params.isolationResistanceKOhm = liveData->hexToDecFromResponse(118, 122, 2, true);
+             liveData->params.batFanStatus = liveData->hexToDecFromResponse(60, 62, 2, true);
+             liveData->params.batFanFeedbackHz = liveData->hexToDecFromResponse(62, 64, 2, true);
+             liveData->params.motorRpm = liveData->hexToDecFromResponse(112, 116, 2, false);
+             // This is more accurate than min/max from BMS. It's required to detect kona/eniro cold gates (min 15C is needed > 43kW charging, min 25C is needed > 58kW charging)
+             liveData->params.batInletC = liveData->hexToDecFromResponse(50, 52, 1, true);
+             liveData->params.bmsUnknownTempA = liveData->hexToDecFromResponse(30, 32, 1, true);
+             liveData->params.batHeaterC = liveData->hexToDecFromResponse(52, 54, 1, true);
+             liveData->params.bmsUnknownTempB = liveData->hexToDecFromResponse(82, 84, 1, true);
+             liveData->params.coolingWaterTempC = liveData->hexToDecFromResponse(14, 16, 1, false);
+             liveData->params.bmsUnknownTempC = liveData->hexToDecFromResponse(18, 20, 1, true);
+             liveData->params.bmsUnknownTempD = liveData->hexToDecFromResponse(46, 48, 1, true);
+             liveData->params.tireFrontLeftPressureBar = liveData->hexToDecFromResponse(14, 16, 2, false) / 72.51886900361;     // === OK Valid *0.2 / 14.503773800722
+             liveData->params.tireFrontRightPressureBar = liveData->hexToDecFromResponse(22, 24, 2, false) / 72.51886900361;     // === OK Valid *0.2 / 14.503773800722
+             liveData->params.tireRearRightPressureBar = liveData->hexToDecFromResponse(30, 32, 2, false) / 72.51886900361;    // === OK Valid *0.2 / 14.503773800722
+             liveData->params.tireRearLeftPressureBar = liveData->hexToDecFromResponse(38, 40, 2, false) / 72.51886900361;     // === OK Valid *0.2 / 14.503773800722
+             liveData->params.tireFrontLeftTempC = liveData->hexToDecFromResponse(16, 18, 2, false)  - 50;      // === OK Valid
+             liveData->params.tireFrontRightTempC = liveData->hexToDecFromResponse(24, 26, 2, false) - 50;      // === OK Valid
+             liveData->params.tireRearRightTempC = liveData->hexToDecFromResponse(32, 34, 2, false) - 50;     // === OK Valid
+             liveData->params.tireRearLeftTempC = liveData->hexToDecFromResponse(40, 42, 2, false) - 50;     // === OK Valid
 
-    // BMS 7e4
-    if (liveData->currentAtshRequest.equals("ATSH7E4")) {
-      if (liveData->commandRequest.equals("220101")) {
-        liveData->params.cumulativeEnergyChargedKWh = liveData->decFromResponse(82, 90) / 10.0;
-        if (liveData->params.cumulativeEnergyChargedKWhStart == -1)
-          liveData->params.cumulativeEnergyChargedKWhStart = liveData->params.cumulativeEnergyChargedKWh;
-        liveData->params.cumulativeEnergyDischargedKWh = liveData->decFromResponse(90, 98) / 10.0;
-        if (liveData->params.cumulativeEnergyDischargedKWhStart == -1)
-          liveData->params.cumulativeEnergyDischargedKWhStart = liveData->params.cumulativeEnergyDischargedKWh;
-        liveData->params.availableDischargePower = liveData->decFromResponse(20, 24) / 100.0;
-        //liveData->params.isolationResistanceKOhm = liveData->hexToDecFromResponse(118, 122, 2, true);
-        liveData->params.batFanStatus = liveData->hexToDecFromResponse(60, 62, 2, true);
-        liveData->params.batFanFeedbackHz = liveData->hexToDecFromResponse(62, 64, 2, true);
-        liveData->params.auxVoltage = liveData->hexToDecFromResponse(64, 66, 2, true) / 10.0;
-        liveData->params.batVoltage = liveData->hexToDecFromResponse(30, 34, 2, false) / 10.0;
-        if (liveData->params.batPowerKw < 0) // Reset charging start time
-          liveData->params.chargingStartTime = liveData->params.currentTime;
-        liveData->params.batPowerKwh100 = liveData->params.batPowerKw / liveData->params.speedKmh * 100;
-        liveData->params.batModuleTempC[0] = liveData->hexToDecFromResponse(38, 40, 1, true);
-        liveData->params.batModuleTempC[1] = liveData->hexToDecFromResponse(40, 42, 1, true);
-        liveData->params.batModuleTempC[2] = liveData->hexToDecFromResponse(42, 44, 1, true);
-        liveData->params.batModuleTempC[3] = liveData->hexToDecFromResponse(44, 46, 1, true);
-        liveData->params.motorRpm = liveData->hexToDecFromResponse(112, 116, 2, false);
-
-        // This is more accurate than min/max from BMS. It's required to detect kona/eniro cold gates (min 15C is needed > 43kW charging, min 25C is needed > 58kW charging)
-        liveData->params.batMinC = liveData->params.batMaxC = liveData->params.batModuleTempC[0];
-        for (uint16_t i = 1; i < liveData->params.batModuleTempCount; i++) {
-          if (liveData->params.batModuleTempC[i] < liveData->params.batMinC)
-            liveData->params.batMinC = liveData->params.batModuleTempC[i];
-          if (liveData->params.batModuleTempC[i] > liveData->params.batMaxC)
-            liveData->params.batMaxC = liveData->params.batModuleTempC[i];
-        }
-        liveData->params.batTempC = liveData->params.batMinC;
-
-        liveData->params.batInletC = liveData->hexToDecFromResponse(50, 52, 1, true);
-        if (liveData->params.speedKmh < 10 && liveData->params.batPowerKw >= 1 && liveData->params.socPerc > 0 && liveData->params.socPerc <= 100) {
-          if ( liveData->params.chargingGraphMinKw[int(liveData->params.socPerc)] < 0 || liveData->params.batPowerKw < liveData->params.chargingGraphMinKw[int(liveData->params.socPerc)])
-            liveData->params.chargingGraphMinKw[int(liveData->params.socPerc)] = liveData->params.batPowerKw;
-          if ( liveData->params.chargingGraphMaxKw[int(liveData->params.socPerc)] < 0 || liveData->params.batPowerKw > liveData->params.chargingGraphMaxKw[int(liveData->params.socPerc)])
-            liveData->params.chargingGraphMaxKw[int(liveData->params.socPerc)] = liveData->params.batPowerKw;
-          liveData->params.chargingGraphBatMinTempC[int(liveData->params.socPerc)] = liveData->params.batMinC;
-          liveData->params.chargingGraphBatMaxTempC[int(liveData->params.socPerc)] = liveData->params.batMaxC;
-          liveData->params.chargingGraphHeaterTempC[int(liveData->params.socPerc)] = liveData->params.batHeaterC;
-          liveData->params.chargingGraphWaterCoolantTempC[int(liveData->params.socPerc)] = liveData->params.coolingWaterTempC;
-        }
-      }
-      // BMS 7e4
-      if (liveData->commandRequest.equals("220102") && liveData->responseRowMerged.substring(12, 14) == "FF") {
-        for (int i = 0; i < 32; i++) {
-          liveData->params.cellVoltage[i] = liveData->hexToDecFromResponse(14 + (i * 2), 14 + (i * 2) + 2, 1, false) / 50;
-        }
-      }
-      // BMS 7e4
-      if (liveData->commandRequest.equals("220103")) {
-        for (int i = 0; i < 32; i++) {
-          liveData->params.cellVoltage[32 + i] = liveData->hexToDecFromResponse(14 + (i * 2), 14 + (i * 2) + 2, 1, false) / 50;
-        }
-      }
-      // BMS 7e4
-      if (liveData->commandRequest.equals("220104")) {
-        for (int i = 0; i < 32; i++) {
-          liveData->params.cellVoltage[64 + i] = liveData->hexToDecFromResponse(14 + (i * 2), 14 + (i * 2) + 2, 1, false) / 50;
-        }
-      }
-      // BMS 7e4
-      if (liveData->commandRequest.equals("220105")) {
-        liveData->params.socPercPrevious = liveData->params.socPerc;
-        liveData->params.sohPerc = liveData->hexToDecFromResponse(56, 60, 2, false) / 10.0;
-        liveData->params.socPerc = liveData->hexToDecFromResponse(68, 70, 1, false) / 2.0;
-
-        // Soc10ced table, record x0% CEC/CED table (ex. 90%->89%, 80%->79%)
-        if (liveData->params.socPercPrevious - liveData->params.socPerc > 0) {
-          byte index = (int(liveData->params.socPerc) == 4) ? 0 : (int)(liveData->params.socPerc / 10) + 1;
-          if ((int(liveData->params.socPerc) % 10 == 9 || int(liveData->params.socPerc) == 4) && liveData->params.soc10ced[index] == -1) {
-            liveData->params.soc10ced[index] = liveData->params.cumulativeEnergyDischargedKWh;
-            liveData->params.soc10cec[index] = liveData->params.cumulativeEnergyChargedKWh;
-            liveData->params.soc10odo[index] = liveData->params.odoKm;
-            liveData->params.soc10time[index] = liveData->params.currentTime;
-          }
-        }
-        liveData->params.bmsUnknownTempA = liveData->hexToDecFromResponse(30, 32, 1, true);
-        liveData->params.batHeaterC = liveData->hexToDecFromResponse(52, 54, 1, true);
-        liveData->params.bmsUnknownTempB = liveData->hexToDecFromResponse(82, 84, 1, true);
-        //
-        for (int i = 30; i < 32; i++) { // ai/aj position
-          liveData->params.cellVoltage[96 - 30 + i] = liveData->hexToDecFromResponse(14 + (i * 2), 14 + (i * 2) + 2, 1, false) / 50;
-        }
-      }
-      // BMS 7e4
-      if (liveData->commandRequest.equals("220106")) {
-        liveData->params.coolingWaterTempC = liveData->hexToDecFromResponse(14, 16, 1, false);
-        liveData->params.bmsUnknownTempC = liveData->hexToDecFromResponse(18, 20, 1, true);
-        liveData->params.bmsUnknownTempD = liveData->hexToDecFromResponse(46, 48, 1, true);
-      }
-    }
-
-    // TPMS 7a0
-    if (liveData->currentAtshRequest.equals("ATSH7A0")) {
-      if (liveData->commandRequest.equals("22c00b")) {
-        liveData->params.tireFrontLeftPressureBar = liveData->hexToDecFromResponse(14, 16, 2, false) / 72.51886900361;     // === OK Valid *0.2 / 14.503773800722
-        liveData->params.tireFrontRightPressureBar = liveData->hexToDecFromResponse(22, 24, 2, false) / 72.51886900361;     // === OK Valid *0.2 / 14.503773800722
-        liveData->params.tireRearRightPressureBar = liveData->hexToDecFromResponse(30, 32, 2, false) / 72.51886900361;    // === OK Valid *0.2 / 14.503773800722
-        liveData->params.tireRearLeftPressureBar = liveData->hexToDecFromResponse(38, 40, 2, false) / 72.51886900361;     // === OK Valid *0.2 / 14.503773800722
-        liveData->params.tireFrontLeftTempC = liveData->hexToDecFromResponse(16, 18, 2, false)  - 50;      // === OK Valid
-        liveData->params.tireFrontRightTempC = liveData->hexToDecFromResponse(24, 26, 2, false) - 50;      // === OK Valid
-        liveData->params.tireRearRightTempC = liveData->hexToDecFromResponse(32, 34, 2, false) - 50;     // === OK Valid
-        liveData->params.tireRearLeftTempC = liveData->hexToDecFromResponse(40, 42, 2, false) - 50;     // === OK Valid
-      }
-    }
+                        if (liveData->params.speedKmh < 10 && liveData->params.batPowerKw >= 1 && liveData->params.socPerc > 0 && liveData->params.socPerc <= 100) {
+               if ( liveData->params.chargingGraphMinKw[int(liveData->params.socPerc)] < 0 || liveData->params.batPowerKw < liveData->params.chargingGraphMinKw[int(liveData->params.socPerc)])
+                 liveData->params.chargingGraphMinKw[int(liveData->params.socPerc)] = liveData->params.batPowerKw;
+               if ( liveData->params.chargingGraphMaxKw[int(liveData->params.socPerc)] < 0 || liveData->params.batPowerKw > liveData->params.chargingGraphMaxKw[int(liveData->params.socPerc)])
+                 liveData->params.chargingGraphMaxKw[int(liveData->params.socPerc)] = liveData->params.batPowerKw;
+               liveData->params.chargingGraphBatMinTempC[int(liveData->params.socPerc)] = liveData->params.batMinC;
+               liveData->params.chargingGraphBatMaxTempC[int(liveData->params.socPerc)] = liveData->params.batMaxC;
+               liveData->params.chargingGraphHeaterTempC[int(liveData->params.socPerc)] = liveData->params.batHeaterC;
+               liveData->params.chargingGraphWaterCoolantTempC[int(liveData->params.socPerc)] = liveData->params.coolingWaterTempC;
+             }
+           }
+// BMS 7e4
+             if (liveData->params.socPercPrevious - liveData->params.socPerc > 0) {
+               byte index = (int(liveData->params.socPerc) == 4) ? 0 : (int)(liveData->params.socPerc / 10) + 1;
+               if ((int(liveData->params.socPerc) % 10 == 9 || int(liveData->params.socPerc) == 4) && liveData->params.soc10ced[index] == -1) {
+                 liveData->params.soc10ced[index] = liveData->params.cumulativeEnergyDischargedKWh;
+                 liveData->params.soc10cec[index] = liveData->params.cumulativeEnergyChargedKWh;
+                 liveData->params.soc10odo[index] = liveData->params.odoKm;
+                 liveData->params.soc10time[index] = liveData->params.currentTime;
+               }
+             }
   */
 }
 
@@ -309,6 +302,11 @@ void CarRenaultZoe::loadTestData() {
   liveData->responseRowMerged = "6161000AA820C8C8C8C2C2000153B400004669FF";
   parseRowMerged();
 
+  // CLUSTER 743
+  liveData->currentAtshRequest = "ATSH743";
+  liveData->commandRequest = "220206";
+  liveData->responseRowMerged = "62020600015459";
+  parseRowMerged();
 
   /*
        niro
@@ -459,5 +457,3 @@ void CarRenaultZoe::loadTestData() {
     liveData->params.soc10time[0] = liveData->params.soc10time[1] + 900;
   */
 }
-
-#endif // CARRENAULTZOE_CPP
diff --git a/CarRenaultZoe.h b/CarRenaultZoe.h
index 75458c3..dae1cc9 100644
--- a/CarRenaultZoe.h
+++ b/CarRenaultZoe.h
@@ -1,5 +1,4 @@
-#ifndef CARRENAULTZOE_H
-#define CARRENAULTZOE_H
+#pragma once
 
 #include "CarInterface.h"
 
@@ -12,5 +11,3 @@ class CarRenaultZoe : public CarInterface {
     void parseRowMerged() override;
     void loadTestData() override;  
 };
-
-#endif // CARRENAULTZOE_H
diff --git a/CommInterface.cpp b/CommInterface.cpp
index f143257..b91fbfd 100644
--- a/CommInterface.cpp
+++ b/CommInterface.cpp
@@ -1,13 +1,88 @@
-#ifndef COMMINTERFACE_CPP
-#define COMMINTERFACE_CPP
-
 #include "CommInterface.h"
-//#include "BoardInterface.h"
+#include "BoardInterface.h"
+//#include "CarInterface.h"
 #include "LiveData.h"
 
-void CommInterface::initComm(LiveData* pLiveData/*, BoardInterface* pBoard*/) {
-    liveData = pLiveData;   
-    //board = pBoard;   
-}
+/**
+ * 
+ */
+void CommInterface::initComm(LiveData* pLiveData, BoardInterface* pBoard) {
     
-#endif // COMMINTERFACE_CPP
+    liveData = pLiveData;   
+    board = pBoard; 
+    response = "";  
+}
+
+/**
+ * Main loop
+ */
+void CommInterface::mainLoop() {
+  
+  // Send command from TTY to OBD2
+  if (syslog->available()) {
+    ch = syslog->read();
+    if (ch == '\r' || ch == '\n') {
+      board->customConsoleCommand(response);
+      response = response + ch;
+      syslog->info(DEBUG_COMM, response);
+      executeCommand(response);
+      response = "";
+    } else {
+      response = response + ch;
+    }
+  }
+
+  // Drop ChargingOn when status was not updated for more than 10 seconds
+  if(liveData->params.currentTime - liveData->params.lastChargingOnTime > 10 && liveData->params.chargingOn) {
+    liveData->params.chargingOn = false;
+  }
+
+  // Can send next command from queue to OBD
+  if (liveData->canSendNextAtCommand) {
+    liveData->canSendNextAtCommand = false;
+    doNextQueueCommand();
+  }
+}
+
+/**
+  Do next AT command from queue
+ */
+bool CommInterface::doNextQueueCommand() { 
+
+  // Restart loop with AT commands
+  if (liveData->commandQueueIndex >= liveData->commandQueueCount) {
+    liveData->commandQueueIndex = liveData->commandQueueLoopFrom;
+    board->redrawScreen();
+  
+    // log every queue loop (temp) TODO and seconds interval
+    liveData->params.sdcardCanNotify = true;
+  }
+
+  // Send AT command to obd
+  liveData->commandRequest = liveData->commandQueue[liveData->commandQueueIndex].request;
+  liveData->commandStartChar = liveData->commandQueue[liveData->commandQueueIndex].startChar; // TODO: add to struct?
+  
+  if (liveData->commandRequest.startsWith("ATSH")) {
+    liveData->currentAtshRequest = liveData->commandRequest;
+  }
+
+  syslog->infoNolf(DEBUG_COMM, ">>> ");
+  syslog->info(DEBUG_COMM, liveData->commandRequest);
+  liveData->responseRowMerged = "";  
+  executeCommand(liveData->commandRequest);
+  liveData->commandQueueIndex++;
+}
+
+/**
+  Parse result from OBD, merge frames to sigle response
+ */
+bool CommInterface::parseResponse() {
+  
+  // 1 frame data
+  syslog->info(DEBUG_COMM, liveData->responseRow);
+
+  // Merge frames 0:xxxx 1:yyyy 2:zzzz to single response xxxxyyyyzzzz string
+  if (liveData->responseRow.length() >= 2 && liveData->responseRow.charAt(1) == ':') {
+    liveData->responseRowMerged += liveData->responseRow.substring(2);
+  }
+}
diff --git a/CommInterface.h b/CommInterface.h
index 076eb7f..f09361f 100644
--- a/CommInterface.h
+++ b/CommInterface.h
@@ -1,19 +1,26 @@
-#ifndef COMMINTERFACE_H
-#define COMMINTERFACE_H
+#pragma once
 
 #include "LiveData.h"
 //#include "BoardInterface.h"
 
+class BoardInterface; // Forward declaration
+
 class CommInterface {
   
   protected:
     LiveData* liveData;   
-    //BoardInterface* board;   
+    BoardInterface* board;   
+    char ch;
+    String response;
+    time_t lastDataSent;
   public:
-    void initComm(LiveData* pLiveData/*, BoardInterface* pBoard**/);
+    void initComm(LiveData* pLiveData, BoardInterface* pBoard);
     virtual void connectDevice() = 0;
     virtual void disconnectDevice() = 0;
     virtual void scanDevices() = 0;
+    virtual void mainLoop();
+    virtual void executeCommand(String cmd) = 0;
+    // 
+    bool doNextQueueCommand();    
+    bool parseResponse();
 };
-
-#endif // COMMINTERFACE_H
diff --git a/CommObd2Ble4.cpp b/CommObd2Ble4.cpp
index b1f191c..9f3c3cb 100644
--- a/CommObd2Ble4.cpp
+++ b/CommObd2Ble4.cpp
@@ -1,29 +1,360 @@
-#ifndef COMMOBD2BLE4_CPP
-#define COMMOBD2BLE4_CPP
-
 #include <BLEDevice.h>
 #include "CommObd2Ble4.h"
+#include "BoardInterface.h"
 #include "LiveData.h"
 
+CommObd2Ble4* commObj;
+BoardInterface* boardObj;
+LiveData* liveDataObj;
+
 /**
- * Connect ble4 adapter
- */
+  BLE callbacks
+*/
+class MyClientCallback : public BLEClientCallbacks {
+
+    // On BLE connect
+    void onConnect(BLEClient* pclient) {
+      syslog->println("onConnect");
+    }
+
+    // On BLE disconnect
+    void onDisconnect(BLEClient* pclient) {
+      liveDataObj->commConnected = false;
+      syslog->println("onDisconnect");
+
+      boardObj->displayMessage("BLE disconnected", "");
+    }
+};
+
+/**
+   Scan for BLE servers and find the first one that advertises the service we are looking for.
+*/
+class MyAdvertisedDeviceCallbacks: public BLEAdvertisedDeviceCallbacks {
+
+    // Called for each advertising BLE server.
+    void onResult(BLEAdvertisedDevice advertisedDevice) {
+
+      syslog->print("BLE advertised device found: ");
+      syslog->println(advertisedDevice.toString().c_str());
+      syslog->println(advertisedDevice.getAddress().toString().c_str());
+
+      // Add to device list (max. 9 devices allowed yet)
+      String tmpStr;
+
+      if (liveDataObj->scanningDeviceIndex < 10) { // && advertisedDevice.haveServiceUUID()
+        for (uint16_t i = 0; i < liveDataObj->menuItemsCount; ++i) {
+          if (liveDataObj->menuItems[i].id == 10001 + liveDataObj->scanningDeviceIndex) {
+            tmpStr = advertisedDevice.toString().c_str();
+            tmpStr.replace("Name: ", "");
+            tmpStr.replace("Address: ", "");
+            tmpStr.toCharArray(liveDataObj->menuItems[i].title, 48);
+            tmpStr = advertisedDevice.getAddress().toString().c_str();
+            tmpStr.toCharArray(liveDataObj->menuItems[i].obdMacAddress, 18);
+          }
+        }
+        liveDataObj->scanningDeviceIndex++;
+      }
+
+      //        if (advertisedDevice.getServiceDataUUID().toString() != "<NULL>") {
+      //          syslog->print("ServiceDataUUID: ");
+      //          syslog->println(advertisedDevice.getServiceDataUUID().toString().c_str());
+      //        if (advertisedDevice.getServiceUUID().toString() != "<NULL>") {
+      //          syslog->print("ServiceUUID: ");
+      //          syslog->println(advertisedDevice.getServiceUUID().toString().c_str());
+      //        }
+      //        }
+
+      if (advertisedDevice.haveServiceUUID() && advertisedDevice.isAdvertisingService(BLEUUID(liveDataObj->settings.serviceUUID)) &&
+          (strcmp(advertisedDevice.getAddress().toString().c_str(), liveDataObj->settings.obdMacAddress) == 0)) {
+        syslog->println("Stop scanning. Found my BLE device.");
+        BLEDevice::getScan()->stop();
+        liveDataObj->foundMyBleDevice = new BLEAdvertisedDevice(advertisedDevice);
+      }
+    }
+};
+
+uint32_t PIN = 1234;
+
+/**
+  BLE Security
+*/
+class MySecurity : public BLESecurityCallbacks {
+
+    uint32_t onPassKeyRequest() {
+      syslog->printf("Pairing password: %d \r\n", PIN);
+      return PIN;
+    }
+
+    void onPassKeyNotify(uint32_t pass_key) {
+      syslog->printf("onPassKeyNotify\r\n");
+    }
+
+    bool onConfirmPIN(uint32_t pass_key) {
+      syslog->printf("onConfirmPIN\r\n");
+      return true;
+    }
+
+    bool onSecurityRequest() {
+      syslog->printf("onSecurityRequest\r\n");
+      return true;
+    }
+
+    void onAuthenticationComplete(esp_ble_auth_cmpl_t auth_cmpl) {
+      if (auth_cmpl.success) {
+        syslog->printf("onAuthenticationComplete\r\n");
+      } else {
+        syslog->println("Auth failure. Incorrect PIN?");
+        liveDataObj->bleConnect = false;
+      }
+    }
+
+};
+
+/**
+   Ble notification callback
+*/
+static void notifyCallback (BLERemoteCharacteristic * pBLERemoteCharacteristic, uint8_t* pData, size_t length, bool isNotify) {
+
+  char ch;
+
+  // Parse multiframes to single response
+  liveDataObj->responseRow = "";
+  for (int i = 0; i <= length; i++) {
+    ch = pData[i];
+    if (ch == '\r'  || ch == '\n' || ch == '\0') {
+      if (liveDataObj->responseRow != "")
+        commObj->parseResponse();
+      liveDataObj->responseRow = "";
+    } else {
+      liveDataObj->responseRow += ch;
+      if (liveDataObj->responseRow == ">") {
+        if (liveDataObj->responseRowMerged != "") {
+          syslog->infoNolf(DEBUG_COMM, "merged:");
+          syslog->info(DEBUG_COMM, liveDataObj->responseRowMerged);
+          boardObj->parseRowMerged();
+        }
+        liveDataObj->responseRowMerged = "";
+        liveDataObj->canSendNextAtCommand = true;
+      }
+    }
+  }
+}
+
+/**
+   Connect ble4 adapter
+*/
 void CommObd2Ble4::connectDevice() {
-  Serial.println("COMM connectDevice");
-}
 
-/** 
- * Disconnect device
- */
-void CommObd2Ble4::disconnectDevice() {
-  Serial.println("COMM disconnectDevice");
+  commObj = this;
+  liveDataObj = liveData;
+  boardObj = board;
+
+  syslog->println("BLE4 connectDevice");
+
+  // Start BLE connection
+  ESP_ERROR_CHECK(esp_bt_controller_mem_release(ESP_BT_MODE_CLASSIC_BT));
+  BLEDevice::init("");
+
+  // Retrieve a Scanner and set the callback we want to use to be informed when we have detected a new device.
+  // Specify that we want active scanning and start the scan to run for 10 seconds.
+  syslog->println("Setup BLE scan");
+  liveData->pBLEScan = BLEDevice::getScan();
+  liveData->pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks());
+  liveData->pBLEScan->setInterval(1349);
+  liveData->pBLEScan->setWindow(449);
+  liveData->pBLEScan->setActiveScan(true);
+
+  // Skip BLE scan if middle button pressed
+  if (strcmp(liveData->settings.obdMacAddress, "00:00:00:00:00:00") != 0 && !board->skipAdapterScan()) {
+    syslog->println(liveData->settings.obdMacAddress);
+    startBleScan();
+  }
 }
 
 /**
- * Scan device list
- */
-void CommObd2Ble4::scanDevices() {
-  Serial.println("COMM scanDevices");
+   Disconnect device
+*/
+void CommObd2Ble4::disconnectDevice() {
+
+  syslog->println("COMM disconnectDevice");
+  btStop();
 }
 
-#endif // COMMOBD2BLE4_CPP
+/**
+   Scan device list, from menu
+*/
+void CommObd2Ble4::scanDevices() {
+
+  syslog->println("COMM scanDevices");
+  startBleScan();
+}
+
+///////////////////////////////////
+
+/**
+   Start ble scan
+*/
+void CommObd2Ble4::startBleScan() {
+
+  liveData->foundMyBleDevice = NULL;
+  liveData->scanningDeviceIndex = 0;
+  board->displayMessage(" > Scanning BLE4 devices", "40sec.or hold middle&RST");
+
+  // Start scanning
+  syslog->println("Scanning BLE devices...");
+  syslog->print("Looking for ");
+  syslog->println(liveData->settings.obdMacAddress);
+  BLEScanResults foundDevices = liveData->pBLEScan->start(40, false);
+  syslog->print("Devices found: ");
+  syslog->println(foundDevices.getCount());
+  syslog->println("Scan done!");
+  liveData->pBLEScan->clearResults(); // delete results fromBLEScan buffer to release memory
+
+  char tmpStr1[20];
+  sprintf(tmpStr1, "Found %d devices", foundDevices.getCount());
+  board->displayMessage(" > Scanning BLE4 devices", tmpStr1);
+
+  // Scan devices from menu, show list of devices
+ if (liveData->menuCurrent == 9999) {
+    syslog->println("Display menu with devices");
+    liveData->menuVisible = true;
+    //liveData->menuCurrent = 9999;
+    liveData->menuItemSelected = 0;
+    board->showMenu();
+  } else {
+    // Redraw screen
+    if (liveData->foundMyBleDevice == NULL) {
+      board->displayMessage("Device not found", "Middle button - menu");
+    } else {
+      board->redrawScreen();
+    }
+  }
+}
+
+/**
+   Do connect BLE with server (OBD device)
+*/
+bool CommObd2Ble4::connectToServer(BLEAddress pAddress) {
+
+  board->displayMessage(" > Connecting device", "");
+
+  syslog->print("liveData->bleConnect ");
+  syslog->println(pAddress.toString().c_str());
+  board->displayMessage(" > Connecting device - init", pAddress.toString().c_str());
+
+  BLEDevice::setEncryptionLevel(ESP_BLE_SEC_ENCRYPT);
+  BLEDevice::setSecurityCallbacks(new MySecurity());
+
+  BLESecurity *pSecurity = new BLESecurity();
+  pSecurity->setAuthenticationMode(ESP_LE_AUTH_BOND); //
+  pSecurity->setCapability(ESP_IO_CAP_KBDISP);
+  pSecurity->setRespEncryptionKey(ESP_BLE_ENC_KEY_MASK | ESP_BLE_ID_KEY_MASK);
+
+  board->displayMessage(" > Connecting device", pAddress.toString().c_str());
+  liveData->pClient = BLEDevice::createClient();
+  liveData->pClient->setClientCallbacks(new MyClientCallback());
+  if (liveData->pClient->connect(pAddress, BLE_ADDR_TYPE_RANDOM) ) syslog->println("liveData->bleConnected");
+  syslog->println(" - liveData->bleConnected to server");
+
+  // Remote service
+  board->displayMessage(" > Connecting device", "Connecting service...");
+  BLERemoteService* pRemoteService = liveData->pClient->getService(BLEUUID(liveData->settings.serviceUUID));
+  if (pRemoteService == nullptr)
+  {
+    syslog->print("Failed to find our service UUID: ");
+    syslog->println(liveData->settings.serviceUUID);
+    board->displayMessage(" > Connecting device", "Unable to find service");
+    return false;
+  }
+  syslog->println(" - Found our service");
+
+  // Get characteristics
+  board->displayMessage(" > Connecting device", "Connecting TxUUID...");
+  liveData->pRemoteCharacteristic = pRemoteService->getCharacteristic(BLEUUID(liveData->settings.charTxUUID));
+  if (liveData->pRemoteCharacteristic == nullptr) {
+    syslog->print("Failed to find our characteristic UUID: ");
+    syslog->println(liveData->settings.charTxUUID);//.toString().c_str());
+    board->displayMessage(" > Connecting device", "Unable to find TxUUID");
+    return false;
+  }
+  syslog->println(" - Found our characteristic");
+
+  // Get characteristics
+  board->displayMessage(" > Connecting device", "Connecting RxUUID...");
+  liveData->pRemoteCharacteristicWrite = pRemoteService->getCharacteristic(BLEUUID(liveData->settings.charRxUUID));
+  if (liveData->pRemoteCharacteristicWrite == nullptr) {
+    syslog->print("Failed to find our characteristic UUID: ");
+    syslog->println(liveData->settings.charRxUUID);//.toString().c_str());
+    board->displayMessage(" > Connecting device", "Unable to find RxUUID");
+    return false;
+  }
+  syslog->println(" - Found our characteristic write");
+
+  board->displayMessage(" > Connecting device", "Register callbacks...");
+  // Read the value of the characteristic.
+  if (liveData->pRemoteCharacteristic->canNotify()) {
+    syslog->println(" - canNotify");
+    if (liveData->pRemoteCharacteristic->canIndicate()) {
+      syslog->println(" - canIndicate");
+      const uint8_t indicationOn[] = {0x2, 0x0};
+      //const uint8_t indicationOff[] = {0x0,0x0};
+      liveData->pRemoteCharacteristic->getDescriptor(BLEUUID((uint16_t)0x2902))->writeValue((uint8_t*)indicationOn, 2, true);
+      //liveData->pRemoteCharacteristic->getDescriptor(BLEUUID((uint16_t)0x2902))->writeValue((uint8_t*)notifyOff,2,true);
+      liveData->pRemoteCharacteristic->registerForNotify(notifyCallback, false);
+      delay(200);
+    }
+  }
+
+  board->displayMessage(" > Connecting device", "Done...");
+  if (liveData->pRemoteCharacteristicWrite->canWrite()) {
+    syslog->println(" - canWrite");
+  }
+
+  return true;
+}
+
+/**
+   Main loop
+*/
+void CommObd2Ble4::mainLoop() {
+
+  // Connect BLE device
+  if (liveData->bleConnect == true && liveData->foundMyBleDevice != NULL) {
+    liveData->pServerAddress = new BLEAddress(liveData->settings.obdMacAddress);
+    if (connectToServer(*liveData->pServerAddress)) {
+
+      liveData->commConnected = true;
+      liveData->bleConnect = false;
+
+      syslog->println("We are now connected to the BLE device.");
+
+      // Print message
+      board->displayMessage(" > Processing init AT cmds", "");
+
+      // Serve first command (ATZ)
+      doNextQueueCommand();
+      
+    } else {
+      syslog->println("We have failed to connect to the server; there is nothing more we will do.");
+    }
+  }
+
+  // Parent declaration
+  CommInterface::mainLoop();
+
+  if (board->scanDevices) {
+    board->scanDevices = false;
+    startBleScan();
+  }
+}
+
+/**
+ * Send command
+ */
+void CommObd2Ble4::executeCommand(String cmd) {
+
+  String tmpStr = cmd + "\r";
+  if (liveData->commConnected) {
+    liveData->pRemoteCharacteristicWrite->writeValue(tmpStr.c_str(), tmpStr.length());
+  }
+}
diff --git a/CommObd2Ble4.h b/CommObd2Ble4.h
index 6681ae9..4d08aba 100644
--- a/CommObd2Ble4.h
+++ b/CommObd2Ble4.h
@@ -1,17 +1,21 @@
-#ifndef COMMOBD2BLE4_H
-#define COMMOBD2BLE4_H
+#pragma once
 
+#include <BLEDevice.h>
 #include "LiveData.h"
 #include "CommInterface.h"
 
 class CommObd2Ble4 : public CommInterface {
-  
+
   protected:
     uint32_t PIN = 1234;
   public:
     void connectDevice() override;
     void disconnectDevice() override;
     void scanDevices() override;
+    void mainLoop() override;
+    void executeCommand(String cmd) override;
+    //
+    void startBleScan();
+    bool connectToServer(BLEAddress pAddress);
+    //
 };
-
-#endif // COMMOBD2BLE4_H
diff --git a/CommObd2Can.cpp b/CommObd2Can.cpp
index dea253e..b26e5d1 100644
--- a/CommObd2Can.cpp
+++ b/CommObd2Can.cpp
@@ -1,7 +1,539 @@
-#ifndef COMMINTERFACE_CPP
-#define COMMINTERFACE_CPP
-
-#include "CommInterface.h"
+#include "CommObd2CAN.h"
+#include "BoardInterface.h"
 #include "LiveData.h"
+#include <mcp_CAN.h>
 
-#endif // COMMINTERFACE_CPP
+//#include <string.h>
+
+/**
+   Connect CAN adapter
+*/
+void CommObd2Can::connectDevice() {
+
+  syslog->println("CAN connectDevice");
+
+  //CAN = new MCP_CAN(pinCanCs); // todo: remove if smart pointer is ok
+  CAN.reset(new MCP_CAN(pinCanCs)); // smart pointer so it's automatically cleaned when out of context and also free to re-init
+  if (CAN == nullptr) {
+    syslog->println("Error: Not enough memory to instantiate CAN class");
+    syslog->println("init_can() failed");
+    return;
+  }
+
+  // Initialize MCP2515 running at 16MHz with a baudrate of 500kb/s and the masks and filters disabled.
+  if (CAN->begin(MCP_STDEXT, CAN_500KBPS, MCP_8MHZ) == CAN_OK) {
+    syslog->println("MCP2515 Initialized Successfully!");
+    board->displayMessage(" > CAN init OK", "");
+  } else {
+    syslog->println("Error Initializing MCP2515...");
+    board->displayMessage(" > CAN init failed", "");
+    return;
+  }
+
+  if (liveData->settings.carType == CAR_BMW_I3_2014) {
+    //initialise mask and filter to allow only receipt of 0x7xx CAN IDs
+    CAN->init_Mask(0, 0, 0x07000000);              // Init first mask...
+    CAN->init_Mask(1, 0, 0x07000000);              // Init second mask...
+    for (uint8_t i = 0; i < 6; ++i) {
+      CAN->init_Filt(i, 0, 0x06000000);           //Init filters
+    }
+  }
+
+  if (MCP2515_OK != CAN->setMode(MCP_NORMAL)) {  // Set operation mode to normal so the MCP2515 sends acks to received data.
+    syslog->println("Error: CAN->setMode(MCP_NORMAL) failed");
+    board->displayMessage(" > CAN init failed", "");
+    return;
+  }
+
+  pinMode(pinCanInt, INPUT);                    // Configuring pin for /INT input
+
+  // Serve first command (ATZ)
+  liveData->commConnected = true;  
+  doNextQueueCommand();
+
+  syslog->println("init_can() done");
+}
+
+/**
+   Disconnect device
+*/
+void CommObd2Can::disconnectDevice() {
+
+  liveData->commConnected = false;  
+  // CAN->setMode(MCP_SLEEP); 
+  syslog->println("COMM disconnectDevice");
+}
+
+/**
+   Scan device list, from menu
+*/
+void CommObd2Can::scanDevices() {
+
+  syslog->println("COMM scanDevices");
+}
+
+/**
+   Main loop
+*/
+void CommObd2Can::mainLoop() {
+
+  CommInterface::mainLoop();
+
+  // if delay between commands is defined, check if this delay is not expired 
+  if (liveData->delayBetweenCommandsMs != 0) {
+    if (bResponseProcessed && (unsigned long)(millis() - lastDataSent) > liveData->delayBetweenCommandsMs) {
+      bResponseProcessed = false;
+      liveData->canSendNextAtCommand = true;
+      return;
+    }
+  }
+
+  // Read data
+  const uint8_t firstByte = receivePID();
+  if ((firstByte & 0xf0) == 0x10) { // First frame, request another
+    sendFlowControlFrame();
+    delay(10);
+    for (uint16_t i = 0; i < 1000; i++) {
+      receivePID();
+      if (rxRemaining <= 2)
+        break;
+      delay(1);
+      // apply timeout for next frames loop too
+      if (lastDataSent != 0 && (unsigned long)(millis() - lastDataSent) > liveData->rxTimeoutMs) {
+        syslog->info(DEBUG_COMM, "CAN execution timeout (multiframe message).");
+        break;
+      }
+    }
+    // Process incomplette messages
+    if (liveData->responseRowMerged.length() > 7) {
+      processMergedResponse();
+      return;
+    }
+  }
+  if (lastDataSent != 0 && (unsigned long)(millis() - lastDataSent) > liveData->rxTimeoutMs) {
+    syslog->info(DEBUG_COMM, "CAN execution timeout. Continue with next command.");
+    liveData->canSendNextAtCommand = true;
+    return;
+  }
+}
+
+/**
+   Send command to CAN bus
+*/
+void CommObd2Can::executeCommand(String cmd) {
+
+  syslog->infoNolf(DEBUG_COMM, "executeCommand ");
+  syslog->info(DEBUG_COMM, cmd);
+
+  if (cmd.equals("") || cmd.startsWith("AT")) { // skip AT commands as not used by direct CAN connection
+    lastDataSent = 0;
+    liveData->canSendNextAtCommand = true;
+    return;
+  }
+
+  // Send command
+  liveData->responseRowMerged = "";
+  liveData->currentAtshRequest.replace(" ", ""); // remove possible spaces
+  String atsh = "0" + liveData->currentAtshRequest.substring(4); // remove ATSH
+  cmd.replace(" ", ""); // remove possible spaces
+  sendPID(liveData->hexToDec(atsh, 2, false), cmd);
+  delay(40);
+}
+
+/**
+   Send PID
+   remark: parameter cmd as const reference to aviod copying
+*/
+void CommObd2Can::sendPID(const uint16_t pid, const String& cmd) {
+
+  uint8_t txBuf[8] = { 0 }; // init with zeroes
+  String tmpStr;
+
+  if (liveData->bAdditionalStartingChar)
+  {
+    struct Packet_t
+    {
+      uint8_t startChar;
+      uint8_t length;
+      uint8_t payload[6];
+    };
+    
+    Packet_t* pPacket = (Packet_t*)txBuf;
+    pPacket->startChar = liveData->commandStartChar; // todo: handle similar way as cmd input param?
+    pPacket->length = cmd.length() / 2;
+    
+    for (uint8_t i = 0; i < sizeof(pPacket->payload); i++) {
+      tmpStr = cmd;
+      tmpStr = tmpStr.substring(i * 2, ((i + 1) * 2));
+      if (tmpStr != "") {
+        pPacket->payload[i] = liveData->hexToDec(tmpStr, 1, false);
+      }
+    }
+  }
+  else
+  {
+    struct Packet_t
+    {
+      uint8_t length;
+      uint8_t payload[7];
+    };
+
+    Packet_t* pPacket = (Packet_t*)txBuf;
+    pPacket->length = cmd.length() / 2;
+    
+    for (uint8_t i = 0; i < sizeof(pPacket->payload); i++) {
+      tmpStr = cmd;
+      tmpStr = tmpStr.substring(i * 2, ((i + 1) * 2));
+      if (tmpStr != "") {
+        pPacket->payload[i] = liveData->hexToDec(tmpStr, 1, false);
+      }
+    }
+  }
+
+  lastPid = pid;
+  bResponseProcessed = false;
+  const uint8_t sndStat = CAN->sendMsgBuf(pid, 0, 8, txBuf); // 11 bit
+  //  uint8_t sndStat = CAN->sendMsgBuf(0x7e4, 1, 8, tmp); // 29 bit extended frame
+  if (sndStat == CAN_OK)  {
+    syslog->infoNolf(DEBUG_COMM, "SENT ");
+    lastDataSent = millis();
+  }   else {
+    syslog->infoNolf(DEBUG_COMM, "Error sending PID ");
+    lastDataSent = millis();
+  }
+  syslog->infoNolf(DEBUG_COMM, pid);
+  for (uint8_t i = 0; i < 8; i++) {
+    sprintf(msgString, " 0x%.2X", txBuf[i]);
+    syslog->infoNolf(DEBUG_COMM, msgString);
+  }
+  syslog->info(DEBUG_COMM, "");
+}
+
+/**
+   sendFlowControlFrame
+*/
+void CommObd2Can::sendFlowControlFrame() {
+
+  uint8_t txBuf[8] = { 0x30, requestFramesCount /*request count*/, 20 /*ms between frames*/ , 0, 0, 0, 0, 0 };
+  
+  // insert start char if needed
+  if (liveData->bAdditionalStartingChar) {
+    memmove(txBuf + 1, txBuf, 7);
+    txBuf[0] = liveData->commandStartChar;
+  }
+    
+  const uint8_t sndStat = CAN->sendMsgBuf(lastPid, 0, 8, txBuf); // 11 bit
+  if (sndStat == CAN_OK)  {
+    syslog->infoNolf(DEBUG_COMM, "Flow control frame sent ");
+  }   else {
+    syslog->infoNolf(DEBUG_COMM, "Error sending flow control frame ");
+  }
+  syslog->infoNolf(DEBUG_COMM, lastPid);
+  for (auto txByte : txBuf) {
+    sprintf(msgString, " 0x%.2X", txByte);
+    syslog->infoNolf(DEBUG_COMM, msgString);
+  }
+  syslog->info(DEBUG_COMM, "");
+}
+
+/**
+   Receive PID
+*/
+uint8_t CommObd2Can::receivePID() {
+
+  if (!digitalRead(pinCanInt))                        // If CAN0_INT pin is low, read receive buffer
+  {
+    lastDataSent = millis();
+    syslog->infoNolf(DEBUG_COMM, " CAN READ ");
+    CAN->readMsgBuf(&rxId, &rxLen, rxBuf);      // Read data: len = data length, buf = data byte(s)
+    
+    if ((rxId & 0x80000000) == 0x80000000)    // Determine if ID is standard (11 bits) or extended (29 bits)
+      sprintf(msgString, "Extended ID: 0x%.8lX  DLC: %1d  Data:", (rxId & 0x1FFFFFFF), rxLen);
+    else
+      sprintf(msgString, "Standard ID: 0x%.3lX       DLC: %1d  Data:", rxId, rxLen);
+
+    syslog->infoNolf(DEBUG_COMM, msgString);
+
+    if ((rxId & 0x40000000) == 0x40000000) {  // Determine if message is a remote request frame.
+      sprintf(msgString, " REMOTE REQUEST FRAME");
+      syslog->infoNolf(DEBUG_COMM, msgString);
+    } else {
+      for (uint8_t i = 0; i < rxLen; i++) {
+        sprintf(msgString, " 0x%.2X", rxBuf[i]);
+        syslog->infoNolf(DEBUG_COMM, msgString);
+      }
+    }
+
+    // Check if this packet shall be discarded due to its length. 
+    // If liveData->expectedPacketLength is set to 0, accept any length.
+    if(liveData->expectedMinimalPacketLength != 0 && rxLen < liveData->expectedMinimalPacketLength) {
+      syslog->info(DEBUG_COMM, " [Ignored packet]");
+      return 0xff;
+    }
+
+    // Filter received messages (Ioniq only)
+    if(liveData->settings.carType == CAR_HYUNDAI_IONIQ_2018) {
+      long unsigned int atsh_response = liveData->hexToDec(liveData->currentAtshRequest.substring(4), 2, false) + 8;
+
+      if(rxId != atsh_response) {
+        syslog->info(DEBUG_COMM, " [Filtered packet]");
+        return 0xff;
+      }
+    }
+    
+    syslog->info(DEBUG_COMM, "");
+    processFrameBytes();
+    //processFrame();
+  } else {
+    //syslog->println(" CAN NOT READ ");
+    return 0xff;
+  }
+
+  const uint8_t rxBuffOffset = liveData->bAdditionalStartingChar? 1 : 0;
+  return rxBuf[0 + rxBuffOffset]; // return byte containing frame type, which requires removing offset byte
+}
+
+static void printHexBuffer(uint8_t* pData, const uint16_t length, const bool bAddNewLine)
+{
+  char str[8] = { 0 };
+  
+  for (uint8_t i = 0; i < length; i++) {
+    sprintf(str, " 0x%.2X", pData[i]);
+    syslog->infoNolf(DEBUG_COMM, str);
+  }
+  
+  if (bAddNewLine) {
+    syslog->info(DEBUG_COMM, "");
+  }
+}
+
+static void buffer2string(String& out_targetString, uint8_t* in_pBuffer, const uint16_t in_length)
+{
+  char str[8] = { 0 };
+  
+  for (uint16_t i = 0; i < in_length; i++) {
+    sprintf(str, "%.2X", in_pBuffer[i]);
+    out_targetString += str;  
+  }
+  
+  
+}
+
+CommObd2Can::enFrame_t CommObd2Can::getFrameType(const uint8_t firstByte) {
+  const uint8_t frameType = (firstByte & 0xf0) >> 4; // frame type is in bits 7 to 4
+  switch(frameType) {
+  case 0:
+    return enFrame_t::single;
+  case 1:
+    return enFrame_t::first;
+  case 2:
+    return enFrame_t::consecutive;
+  default:
+    return enFrame_t::unknown;
+  }
+}
+  
+
+
+/**
+   Process can frame on byte level
+   https://en.wikipedia.org/wiki/ISO_15765-2
+ */
+bool CommObd2Can::processFrameBytes() {
+  const uint8_t rxBuffOffset = liveData->bAdditionalStartingChar ? 1 : 0;
+  uint8_t* pDataStart = rxBuf + rxBuffOffset; // set pointer to data start based on specific offset of car
+  const auto frameType = getFrameType(*pDataStart);
+  const uint8_t frameLenght = rxLen - rxBuffOffset;
+  
+  switch (frameType) {
+  case enFrame_t::single: // Single frame
+    {
+      struct SingleFrame_t
+      {
+        uint8_t size : 4;
+        uint8_t frameType : 4;
+        uint8_t pData[];
+      };
+      
+      SingleFrame_t* pSingleFrame = (SingleFrame_t*)pDataStart;
+      mergedData.assign(pSingleFrame->pData, pSingleFrame->pData + pSingleFrame->size);
+      
+      rxRemaining = 0;
+      
+      //syslog->print("----Processing SingleFrame payload: "); printHexBuffer(pSingleFrame->pData, pSingleFrame->size, true);
+      
+      // single frame - process directly
+      buffer2string(liveData->responseRowMerged, mergedData.data(), mergedData.size());
+      liveData->vResponseRowMerged.assign(mergedData.begin(), mergedData.end());
+      processMergedResponse();
+
+      return true;
+    }
+    break;
+    
+  case enFrame_t::first: // First frame
+    {
+      struct FirstFrame_t
+      {
+        uint8_t sizeMSB : 4;
+        uint8_t frameType : 4;
+        uint8_t sizeLSB : 8;
+        uint8_t pData[];
+    
+        uint16_t lengthOfFullPacket() { return (256 * sizeMSB) + sizeLSB; }
+    
+      };
+      
+      FirstFrame_t* pFirstFrame = (FirstFrame_t*)pDataStart;
+      
+      rxRemaining = pFirstFrame->lengthOfFullPacket(); // length of complete data
+      
+      mergedData.clear();
+      dataRows.clear();
+      
+      const uint8_t framePayloadSize = frameLenght - sizeof(FirstFrame_t);    // remove one byte of header
+      dataRows[0].assign(pFirstFrame->pData, pFirstFrame->pData + framePayloadSize);
+      rxRemaining -= framePayloadSize;
+      
+      //syslog->print("----Processing FirstFrame payload: "); printHexBuffer(pFirstFrame->pData, framePayloadSize, true);
+    }
+    break;
+    
+  case enFrame_t::consecutive: // Consecutive frame
+    {
+      struct ConsecutiveFrame_t
+      {
+        uint8_t index : 4;
+        uint8_t frameType : 4;
+        uint8_t pData[];
+      };
+      
+      const uint8_t structSize = sizeof(ConsecutiveFrame_t);
+      //syslog->print("[debug] sizeof(ConsecutiveFrame_t) is expected to be 1 and it's "); syslog->println(structSize);
+      
+      ConsecutiveFrame_t* pConseqFrame = (ConsecutiveFrame_t*)pDataStart;
+      const uint8_t framePayloadSize = frameLenght - sizeof(ConsecutiveFrame_t);  // remove one byte of header
+      dataRows[pConseqFrame->index].assign(pConseqFrame->pData, pConseqFrame->pData + framePayloadSize);
+      rxRemaining -= framePayloadSize;
+      
+      //syslog->print("----Processing ConsecFrame payload: "); printHexBuffer(pConseqFrame->pData, framePayloadSize, true);
+    }
+    break;
+    
+  default:
+    syslog->infoNolf(DEBUG_COMM, "Unknown frame type within CommObd2Can::processFrameBytes(): "); 
+    syslog->info(DEBUG_COMM, (uint8_t)frameType);
+    return false;
+    break;
+  } // \switch (frameType)
+  
+  // Merge data if all data was received
+  if (rxRemaining <= 0) {
+    // multiple frames and no data remaining - merge everything to single packet
+    for (int i = 0; i < dataRows.size(); i++) {
+      //syslog->print("------merging packet index ");
+      //syslog->print(i);
+      //syslog->print(" with length ");
+      //syslog->println(dataRows[i].size());
+      
+      mergedData.insert(mergedData.end(), dataRows[i].begin(), dataRows[i].end());
+    }
+    
+    buffer2string(liveData->responseRowMerged, mergedData.data(), mergedData.size()); // output for string parsing
+    liveData->vResponseRowMerged.assign(mergedData.begin(), mergedData.end());   // output for binary parsing
+    processMergedResponse();
+  }
+  
+  return true;
+}
+
+/**
+   Process can frame
+   https://en.wikipedia.org/wiki/ISO_15765-2
+*/
+bool CommObd2Can::processFrame() {
+
+  const uint8_t frameType = (rxBuf[0] & 0xf0) >> 4;
+  uint8_t start = 1; // Single and Consecutive starts with pos 1
+  uint8_t index = 0; // 0 - f
+
+  liveData->responseRow = "";
+  switch (frameType) {
+    // Single frame
+    case 0:
+      rxRemaining = (rxBuf[1] & 0x0f);
+      requestFramesCount = 0;
+      break;
+    // First frame
+    case 1:
+      rxRemaining = ((rxBuf[0] & 0x0f) << 8) + rxBuf[1];
+      requestFramesCount = ceil((rxRemaining - 6) / 7.0);
+      liveData->responseRowMerged = "";
+      for (uint16_t i = 0; i < rxRemaining - 1; i++)
+        liveData->responseRowMerged += "00";
+      liveData->responseRow = "0:";
+      start = 2;
+      break;
+    // Consecutive frames
+    case 2:
+      index = (rxBuf[0] & 0x0f);
+      sprintf(msgString, "%.1X:", index);
+      liveData->responseRow = msgString; // convert 0..15 to ascii 0..F);
+      break;
+  }
+
+  syslog->infoNolf(DEBUG_COMM, "> frametype:");
+  syslog->infoNolf(DEBUG_COMM, frameType);
+  syslog->infoNolf(DEBUG_COMM, ", r: ");
+  syslog->infoNolf(DEBUG_COMM, rxRemaining);
+  syslog->infoNolf(DEBUG_COMM, "   ");
+
+  for (uint8_t i = start; i < rxLen; i++) {
+    sprintf(msgString, "%.2X", rxBuf[i]);
+    liveData->responseRow += msgString;
+    rxRemaining--;
+  }
+
+  syslog->infoNolf(DEBUG_COMM, ", r: ");
+  syslog->infoNolf(DEBUG_COMM, rxRemaining);
+  syslog->info(DEBUG_COMM, "   ");
+
+  //parseResponse();
+  // We need to sort frames
+  // 1 frame data
+  syslog->info(DEBUG_COMM, liveData->responseRow);
+  // Merge frames 0:xxxx 1:yyyy 2:zzzz to single response xxxxyyyyzzzz string
+  if (liveData->responseRow.length() >= 2 && liveData->responseRow.charAt(1) == ':') {
+    //liveData->responseRowMerged += liveData->responseRow.substring(2);
+    uint8_t rowNo = liveData->hexToDec(liveData->responseRow.substring(0, 1), 1, false);
+    uint16_t startPos = (rowNo * 14) - ((rowNo > 0) ? 2 : 0);
+    uint16_t endPos = ((rowNo + 1) * 14) - ((rowNo > 0) ? 2 : 0);
+    liveData->responseRowMerged = liveData->responseRowMerged.substring(0, startPos) + liveData->responseRow.substring(2) + liveData->responseRowMerged.substring(endPos);
+    syslog->info(DEBUG_COMM, liveData->responseRowMerged);
+  }
+
+  // Send response to board module
+  if (rxRemaining <= 2) {
+    processMergedResponse();
+    return false;
+  }
+
+  return true;
+}
+
+/**
+   processMergedResponse
+*/
+void CommObd2Can::processMergedResponse() {
+  syslog->infoNolf(DEBUG_COMM, "merged:");
+  syslog->info(DEBUG_COMM, liveData->responseRowMerged);
+  board->parseRowMerged();
+  
+  liveData->responseRowMerged = "";
+  liveData->vResponseRowMerged.clear();
+  bResponseProcessed = true; // to allow delay untill next message
+  
+  if (liveData->delayBetweenCommandsMs == 0) {
+    liveData->canSendNextAtCommand = true; // allow next command immediately
+  }
+}
diff --git a/CommObd2Can.h b/CommObd2Can.h
index b408ba3..f154df8 100644
--- a/CommObd2Can.h
+++ b/CommObd2Can.h
@@ -1,12 +1,54 @@
-#ifndef COMMOBD2CAN_H
-#define COMMOBD2CAN_H
+#pragma once
 
 #include "LiveData.h"
+#include "CommInterface.h"
+#include <mcp_can.h>
+
+#include <memory>
+#include <vector>
+#include <unordered_map>
 
 class CommObd2Can : public CommInterface {
-  
-  protected:
-  public:
-};
 
-#endif // COMMOBD2CAN_H
+  protected:
+    const uint8_t pinCanInt = 15;
+    const uint8_t pinCanCs = 12;
+  	std::unique_ptr <MCP_CAN> CAN;
+    long unsigned int rxId;
+    unsigned char rxLen = 0;
+    uint8_t rxBuf[32];
+    int16_t rxRemaining; // Remaining bytes to complete message, signed is ok
+    uint8_t requestFramesCount;
+    char msgString[128];                        // Array to store serial string
+    uint16_t lastPid;
+    unsigned long lastDataSent = 0;
+    
+    std::vector<uint8_t> mergedData;
+    std::unordered_map<uint16_t, std::vector<uint8_t>> dataRows;
+    bool bResponseProcessed = false;
+    
+    enum class enFrame_t
+    {
+      single = 0,
+      first = 1,
+      consecutive = 2,
+      unknown = 9
+    };
+    
+  public:
+    void connectDevice() override;
+    void disconnectDevice() override;
+    void scanDevices() override;
+    void mainLoop() override;
+    void executeCommand(String cmd) override;
+  
+  private:
+    void sendPID(const uint16_t pid, const String& cmd);
+    void sendFlowControlFrame();
+    uint8_t receivePID();
+    enFrame_t getFrameType(const uint8_t firstByte);
+    bool processFrameBytes();
+    bool processFrame();
+    void processMergedResponse();
+  
+};
diff --git a/INSTALLATION.md b/INSTALLATION.md
index 461ba58..53aaf54 100644
--- a/INSTALLATION.md
+++ b/INSTALLATION.md
@@ -6,7 +6,7 @@ M5STACK (Many thanks to DimZen)
 
 https://docs.google.com/document/d/17vJmeveNfN0exQy9wKC-5igU8zzNjsuOn1DPuPV_yJA/edit?usp=sharing
 
-TTGO-T4 (older)
+TTGO-T4 (older guide)
 
 https://docs.google.com/document/d/1nEezrtXY-8X6mQ1hiZVWDjBVse1sXQg1SlnizaRmJwU/edit?usp=sharing
 
diff --git a/LiveData.cpp b/LiveData.cpp
index 0e178cd..3eee77e 100644
--- a/LiveData.cpp
+++ b/LiveData.cpp
@@ -1,16 +1,21 @@
-
-#ifndef LIVEDATA_CPP
-#define LIVEDATA_CPP
-
 #include "LiveData.h"
 #include "menu.h"
 
+LogSerial* syslog;
+
+/**
+ * Debug level
+ */
+void debug(String msg, uint8_t debugLevel) {
+  syslog->println(msg);
+}
+
 /**
    Init params with default values
 */
 void LiveData::initParams() {
 
-  params.automaticShutdownTimer = 0;
+  params.mainLoopCounter = 0;
   // SIM
   params.lastDataSent = 0;
   params.sim800l_enabled = false;
@@ -28,18 +33,22 @@ void LiveData::initParams() {
   params.gpsAlt = -1;
   // Car data
   params.ignitionOn = false;
-  params.ignitionOnPrevious = false;
+  params.lastIgnitionOnTime = 0;
   params.operationTimeSec = 0;
   params.chargingStartTime = params.currentTime = 0;
-  params.lightInfo = 0;
+  params.chargingOn = false;
   params.headLights = false;
   params.dayLights = false;
   params.brakeLights = false;
-  params.brakeLightInfo = 0;
+  params.trunkDoorOpen = false;
+  params.leftFrontDoorOpen = false;
+  params.rightFrontDoorOpen = false;
+  params.leftRearDoorOpen = false;
+  params.rightRearDoorOpen = false;
+  params.hoodDoorOpen = false;
   params.forwardDriveMode = false;
   params.reverseDriveMode = false;
   params.parkModeOrNeutral = false;
-  params.espState = 0;
   params.speedKmh = -1;
   params.motorRpm = -1;
   params.odoKm = -1;
@@ -185,9 +194,3 @@ float LiveData::celsius2temperature(float inCelsius) {
 float LiveData::bar2pressure(float inBar) {
   return (settings.pressureUnit == 'b') ? inBar : inBar * 14.503773800722;
 }
-
-
-
-
-//
-#endif // LIVEDATA_CPP
diff --git a/LiveData.h b/LiveData.h
index 125a4e6..53fbaa4 100644
--- a/LiveData.h
+++ b/LiveData.h
@@ -1,6 +1,4 @@
-
-#ifndef LIVEDATA_H
-#define LIVEDATA_H
+#pragma once
 
 #include <Arduino.h>
 #include <stdint.h>
@@ -9,6 +7,8 @@
 #include <sys/time.h>
 #include <BLEDevice.h>
 #include "config.h"
+#include "LogSerial.h"
+#include <vector>
 
 // SUPPORTED CARS
 #define CAR_KIA_ENIRO_2020_64     0
@@ -18,11 +18,13 @@
 #define CAR_HYUNDAI_KONA_2020_39  4
 #define CAR_RENAULT_ZOE           5
 #define CAR_KIA_NIRO_PHEV         6
+#define CAR_BMW_I3_2014           7
 #define CAR_DEBUG_OBD2_KIA        999
 
-// 
+// COMM TYPE
 #define COMM_TYPE_OBD2BLE4  0
 #define COMM_TYPE_OBD2CAN   1
+#define COMM_TYPE_OBD2BT3   2
 
 // SCREENS
 #define SCREEN_BLANK  0
@@ -32,14 +34,18 @@
 #define SCREEN_CELLS  4
 #define SCREEN_CHARGING 5
 #define SCREEN_SOC10  6
-#define SCREEN_DEBUG  7
+
+// 
+#define MONTH_SEC     2678400
+
+extern LogSerial* syslog;
 
 // Structure with realtime values
 typedef struct {
   // System
   time_t currentTime;
   time_t chargingStartTime;
-  time_t automaticShutdownTimer;
+  uint32_t mainLoopCounter; 
   // SIM
   time_t lastDataSent;
   bool sim800l_enabled;
@@ -55,7 +61,9 @@ typedef struct {
   char sdcardFilename[32];
   // Car params
   bool ignitionOn;
-  bool ignitionOnPrevious;
+  bool chargingOn;
+  time_t lastIgnitionOnTime;
+  time_t lastChargingOnTime;
   uint64_t operationTimeSec;
   bool sdcardCanNotify;
   bool forwardDriveMode;
@@ -64,9 +72,15 @@ typedef struct {
   bool headLights;
   bool dayLights;
   bool brakeLights;
-  uint8_t lightInfo;
+  bool trunkDoorOpen;
+  bool leftFrontDoorOpen;
+  bool rightFrontDoorOpen;
+  bool leftRearDoorOpen;
+  bool rightRearDoorOpen;
+  bool hoodDoorOpen;
+/*  uint8_t lightInfo;
   uint8_t brakeLightInfo;
-  uint8_t espState;
+  uint8_t espState;*/
   float batteryTotalAvailableKWh;
   float speedKmh;
   float motorRpm;
@@ -106,6 +120,7 @@ typedef struct {
   float auxPerc;
   float auxCurrentAmp;
   float auxVoltage;
+  float auxTemperature;
   float indoorTemperature;
   float outdoorTemperature;
   float tireFrontLeftTempC;
@@ -161,11 +176,11 @@ typedef struct {
   // =================================
   byte defaultScreen; // 1 .. 6
   byte lcdBrightness; // 0 - auto, 1 .. 100%
-  byte debugScreen; // 0 - off, 1 - on
+  byte sleepModeEnabled; // 0 - off, 1 - on
   byte predrawnChargingGraphs; // 0 - off, 1 - on
   // === settings version 4
   // =================================
-  byte commType; // 0 - OBD2 BLE4 adapter, 1 - CAN 
+  byte commType; // 0 - OBD2 BLE4 adapter, 1 - CAN, 2 - BT3
   // Wifi
   byte wifiEnabled; // 0/1 
   char wifiSsid[32];
@@ -189,23 +204,36 @@ typedef struct {
   // === settings version 5
   // =================================
   byte gpsHwSerialPort; // 255-off, 0,1,2 - hw serial
+  byte gprsHwSerialPort; // 255-off, 0,1,2 - hw serial
+  // === settings version 6
+  // =================================
+  byte serialConsolePort; // 255-off, 0 - hw serial (std)
+  uint8_t debugLevel; // 0 - info only, 1 - debug communication (BLE/CAN), 2 - debug GSM, 3 - debug SDcard
+  uint16_t sdcardLogIntervalSec; // every x seconds
+  uint16_t gprsLogIntervalSec; // every x seconds
   //  
 } SETTINGS_STRUC;
 
-
-//
+// LiveData class
 class LiveData {
   protected:
   public:
     // Command loop
+    struct Command_t {
+      uint8_t startChar; // special starting character used by some cars
+      String request;
+    };
+    
     uint16_t commandQueueCount;
     uint16_t commandQueueLoopFrom;
-    String commandQueue[300];
+    std::vector<Command_t> commandQueue;
     String responseRow;
     String responseRowMerged;
+    std::vector<uint8_t> vResponseRowMerged;
     uint16_t commandQueueIndex;
     bool canSendNextAtCommand = false;
-    String commandRequest = "";
+    uint8_t commandStartChar; 
+    String commandRequest = ""; // TODO: us Command_t struct
     String currentAtshRequest = "";
     // Menu
     bool menuVisible = false;
@@ -216,15 +244,22 @@ class LiveData {
     uint16_t scanningDeviceIndex = 0;
     MENU_ITEM* menuItems;
 
+    // Comm
+    boolean commConnected = true;
     // Bluetooth4
     boolean bleConnect = true;
-    boolean bleConnected = false;
     BLEAddress *pServerAddress;
     BLERemoteCharacteristic* pRemoteCharacteristic;
     BLERemoteCharacteristic* pRemoteCharacteristicWrite;
     BLEAdvertisedDevice* foundMyBleDevice;
     BLEClient* pClient;
     BLEScan* pBLEScan;
+  
+    // Canbus
+    bool bAdditionalStartingChar = false;  // some cars uses additional starting character in beginning of tx and rx messages
+    uint8_t expectedMinimalPacketLength = 0;  // what length of packet should be accepted. Set to 0 to accept any length
+    uint16_t rxTimeoutMs = 100; // timeout for receiving of CAN response
+    uint16_t delayBetweenCommandsMs = 0; // delay between commands, set to 0 if no delay is needed 
     
     // Params
     PARAMS_STRUC params;     // Realtime sensor values
@@ -240,7 +275,3 @@ class LiveData {
     float celsius2temperature(float inCelsius);
     float bar2pressure(float inBar);
 };
-
-
-//
-#endif // LIVEDATA_H
diff --git a/LogSerial.cpp b/LogSerial.cpp
new file mode 100644
index 0000000..6ecb71f
--- /dev/null
+++ b/LogSerial.cpp
@@ -0,0 +1,15 @@
+#include "LogSerial.h"
+
+/**
+ * Constructor
+ */
+LogSerial::LogSerial() : HardwareSerial(0) {   
+  HardwareSerial::begin(115200);
+}
+
+/**
+ * Set debug level
+ */
+void LogSerial::setDebugLevel(uint8_t aDebugLevel) {
+  debugLevel = aDebugLevel;
+}
diff --git a/LogSerial.h b/LogSerial.h
new file mode 100644
index 0000000..f38a6dc
--- /dev/null
+++ b/LogSerial.h
@@ -0,0 +1,58 @@
+#pragma once
+
+#include <HardwareSerial.h>
+
+// DEBUG LEVEL
+#define DEBUG_NONE    0
+#define DEBUG_COMM    1 // filter comm
+#define DEBUG_GSM     2 // filter gsm messages
+#define DEBUG_SDCARD  3 // filter sdcard
+
+//
+class LogSerial: public HardwareSerial {
+  protected:
+    uint8_t debugLevel;
+  public:
+    LogSerial();
+    //
+    void setDebugLevel(uint8_t aDebugLevel);
+    // info
+    template <class T, typename... Args> void info(uint8_t aDebugLevel, T msg) {
+      if (debugLevel != DEBUG_NONE && aDebugLevel != DEBUG_NONE && aDebugLevel != debugLevel)
+        return;
+      println(msg);
+    }
+    template <class T, typename... Args> void infoNolf(uint8_t aDebugLevel, T msg) {
+      if (debugLevel != DEBUG_NONE && aDebugLevel != DEBUG_NONE && aDebugLevel != debugLevel)
+        return;
+      print(msg);
+    }
+    // warning
+    template <class T, typename... Args> void warn(uint8_t aDebugLevel, T msg) {
+      if (debugLevel != DEBUG_NONE && aDebugLevel != DEBUG_NONE && aDebugLevel != debugLevel)
+        return;
+      print("WARN ");
+      println(msg);
+    }
+    template <class T, typename... Args> void warnNolf(uint8_t aDebugLevel, T msg) {
+      if (debugLevel != DEBUG_NONE && aDebugLevel != DEBUG_NONE && aDebugLevel != debugLevel)
+        return;
+      print("WARN ");
+      print(msg);
+    }
+
+    // error
+    template <class T, typename... Args> void err(uint8_t aDebugLevel, T msg) {
+      if (debugLevel != DEBUG_NONE && aDebugLevel != DEBUG_NONE && aDebugLevel != debugLevel)
+        return;
+      print("ERR ");
+      println(msg);
+    }
+    template <class T, typename... Args> void errNolf(uint8_t aDebugLevel, T msg) {
+      if (debugLevel != DEBUG_NONE && aDebugLevel != DEBUG_NONE && aDebugLevel != debugLevel)
+        return;
+      print("ERR ");
+      print(msg);
+    }
+    
+};
diff --git a/README.md b/README.md
index 5fb13ce..51c9af5 100644
--- a/README.md
+++ b/README.md
@@ -1,31 +1,30 @@
 # evDash (old enirodashboard)
 
 Supported devices
-1. LILYGO TTGO T4 v1.3
-2. M5STACK CORE1 IOT Development Kit
+1. M5STACK CORE1 IOT Development Kit (best option)
+2. LILYGO TTGO T4 v1.3 (!!! limited support, no SDcard/GSM/GPS/CAN module)
 
 Working only with electric vehicles 
 Kia e-NIRO (EV), Hyundai Kona EV, Hyundai Ioniq EV, Kia Niro Phev 8.9kWh
-Vgate iCar Pro Bluetooth 4.0 (BLE4) OBD2 adapter is required. See Release notes, quick installation via flash tool bellow. 
+Vgate iCar Pro Bluetooth 4.0 (BLE4) OBD2 adapter is required or CAN (m5 COMMU module).
+See Release notes, quick installation via flash tool bellow. 
 
 Use it at your own risk!
-Author: nick.n17@gmail.com (Lubos Petrovic / Slovakia)
 
-## Supporting me
-
-- Buy Me a Beer via paypal https://www.paypal.me/nickn17
-- EU companies can support me via IBAN/Invoice (my company is non-VAT payer in Slovakia).
-
-Many thanks to Blas, Jens, Калин, Aleš Dokupil and others for help. Thank you for supporting me. 
+evDash Discord server: https://discord.gg/rfAvH7xzTr
 
 ## Required hardware
 Board
 - M5STACK CORE1 IOT Development Kit(~EUR 35)
   https://rlx.sk/sk/m5stack/7285-esp32-basic-core-iot-development-kit-m5-k001-m5stack.html
+- optional M5 COMMU (CAN) module
+- optional M5 GPS NEO-M8N (with external atenna)
+- optional M5 SIM800L GPS module (dev)
   
 or 
-- LILYGO TTGO T4 v1.3 (~USD $30) https://www.banggood.com/LILYGO-TTGO-T-Watcher-BTC-Ticker-ESP32-For-Bitcoin-Price-Program-4M-SPI-Flash-Psram-LCD-Display-Module-p-1345292.html
-I RECOMMEND TO REMOVE LION BATTERY DUE TO HIGH SUMMER TEMPERATURES
+- older device LILYGO TTGO T4 v1.3 (~USD $30) 
+  https://www.banggood.com/LILYGO-TTGO-T-Watcher-BTC-Ticker-ESP32-For-Bitcoin-Price-Program-4M-SPI-Flash-Psram-LCD-Display-Module-p-1345292.html
+  I RECOMMEND TO REMOVE LION BATTERY DUE TO HIGH SUMMER TEMPERATURES
 
 OBD2 adapter
 - Supported is only this model... Vgate iCar Pro Bluetooth 4.0 (BLE4) OBD2 (~USD $30)
@@ -51,15 +50,19 @@ See INSTALLATION.md
 
 Screen list
 - no0. blank screen, lcd off
-- no1. auto mode (summary info / speed kmh / charging graph)
-- no2. summary info (default)
-- no3. speed kmh + kwh/100km (or kw for discharge)
+- no1. automatic mode (summary info / speed kmh / charging graph)
+- no2. summary info
+- no3. speed kmh + kwh/100km
 - no4. battery cells + battery module temperatures
 - no5. charging graph
-- no6. consumption table. Can be used to measure available battery capacity! 
-- no7. debug screen (default off in the menu)
+- no6. consumption table. Can be used to measure available battery capacity.
 
-![image](https://github.com/nickn17/evDash/blob/master/screenshots/v1.jpg)
+![image](https://github.com/nickn17/evDash/blob/master/screenshots/v2.jpg)
+![image](https://github.com/nickn17/evDash/blob/master/screenshots/v2_m5charging2.jpg)
 
-[![Watch the video](https://github.com/nickn17/evDash/blob/master/screenshots/v0.9.jpg)](https://www.youtube.com/watch?v=Jg5VP2P58Yg&)
+## Supporting me
+
+- nick.n17@gmail.com (Lubos Petrovic / Slovakia)
+- Buy Me a Beer via paypal https://www.paypal.me/nickn17
+- Many thanks to all evDash contributors.
 
diff --git a/RELEASENOTES.md b/RELEASENOTES.md
index a286d0d..d465ee7 100644
--- a/RELEASENOTES.md
+++ b/RELEASENOTES.md
@@ -2,6 +2,20 @@
 
 ### Next version
 
+### v2.2.0 2020-12-29
+- Direct CAN support with m5 COMMU module (instead obd2 BLE4 adapter). RECOMMENDED
+- EvDash deep sleep & wake up for Hyundai Ioniq/Kona & Kia e-Niro (kolaCZek).
+- Send data via GPRS to own server (kolaCZek). Simple web api project https://github.com/kolaCZek/evDash_serverapi)
+- Better support for Hyundai Ioniq (kolaCZek).
+- Kia e-niro - added support for open doors/hood/trunk.
+- Serial console off/on and improved logging & debug level setting
+- Avoid GPS on UART0 collision with serial console.
+- DEV initial support for Bmw i3 (Janulo)
+- Command queue refactoring (Janulo)
+- Sdcard is working only with m5stack
+- Removed debug screen
+- M5 mute speaker fix
+
 ### v2.1.1 2020-12-14
 - tech refactoring: `hexToDecFromResponse`, `decFromResponse`
 - added support for GPS module on HW UART (user HWUART=2 for m5stack NEO-M8N)
diff --git a/SIM800L.cpp b/SIM800L.cpp
index d79eda8..e17e4f2 100644
--- a/SIM800L.cpp
+++ b/SIM800L.cpp
@@ -45,6 +45,9 @@ const char AT_CMD_CFUN0[] PROGMEM = "AT+CFUN=0";                              //
 const char AT_CMD_CFUN1[] PROGMEM = "AT+CFUN=1";                              // Switch normal power mode
 const char AT_CMD_CFUN4[] PROGMEM = "AT+CFUN=4";                              // Switch sleep power mode
 
+const char AC_CMD_CSCLK0[] PROGMEM = "AT+CSCLK=0";                            // AT+CSCLK=0 command
+const char AC_CMD_CSCLK2[] PROGMEM = "AT+CSCLK=2";                            // AT+CSCLK=2 command
+
 const char AT_CMD_CREG_TEST[] PROGMEM = "AT+CREG?";                           // Check the network registration status
 const char AT_CMD_SAPBR_GPRS[] PROGMEM = "AT+SAPBR=3,1,\"Contype\",\"GPRS\""; // Configure the GPRS bearer
 const char AT_CMD_SAPBR_APN[] PROGMEM = "AT+SAPBR=3,1,\"APN\",";              // Configure the APN for the GPRS
@@ -721,6 +724,16 @@ bool SIM800L::setPowerMode(PowerMode powerMode) {
   return currentPowerMode == powerMode;
 }
 
+void SIM800L::exitSleepMode() {
+  sendCommand_P(AC_CMD_CSCLK0);
+  purgeSerial();
+}
+
+void SIM800L::enterSleepMode() {
+  sendCommand_P(AC_CMD_CSCLK2);
+  purgeSerial();
+}
+
 /**
  * Status function: Check the strengh of the signal
  */
diff --git a/SIM800L.h b/SIM800L.h
index d49597c..8845855 100644
--- a/SIM800L.h
+++ b/SIM800L.h
@@ -66,6 +66,8 @@ class SIM800L {
 
     // Define the power mode (for parameter: see PowerMode enum)
     bool setPowerMode(PowerMode powerMode);
+    void enterSleepMode();
+    void exitSleepMode();
 
     // Enable/disable GPRS
     bool setupGPRS(const char *apn);
diff --git a/config.h b/config.h
index 753a201..3c145c6 100644
--- a/config.h
+++ b/config.h
@@ -1,10 +1,9 @@
-#ifndef CONFIG_H
-#define CONFIG_H
+#pragma once
 
 #include <BLEDevice.h>
 
-#define APP_VERSION "v2.1.1"
-#define APP_RELEASE_DATE "2020-12-14"
+#define APP_VERSION "v2.2.0"
+#define APP_RELEASE_DATE "2020-12-29"
 
 // TFT COLORS FOR TTGO
 #define TFT_BLACK       0x0000      /*   0,   0,   0 */
@@ -48,13 +47,8 @@
 ////////////////////////////////////////////////////////////
 // SIM800L
 /////////////////////////////////////////////////////////////
-
-#ifdef SIM800L_ENABLED
-#define SIM800L_RX 16
-#define SIM800L_TX 17
 #define SIM800L_RST 5
 #define SIM800L_TIMER 60
-#endif //SIM800L_ENABLED
 
 // MENU ITEM
 typedef struct {
@@ -67,9 +61,14 @@ typedef struct {
 } MENU_ITEM;
 
 #define MENU_VEHICLE_TYPE 1
+#define MENU_ADAPTER_TYPE 5
 #define MENU_SAVE_SETTINGS 9
 #define MENU_APP_VERSION 10
 //
+#define MENU_ADAPTER_BLE4 501
+#define MENU_ADAPTER_CAN 502
+#define MENU_ADAPTER_BT3 503
+//
 #define MENU_WIFI 301
 #define MENU_GPRS 302
 #define MENU_NTP 303
@@ -80,8 +79,10 @@ typedef struct {
 #define MENU_PREDRAWN_GRAPHS 308
 #define MENU_REMOTE_UPLOAD 309
 #define MENU_HEADLIGHTS_REMINDER 310
-#define MENU_DEBUG_SCREEN 311
+#define MENU_SLEEP_MODE 311
 #define MENU_GPS 312
+#define MENU_SERIAL_CONSOLE 313
+#define MENU_DEBUG_LEVEL 314
 //
 #define MENU_DISTANCE_UNIT 401
 #define MENU_TEMPERATURE_UNIT 402
@@ -95,6 +96,5 @@ typedef struct {
 #define MENU_SDCARD_AUTOSTARTLOG 3042
 #define MENU_SDCARD_MOUNT_STATUS 3043
 #define MENU_SDCARD_REC 3044
+#define MENU_SDCARD_INTERVAL 3045
 // 
-
-#endif // CONFIG_H
diff --git a/dist/m5stack_core1/evDash.ino.bin b/dist/m5stack_core1/evDash.ino.bin
index deff462..b64dbec 100644
Binary files a/dist/m5stack_core1/evDash.ino.bin and b/dist/m5stack_core1/evDash.ino.bin differ
diff --git a/dist/ttgo_t4_v13/evDash.ino.bin b/dist/ttgo_t4_v13/evDash.ino.bin
index 34accf9..bb9b62b 100644
Binary files a/dist/ttgo_t4_v13/evDash.ino.bin and b/dist/ttgo_t4_v13/evDash.ino.bin differ
diff --git a/evDash.ino b/evDash.ino
index dd9a0eb..9c8ff5b 100644
--- a/evDash.ino
+++ b/evDash.ino
@@ -1,20 +1,16 @@
 /*
-   Project renamed from eNiroDashboard to evDash
-
-  !! working only with OBD BLE 4.0 adapters
-  !! Supported adapter is Vgate ICar Pro (must be BLE4.0 version)
-  !! Not working with standard BLUETOOTH 3 adapters
+  Project renamed from eNiroDashboard to evDash
 
   Serial console commands
 
-    serviceUUID=xxx
-    charTxUUID=xxx
-    charRxUUID=xxx
-    wifiSsid=xxx
-    wifiPassword=xxx
-    gprsApn=xxx
-    remoteApiUrl=xxx
-    remoteApiKey=xxx
+   serviceUUID=xxx
+   charTxUUID=xxx
+   charRxUUID=xxx
+   wifiSsid=xxx
+   wifiPassword=xxx
+   gprsApn=xxx
+   remoteApiUrl=xxx
+   remoteApiKey=xxx
 
   Required libraries
   - esp32 board support
@@ -31,10 +27,8 @@
 ////////////////////////////////////////////////////////////
 
 // Boards
-#define BOARD_TTGO_T4
-//#define BOARD_M5STACK_CORE
-
-//#define SIM800L_ENABLED
+//#define BOARD_TTGO_T4
+#define BOARD_M5STACK_CORE
 
 ////////////////////////////////////////////////////////////
 ////////////////////////////////////////////////////////////
@@ -49,8 +43,6 @@
 #include "BoardM5stackCore.h"
 #endif // BOARD_M5STACK_CORE
 
-#include <BLEDevice.h>
-#include <sys/time.h>
 #include "config.h"
 #include "LiveData.h"
 #include "CarInterface.h"
@@ -59,485 +51,34 @@
 #include "CarRenaultZoe.h"
 #include "CarKiaNiroPhev.h"
 #include "CarKiaDebugObd2.h"
-
-#ifdef SIM800L_ENABLED
-#include <ArduinoJson.h>
-#include "SIM800L.h"
-
-SIM800L* sim800l;
-HardwareSerial SerialGPRS(2);
-#endif //SIM800L_ENABLED
-
-// Temporary variables
-char ch;
-String line;
+#include "CarBmwI3.h"
 
 // Board, Car, Livedata (params, settings)
 BoardInterface* board;
 CarInterface* car;
 LiveData* liveData;
 
-/**
-  Do next AT command from queue
-*/
-bool doNextAtCommand() {
-
-  // Restart loop with AT commands
-  if (liveData->commandQueueIndex >= liveData->commandQueueCount) {
-    liveData->commandQueueIndex = liveData->commandQueueLoopFrom;
-    board->redrawScreen();
-    // log every queue loop (temp)
-    liveData->params.sdcardCanNotify = true;
-  }
-
-  // Send AT command to obd
-  liveData->commandRequest = liveData->commandQueue[liveData->commandQueueIndex];
-  if (liveData->commandRequest.startsWith("ATSH")) {
-    liveData->currentAtshRequest = liveData->commandRequest;
-  }
-
-  Serial.print(">>> ");
-  Serial.println(liveData->commandRequest);
-  String tmpStr = liveData->commandRequest + "\r";
-  liveData->pRemoteCharacteristicWrite->writeValue(tmpStr.c_str(), tmpStr.length());
-  liveData->commandQueueIndex++;
-
-  return true;
-}
-
-/**
-  Parse result from OBD, create single line liveData->responseRowMerged
-*/
-bool parseRow() {
-
-  // Simple 1 line responses
-  Serial.print("");
-  Serial.println(liveData->responseRow);
-
-  // Merge 0:xxxx 1:yyyy 2:zzzz to single xxxxyyyyzzzz string
-  if (liveData->responseRow.length() >= 2 && liveData->responseRow.charAt(1) == ':') {
-    if (liveData->responseRow.charAt(0) == '0') {
-      liveData->responseRowMerged = "";
-    }
-    liveData->responseRowMerged += liveData->responseRow.substring(2);
-  }
-
-  return true;
-}
-
-/**
-  Parse merged row (after merge completed)
-*/
-bool parseRowMerged() {
-
-  Serial.print("merged:");
-  Serial.println(liveData->responseRowMerged);
-
-  // Catch output for debug screen
-  if (board->displayScreen == SCREEN_DEBUG) {
-    if (board->debugCommandIndex == liveData->commandQueueIndex) {
-      board->debugAtshRequest = liveData->currentAtshRequest;
-      board->debugCommandRequest = liveData->commandRequest;
-      board->debugLastString  = liveData->responseRowMerged;
-    }
-  }
-
-  // Parse by selected car interface
-  car->parseRowMerged();
-
-  return true;
-}
-
-/**
-  BLE callbacks
-*/
-class MyClientCallback : public BLEClientCallbacks {
-
-    /**
-      On BLE connect
-    */
-    void onConnect(BLEClient* pclient) {
-      Serial.println("onConnect");
-    }
-
-    /**
-      On BLE disconnect
-    */
-    void onDisconnect(BLEClient* pclient) {
-      //connected = false;
-      Serial.println("onDisconnect");
-      board->displayMessage("BLE disconnected", "");
-    }
-};
-
-/**
-   Scan for BLE servers and find the first one that advertises the service we are looking for.
-*/
-class MyAdvertisedDeviceCallbacks: public BLEAdvertisedDeviceCallbacks {
-
-    /**
-      Called for each advertising BLE server.
-    */
-    void onResult(BLEAdvertisedDevice advertisedDevice) {
-
-      Serial.print("BLE advertised device found: ");
-      Serial.println(advertisedDevice.toString().c_str());
-      Serial.println(advertisedDevice.getAddress().toString().c_str());
-
-      // Add to device list (max. 9 devices allowed yet)
-      String tmpStr;
-
-      if (liveData->scanningDeviceIndex < 10/* && advertisedDevice.haveServiceUUID()*/) {
-        for (uint16_t i = 0; i < liveData->menuItemsCount; ++i) {
-          if (liveData->menuItems[i].id == 10001 + liveData->scanningDeviceIndex) {
-            tmpStr = advertisedDevice.toString().c_str();
-            tmpStr.replace("Name: ", "");
-            tmpStr.replace("Address: ", "");
-            tmpStr.toCharArray(liveData->menuItems[i].title, 48);
-            tmpStr = advertisedDevice.getAddress().toString().c_str();
-            tmpStr.toCharArray(liveData->menuItems[i].obdMacAddress, 18);
-          }
-        }
-        liveData->scanningDeviceIndex++;
-      }
-      /*
-        if (advertisedDevice.getServiceDataUUID().toString() != "<NULL>") {
-        Serial.print("ServiceDataUUID: ");
-        Serial.println(advertisedDevice.getServiceDataUUID().toString().c_str());
-        if (advertisedDevice.getServiceUUID().toString() != "<NULL>") {
-          Serial.print("ServiceUUID: ");
-          Serial.println(advertisedDevice.getServiceUUID().toString().c_str());
-        }
-        }*/
-
-      if (advertisedDevice.haveServiceUUID() && advertisedDevice.isAdvertisingService(BLEUUID(liveData->settings.serviceUUID)) &&
-          (strcmp(advertisedDevice.getAddress().toString().c_str(), liveData->settings.obdMacAddress) == 0)) {
-        Serial.println("Stop scanning. Found my BLE device.");
-        BLEDevice::getScan()->stop();
-        liveData->foundMyBleDevice = new BLEAdvertisedDevice(advertisedDevice);
-      }
-    }
-};
-
-uint32_t PIN = 1234;
-
-/**
-  BLE Security
-*/
-class MySecurity : public BLESecurityCallbacks {
-
-    uint32_t onPassKeyRequest() {
-      Serial.printf("Pairing password: %d \r\n", PIN);
-      return PIN;
-    }
-
-    void onPassKeyNotify(uint32_t pass_key) {
-      Serial.printf("onPassKeyNotify\r\n");
-    }
-
-    bool onConfirmPIN(uint32_t pass_key) {
-      Serial.printf("onConfirmPIN\r\n");
-      return true;
-    }
-
-    bool onSecurityRequest() {
-      Serial.printf("onSecurityRequest\r\n");
-      return true;
-    }
-
-    void onAuthenticationComplete(esp_ble_auth_cmpl_t auth_cmpl) {
-      if (auth_cmpl.success) {
-        Serial.printf("onAuthenticationComplete\r\n");
-      } else {
-        Serial.println("Auth failure. Incorrect PIN?");
-        liveData->bleConnect = false;
-      }
-    }
-};
-
-/**
-   Ble notification callback
-*/
-static void notifyCallback (BLERemoteCharacteristic * pBLERemoteCharacteristic, uint8_t* pData, size_t length, bool isNotify) {
-
-  char ch;
-
-  // Parse multi line response to single lines
-  liveData->responseRow = "";
-  for (int i = 0; i <= length; i++) {
-    ch = pData[i];
-    if (ch == '\r'  || ch == '\n' || ch == '\0') {
-      if (liveData->responseRow != "")
-        parseRow();
-      liveData->responseRow = "";
-    } else {
-      liveData->responseRow += ch;
-      if (liveData->responseRow == ">") {
-        if (liveData->responseRowMerged != "") {
-          parseRowMerged();
-        }
-        liveData->responseRowMerged = "";
-        liveData->canSendNextAtCommand = true;
-      }
-    }
-  }
-}
-
-/**
-   Do connect BLE with server (OBD device)
-*/
-bool connectToServer(BLEAddress pAddress) {
-
-  board->displayMessage(" > Connecting device", "");
-
-  Serial.print("liveData->bleConnect ");
-  Serial.println(pAddress.toString().c_str());
-  board->displayMessage(" > Connecting device - init", pAddress.toString().c_str());
-
-  BLEDevice::setEncryptionLevel(ESP_BLE_SEC_ENCRYPT);
-  BLEDevice::setSecurityCallbacks(new MySecurity());
-
-  BLESecurity *pSecurity = new BLESecurity();
-  pSecurity->setAuthenticationMode(ESP_LE_AUTH_BOND); //
-  pSecurity->setCapability(ESP_IO_CAP_KBDISP);
-  pSecurity->setRespEncryptionKey(ESP_BLE_ENC_KEY_MASK | ESP_BLE_ID_KEY_MASK);
-
-  board->displayMessage(" > Connecting device", pAddress.toString().c_str());
-  liveData->pClient = BLEDevice::createClient();
-  liveData->pClient->setClientCallbacks(new MyClientCallback());
-  if (liveData->pClient->connect(pAddress, BLE_ADDR_TYPE_RANDOM) ) Serial.println("liveData->bleConnected");
-  Serial.println(" - liveData->bleConnected to server");
-
-  // Remote service
-  board->displayMessage(" > Connecting device", "Connecting service...");
-  BLERemoteService* pRemoteService = liveData->pClient->getService(BLEUUID(liveData->settings.serviceUUID));
-  if (pRemoteService == nullptr)
-  {
-    Serial.print("Failed to find our service UUID: ");
-    Serial.println(liveData->settings.serviceUUID);
-    board->displayMessage(" > Connecting device", "Unable to find service");
-    return false;
-  }
-  Serial.println(" - Found our service");
-
-  // Get characteristics
-  board->displayMessage(" > Connecting device", "Connecting TxUUID...");
-  liveData->pRemoteCharacteristic = pRemoteService->getCharacteristic(BLEUUID(liveData->settings.charTxUUID));
-  if (liveData->pRemoteCharacteristic == nullptr) {
-    Serial.print("Failed to find our characteristic UUID: ");
-    Serial.println(liveData->settings.charTxUUID);//.toString().c_str());
-    board->displayMessage(" > Connecting device", "Unable to find TxUUID");
-    return false;
-  }
-  Serial.println(" - Found our characteristic");
-
-  // Get characteristics
-  board->displayMessage(" > Connecting device", "Connecting RxUUID...");
-  liveData->pRemoteCharacteristicWrite = pRemoteService->getCharacteristic(BLEUUID(liveData->settings.charRxUUID));
-  if (liveData->pRemoteCharacteristicWrite == nullptr) {
-    Serial.print("Failed to find our characteristic UUID: ");
-    Serial.println(liveData->settings.charRxUUID);//.toString().c_str());
-    board->displayMessage(" > Connecting device", "Unable to find RxUUID");
-    return false;
-  }
-  Serial.println(" - Found our characteristic write");
-
-  board->displayMessage(" > Connecting device", "Register callbacks...");
-  // Read the value of the characteristic.
-  if (liveData->pRemoteCharacteristic->canNotify()) {
-    Serial.println(" - canNotify");
-    //liveData->pRemoteCharacteristic->registerForNotify(notifyCallback);
-    if (liveData->pRemoteCharacteristic->canIndicate()) {
-      Serial.println(" - canIndicate");
-      const uint8_t indicationOn[] = {0x2, 0x0};
-      //const uint8_t indicationOff[] = {0x0,0x0};
-      liveData->pRemoteCharacteristic->getDescriptor(BLEUUID((uint16_t)0x2902))->writeValue((uint8_t*)indicationOn, 2, true);
-      //liveData->pRemoteCharacteristic->getDescriptor(BLEUUID((uint16_t)0x2902))->writeValue((uint8_t*)notifyOff,2,true);
-      liveData->pRemoteCharacteristic->registerForNotify(notifyCallback, false);
-      delay(200);
-    }
-  }
-
-  board->displayMessage(" > Connecting device", "Done...");
-  if (liveData->pRemoteCharacteristicWrite->canWrite()) {
-    Serial.println(" - canWrite");
-  }
-
-  return true;
-}
-
-/**
-   Start ble scan
-*/
-void startBleScan() {
-
-  liveData->foundMyBleDevice = NULL;
-  liveData->scanningDeviceIndex = 0;
-  board->displayMessage(" > Scanning BLE4 devices", "40sec.or hold middle&RST");
-
-  // Start scanning
-  Serial.println("Scanning BLE devices...");
-  Serial.print("Looking for ");
-  Serial.println(liveData->settings.obdMacAddress);
-  BLEScanResults foundDevices = liveData->pBLEScan->start(40, false);
-  Serial.print("Devices found: ");
-  Serial.println(foundDevices.getCount());
-  Serial.println("Scan done!");
-  liveData->pBLEScan->clearResults(); // delete results fromBLEScan buffer to release memory
-
-  char tmpStr1[20];
-  sprintf(tmpStr1, "Found %d devices", foundDevices.getCount());
-  board->displayMessage(" > Scanning BLE4 devices", tmpStr1);
-
-  // Scan devices from menu, show list of devices
-  if (liveData->menuItemSelected == 2) {
-    Serial.println("Display menu with devices");
-    liveData->menuVisible = true;
-    liveData->menuCurrent = 9999;
-    liveData->menuItemSelected = 0;
-    board->showMenu();
-  } else {
-    // Redraw screen
-    if (liveData->foundMyBleDevice == NULL) {
-      board->displayMessage("Device not found", "Middle button - menu");
-    } else {
-      board->redrawScreen();
-    }
-  }
-}
-
-/**
-  SIM800L
-*/
-#ifdef SIM800L_ENABLED
-bool sim800lSetup() {
-  Serial.println("Setting SIM800L module");
-  
-  SerialGPRS.begin(9600);
-  
-  sim800l = new SIM800L((Stream *)&SerialGPRS, SIM800L_RST, 512 , 512);
-  // SIM800L DebugMode:
-  //sim800l = new SIM800L((Stream *)&SerialGPRS, SIM800L_RST, 512 , 512, (Stream *)&Serial);
-
-  bool sim800l_ready = sim800l->isReady();
-  for (uint8_t i = 0; i < 5 && !sim800l_ready; i++) {
-    Serial.println("Problem to initialize SIM800L module, retry in 1 sec");
-    delay(1000);
-    sim800l_ready = sim800l->isReady();
-  }
-
-  if (!sim800l_ready) {
-    Serial.println("Problem to initialize SIM800L module");
-  } else {
-    Serial.println("SIM800L module initialized");
-
-    Serial.print("Setting GPRS APN to: ");
-    Serial.println(liveData->settings.gprsApn);
-
-    bool sim800l_gprs = sim800l->setupGPRS(liveData->settings.gprsApn);
-    for (uint8_t i = 0; i < 5 && !sim800l_gprs; i++) {
-     Serial.println("Problem to set GPRS APN, retry in 1 sec");
-      delay(1000);
-      sim800l_gprs = sim800l->setupGPRS(liveData->settings.gprsApn);
-    }
-
-    if (sim800l_gprs) {
-      liveData->params.sim800l_enabled = true;
-      Serial.println("GPRS APN set OK");
-    } else {
-      Serial.println("Problem to set GPRS APN");
-    }
-  }
-
-  return true;
-}
-
-bool sendDataViaGPRS() {
-  Serial.println("Sending data via GPRS");
-
-  NetworkRegistration network = sim800l->getRegistrationStatus();
-  if (network != REGISTERED_HOME && network != REGISTERED_ROAMING) {
-    Serial.println("SIM800L module not connected to network, skipping data send");
-    return false;
-  }
-
-  if(!sim800l->isConnectedGPRS()) {
-    Serial.println("GPRS not connected... Connecting");
-    bool connected = sim800l->connectGPRS();
-    for (uint8_t i = 0; i < 5 && !connected; i++) {
-      Serial.println("Problem to connect GPRS, retry in 1 sec");
-      delay(1000);
-      connected = sim800l->connectGPRS();
-    }
-    if(connected) {
-      Serial.println("GPRS connected!");
-    } else {
-      Serial.println("GPRS not connected! Reseting SIM800L module!");
-      sim800l->reset();
-      sim800lSetup();
-
-      return false;
-    }
-  }
-
-  Serial.println("Start HTTP POST...");
-
-  StaticJsonDocument<512> jsonData;
-
-  jsonData["apikey"] = liveData->settings.remoteApiKey;
-  jsonData["carType"] = liveData->settings.carType;
-  jsonData["socPerc"] = liveData->params.socPerc;
-  jsonData["sohPerc"] = liveData->params.sohPerc;
-  jsonData["batPowerKw"] = liveData->params.batPowerKw;
-  jsonData["batPowerAmp"] = liveData->params.batPowerAmp;
-  jsonData["batVoltage"] = liveData->params.batVoltage;
-  jsonData["auxVoltage"] = liveData->params.auxVoltage;
-  jsonData["auxAmp"] = liveData->params.auxCurrentAmp;
-  jsonData["batMinC"] = liveData->params.batMinC;
-  jsonData["batMaxC"] = liveData->params.batMaxC;
-  jsonData["batInletC"] = liveData->params.batInletC;
-  jsonData["batFanStatus"] = liveData->params.batFanStatus;
-  jsonData["speedKmh"] = liveData->params.speedKmh;
-  jsonData["cumulativeEnergyChargedKWh"] = liveData->params.cumulativeEnergyChargedKWh;
-  jsonData["cumulativeEnergyDischargedKWh"] = liveData->params.cumulativeEnergyDischargedKWh;
-
-  char payload[512];
-  serializeJson(jsonData, payload);
-
-  Serial.print("Sending payload: ");
-  Serial.println(payload);
-
-  Serial.print("Remote API server: ");
-  Serial.println(liveData->settings.remoteApiUrl);
-
-  uint16_t rc = sim800l->doPost(liveData->settings.remoteApiUrl, "application/json", payload, 10000, 10000);
-  if (rc == 200) {
-    Serial.println("HTTP POST successful");
-  } else {
-    // Failed...
-    Serial.print("HTTP POST error: ");
-    Serial.println(rc);
-  }
-
-  return true;
-}
-#endif //SIM800L_ENABLED
-
 /**
   Setup device
 */
 void setup(void) {
 
-  // Serial console, init structures
-  Serial.begin(115200);
-  Serial.println("");
-  Serial.println("Booting device...");
+  // Serial console
+  syslog = new LogSerial();
+  syslog->println("\nBooting device...");
 
-  // Init settings/params, board library
-  line = "";
+  // Init settings/params
   liveData = new LiveData();
   liveData->initParams();
 
+  // Turn off serial console
+  if (liveData->settings.serialConsolePort == 255) {
+    syslog->println("Serial console disabled...");
+    syslog->flush();
+    syslog->end();
+  }
+
+  // Init board
 #ifdef BOARD_TTGO_T4
   board = new BoardTtgoT4v13();
 #endif // BOARD_TTGO_T4
@@ -548,124 +89,47 @@ void setup(void) {
   board->loadSettings();
   board->initBoard();
 
-  // Car interface
-  if (liveData->settings.carType == CAR_KIA_ENIRO_2020_64 || liveData->settings.carType == CAR_HYUNDAI_KONA_2020_64 ||
-      liveData->settings.carType == CAR_KIA_ENIRO_2020_39 || liveData->settings.carType == CAR_HYUNDAI_KONA_2020_39) {
-    car = new CarKiaEniro();
-  } else if (liveData->settings.carType == CAR_HYUNDAI_IONIQ_2018) {
-    car = new CarHyundaiIoniq();
-  } else if (liveData->settings.carType == CAR_KIA_NIRO_PHEV) {
-    car = new CarKiaNiroPhev();
-  } else if (liveData->settings.carType == CAR_RENAULT_ZOE) {
-    car = new CarRenaultZoe();
-  } else {
-    // if (liveData->settings.carType == CAR_DEBUG_OBD2_KIA)
-    car = new CarKiaDebugObd2();
+  // Init selected car interface
+  switch (liveData->settings.carType) {
+    case CAR_KIA_ENIRO_2020_39:
+    case CAR_KIA_ENIRO_2020_64:
+    case CAR_HYUNDAI_KONA_2020_39:
+    case CAR_HYUNDAI_KONA_2020_64:
+      car = new CarKiaEniro();
+      break;
+    case CAR_HYUNDAI_IONIQ_2018:
+      car = new CarHyundaiIoniq();
+      break;
+    case CAR_KIA_NIRO_PHEV:
+      car = new CarKiaNiroPhev();
+      break;
+    case CAR_RENAULT_ZOE:
+      car = new CarRenaultZoe();
+      break;
+    case CAR_BMW_I3_2014:
+      car = new CarBmwI3();
+      break;
+    default:
+      car = new CarKiaDebugObd2();
   }
+
   car->setLiveData(liveData);
   car->activateCommandQueue();
   board->attachCar(car);
-  board->debugCommandIndex = liveData->commandQueueLoopFrom;
+
+  // Finish board setup
+  board->afterSetup();
 
   // Redraw screen
   board->redrawScreen();
 
-  // Init time library
-  struct timeval tv;
-  tv.tv_sec = 1589011873;
-  settimeofday(&tv, NULL);
-  struct tm now;
-  getLocalTime(&now, 0);
-  liveData->params.chargingStartTime = liveData->params.currentTime = mktime(&now);
-
-  // Start BLE connection
-  Serial.println("Start BLE with PIN auth");
-  ESP_ERROR_CHECK(esp_bt_controller_mem_release(ESP_BT_MODE_CLASSIC_BT));
-  BLEDevice::init("");
-
-  // Retrieve a Scanner and set the callback we want to use to be informed when we have detected a new device.
-  // Specify that we want active scanning and start the scan to run for 10 seconds.
-  Serial.println("Setup BLE scan");
-  liveData->pBLEScan = BLEDevice::getScan();
-  liveData->pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks());
-  liveData->pBLEScan->setInterval(1349);
-  liveData->pBLEScan->setWindow(449);
-  liveData->pBLEScan->setActiveScan(true);
-
-  // Skip BLE scan if middle button pressed
-  if (strcmp(liveData->settings.obdMacAddress, "00:00:00:00:00:00") != 0 && !board->skipAdapterScan()) {
-    Serial.println(liveData->settings.obdMacAddress);
-    startBleScan();
-  }
-
-#ifdef SIM800L_ENABLED
-  sim800lSetup();
-#endif //SIM800L_ENABLED
-
-  // Hold right button
-  board->afterSetup();
-
   // End
-  Serial.println("Device setup completed");
+  syslog->println("Device setup completed");
 }
 
 /**
-  Loop
+  Main loop
 */
 void loop() {
-
-  // Connect BLE device
-  if (liveData->bleConnect == true && liveData->foundMyBleDevice != NULL) {
-    liveData->pServerAddress = new BLEAddress(liveData->settings.obdMacAddress);
-    if (connectToServer(*liveData->pServerAddress)) {
-
-      liveData->bleConnected = true;
-      liveData->bleConnect = false;
-
-      Serial.println("We are now connected to the BLE device.");
-
-      // Print message
-      board->displayMessage(" > Processing init AT cmds", "");
-
-      // Serve first command (ATZ)
-      doNextAtCommand();
-    } else {
-      Serial.println("We have failed to connect to the server; there is nothing more we will do.");
-    }
-  }
-
-  // Send command from TTY to OBD2
-  if (Serial.available()) {
-    ch = Serial.read();
-    if (ch == '\r' || ch == '\n') {
-      board->customConsoleCommand(line);
-      line = line + ch;
-      Serial.println(line);
-      if (liveData->bleConnected) {
-        liveData->pRemoteCharacteristicWrite->writeValue(line.c_str(), line.length());
-      }
-      line = "";
-    } else {
-      line = line + ch;
-    }
-  }
-
-  // Can send next command from queue to OBD
-  if (liveData->canSendNextAtCommand) {
-    liveData->canSendNextAtCommand = false;
-    doNextAtCommand();
-  }
-
-#ifdef SIM800L_ENABLED
-  if (liveData->params.lastDataSent + SIM800L_TIMER < liveData->params.currentTime && liveData->params.sim800l_enabled) {
-    sendDataViaGPRS();
-    liveData->params.lastDataSent = liveData->params.currentTime;
-  }
-#endif // SIM800L_ENABLED
-
   board->mainLoop();
-  if (board->scanDevices) {
-    board->scanDevices = false;
-    startBleScan();
-  }
 }
diff --git a/menu.h b/menu.h
index fbe5b61..2a482da 100644
--- a/menu.h
+++ b/menu.h
@@ -1,11 +1,12 @@
+#pragma once
 
-
-#include "config.h";
+#include "config.h"
 
 MENU_ITEM menuItemsSource[100] = {
 
   {0, 0, 0, "<- exit menu"},
   {MENU_VEHICLE_TYPE, 0, -1, "Vehicle type"},
+  {MENU_ADAPTER_TYPE, 0, -1, "Adapter type"},
   {2, 0, -1, "Select OBD2 BLE4 adapter"},
   {3, 0, -1, "Others"},
   {4, 0, -1, "Units"},
@@ -21,14 +22,22 @@ MENU_ITEM menuItemsSource[100] = {
   {104, 1, -1,  "Kia eNiro 2020 39kWh"},
   {105, 1, -1,  "Hyundai Kona 2020 39kWh"},
   {106, 1, -1,  "Renault Zoe 22kWh (DEV)"},
-  {107, 1, -1,  "Kia Niro PHEV 8.9kWh"},
+  {107, 1, -1,  "Kia Niro PHEV 8.9kWh (DEV)"},
+  {108, 1, -1,  "BMW i3 2014 22kWh (DEV)"},
   {120, 1, -1,  "Debug OBD2 Kia"},
 
+  {MENU_ADAPTER_BLE4-1, MENU_ADAPTER_TYPE, 0, "<- parent menu"},
+  {MENU_ADAPTER_BLE4, MENU_ADAPTER_TYPE, -1, "Bluetooth 4 (BLE4)"},
+  {MENU_ADAPTER_CAN, MENU_ADAPTER_TYPE, -1,  "CAN bus (MCP2515-1/SO)"},
+  //{MENU_ADAPTER_BT3, MENU_ADAPTER_TYPE, -1,  "Bluetooth 3 (dev)"},
+  
   {300, 3, 0, "<- parent menu"},
   // {MENU_WIFI, 3, -1, "[dev] WiFi network"},
   {MENU_SDCARD, 3, -1, "SD card"},
   {MENU_GPS, 3, -1, "GPS"},
-  {MENU_GPRS, 3, -1, "[dev] GSM/GPRS"},
+  {MENU_GPRS, 3, -1, "GSM/GPRS"},
+  {MENU_SERIAL_CONSOLE, 3, -1, "Serial console"},
+  {MENU_DEBUG_LEVEL, 3, -1, "Debug level"},
   //{MENU_REMOTE_UPLOAD, 3, -1, "[dev] Remote upload"},
   //{MENU_NTP, 3, -1, "[dev] NTP"},
   {MENU_SCREEN_ROTATION, 3, -1, "Screen rotation"},
@@ -36,21 +45,8 @@ MENU_ITEM menuItemsSource[100] = {
   {MENU_SCREEN_BRIGHTNESS, 3, -1, "LCD brightness"},
   {MENU_PREDRAWN_GRAPHS, 3, -1, "Pre-drawn ch.graphs"},
   {MENU_HEADLIGHTS_REMINDER, 3, -1, "Headlight reminder"},
-  {MENU_DEBUG_SCREEN, 3, -1, "Debug screen"},
+  {MENU_SLEEP_MODE, 3, -1, "SleepMode"},
   
-/*
-  // NTP
-  byte ntpEnabled; // 0/1 
-  byte ntpTimezone;
-  byte ntpDaySaveTime; // 0/1
-  // GPRS SIM800L 
-  byte gprsEnabled; // 0/1 
-  char gprsApn[64];
-  // Remote upload
-  byte remoteUploadEnabled; // 0/1 
-  char remoteApiUrl[64];
-  char remoteApiKey[32];*/
-
   {400, 4, 0, "<- parent menu"},
   {MENU_DISTANCE_UNIT, 4, -1, "Distance"},
   {MENU_TEMPERATURE_UNIT, 4, -1, "Temperature"},
@@ -61,11 +57,12 @@ MENU_ITEM menuItemsSource[100] = {
   {MENU_WIFI_SSID, 301, -1, "SSID"},
   {MENU_WIFI_PASSWORD, 301, -1, "Password"},
 
-  {3040, 304, 3, "<- parent menu"},
-  {MENU_SDCARD_ENABLED, 304, -1, "SD enabled"},
-  {MENU_SDCARD_AUTOSTARTLOG, 304, -1, "Autostart log enabled"},
-  {MENU_SDCARD_MOUNT_STATUS, 304, -1, "Status"},
-  {MENU_SDCARD_REC, 304, -1, "Record"},
+  {MENU_SDCARD*10, MENU_SDCARD, 3, "<- parent menu"},
+  {MENU_SDCARD_ENABLED, MENU_SDCARD, -1, "SD enabled"},
+  {MENU_SDCARD_AUTOSTARTLOG, MENU_SDCARD, -1, "Autostart log enabled"},
+  {MENU_SDCARD_MOUNT_STATUS, MENU_SDCARD, -1, "Status"},
+  {MENU_SDCARD_REC, MENU_SDCARD, -1, "Record"},
+  //{MENU_SDCARD_INTERVAL, MENU_SDCARD, -1, "Log interval sec."},
   
   {3060, 306, 3, "<- parent menu"},
   {3061, 306, -1, "Auto mode"},
diff --git a/screenshots/v2.jpg b/screenshots/v2.jpg
new file mode 100644
index 0000000..d86a39f
Binary files /dev/null and b/screenshots/v2.jpg differ
diff --git a/screenshots/v2_m5charging2.jpg b/screenshots/v2_m5charging2.jpg
new file mode 100644
index 0000000..4083beb
Binary files /dev/null and b/screenshots/v2_m5charging2.jpg differ
diff --git a/screenshots/v2_m5speed.jpg b/screenshots/v2_m5speed.jpg
new file mode 100644
index 0000000..a28e9d8
Binary files /dev/null and b/screenshots/v2_m5speed.jpg differ