Comm interface

Board320_240.cpp

@@ -66,6 +66,8 @@ void Board320_240::afterSetup() {
 
   // Init GPS
   if (liveData->settings.gpsHwSerialPort <= 2) {
+    Serial.print("GPS initialization on hw uart: "); 
+    Serial.println(liveData->settings.gpsHwSerialPort);
     gpsHwUart = new HardwareSerial(liveData->settings.gpsHwSerialPort);
     gpsHwUart->begin(9600);
   }
@@ -73,10 +75,13 @@ void Board320_240::afterSetup() {
   // SD card
   if (liveData->settings.sdcardEnabled == 1) {
     if (sdcardMount() && liveData->settings.sdcardAutstartLog == 1) {
+      Serial.println("Toggle recording on SD card");
       sdcardToggleRecording();
     }
   }
 
+  // Init from parent class
+  Serial.println("BoardInterface::afterSetup");
   BoardInterface::afterSetup();
 }
 
@@ -1032,7 +1037,7 @@ 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: scanDevices = true; commInterface->scanDevices(); return;
       // Reset settings
       case 8: resetSettings(); hideMenu(); return;
       // Save settings
@@ -1187,6 +1192,8 @@ void Board320_240::redrawScreen() {
 */
 void Board320_240::loadTestData() {
 
+  Serial.println("Loading test data");
+  
   testDataMode = true; // skip lights off message
   carInterface->loadTestData();
   redrawScreen();
@@ -1314,6 +1321,9 @@ void Board320_240::mainLoop() {
   // Shutdown when car is off
   if (liveData->params.automaticShutdownTimer != 0 && liveData->params.currentTime - liveData->params.automaticShutdownTimer > 5)
     shutdownDevice();
+
+  // Read data from BLE/CAN
+  commInterface->mainLoop();    
 }
 
 /**

BoardInterface.cpp

@@ -51,7 +51,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();
@@ -209,13 +210,19 @@ void BoardInterface::loadSettings() {
 void BoardInterface::afterSetup() {
 
   // Init Comm iterface
+  Serial.print("Init communication device: ");
+  Serial.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();
+    Serial.println("BT3 not implemented");
   }
-  //commInterface->initComm(liveData, NULL);
+
+  commInterface->initComm(liveData, this);
   commInterface->connectDevice();
 }
 
@@ -241,6 +248,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
 */

BoardInterface.h

@@ -35,6 +35,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;

CommInterface.cpp

@@ -2,12 +2,13 @@
 #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*/) {
+void CommInterface::initComm(LiveData* pLiveData, BoardInterface* pBoard) {
     liveData = pLiveData;   
-    //board = pBoard;   
+    board = pBoard;   
 }
     
 #endif // COMMINTERFACE_CPP

CommInterface.h

@@ -4,16 +4,21 @@
 #include "LiveData.h"
 //#include "BoardInterface.h"
 
+class BoardInterface; // Forward declaration
+
 class CommInterface {
   
   protected:
     LiveData* liveData;   
-    //BoardInterface* board;   
+    BoardInterface* board;   
+    char ch;
+    String line;
   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() = 0;
 };
 
 #endif // COMMINTERFACE_H

CommObd2Ble4.cpp

@@ -3,27 +3,430 @@
 
 #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) {
+      Serial.println("onConnect");
+    }
+
+    // On BLE disconnect
+    void onDisconnect(BLEClient* pclient) {
+      //connected = false;
+      Serial.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) {
+
+      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 (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>") {
+//          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(liveDataObj->settings.serviceUUID)) &&
+          (strcmp(advertisedDevice.getAddress().toString().c_str(), liveDataObj->settings.obdMacAddress) == 0)) {
+        Serial.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() {
+      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?");
+        liveDataObj->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
+  liveDataObj->responseRow = "";
+  for (int i = 0; i <= length; i++) {
+    ch = pData[i];
+    if (ch == '\r'  || ch == '\n' || ch == '\0') {
+      if (liveDataObj->responseRow != "")
+        commObj->parseRow();
+      liveDataObj->responseRow = "";
+    } else {
+      liveDataObj->responseRow += ch;
+      if (liveDataObj->responseRow == ">") {
+        if (liveDataObj->responseRowMerged != "") {
+          commObj->parseRowMerged();
+        }
+        liveDataObj->responseRowMerged = "";
+        liveDataObj->canSendNextAtCommand = true;
+      }
+    }
+  }
+}
+
+/**
+   Connect ble4 adapter
+*/
 void CommObd2Ble4::connectDevice() {
-  Serial.println("COMM connectDevice");
-}
 
-/** 
+  commObj = this;
- * Disconnect device
+  liveDataObj = liveData;
- */
+  boardObj = board;
-void CommObd2Ble4::disconnectDevice() {
+  line = "";
-  Serial.println("COMM disconnectDevice");
+ 
+  Serial.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.
+  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();
+  }
 }
 
 /**
- * Scan device list
+   Disconnect device
- */
+*/
+void CommObd2Ble4::disconnectDevice() {
+
+  Serial.println("COMM disconnectDevice");
+  btStop();
+}
+
+/**
+   Scan device list, from menu
+*/
 void CommObd2Ble4::scanDevices() {
+  
   Serial.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
+  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();
+    }
+  }
+}
+
+/**
+   Do connect BLE with server (OBD device)
+*/
+bool CommObd2Ble4::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");
+    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;
+}
+
+/**
+ * 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->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();
+  }
+
+  if (board->scanDevices) {
+    board->scanDevices = false;
+    startBleScan();
+  }
+
+}
+
+/**
+  Do next AT command from queue
+*/
+bool CommObd2Ble4::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->responseRowMerged = "";
+  liveData->pRemoteCharacteristicWrite->writeValue(tmpStr.c_str(), tmpStr.length());
+  liveData->commandQueueIndex++;
+
+  return true;
+}
+    
+/**
+  Parse result from OBD, create single line liveData->responseRowMerged
+*/
+bool CommObd2Ble4::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) == ':') {
+    liveData->responseRowMerged += liveData->responseRow.substring(2);
+  }
+
+  return true;
+}
+
+/**
+  Parse merged row (after merge completed)
+*/
+bool CommObd2Ble4::parseRowMerged() {
+
+  Serial.print("merged:");
+  Serial.println(liveData->responseRowMerged);
+
+  // Parse by selected car interface
+  board->parseRowMerged();
+
+  return true;
 }
 
 #endif // COMMOBD2BLE4_CPP

CommObd2Ble4.h

@@ -1,17 +1,25 @@
 #ifndef COMMOBD2BLE4_H
 #define COMMOBD2BLE4_H
 
+#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 startBleScan();
+    bool connectToServer(BLEAddress pAddress);
+    bool doNextAtCommand();
+    bool parseRow();
+    bool parseRowMerged();
 };
 
 #endif // COMMOBD2BLE4_H

CommObd2Can.cpp

@@ -1,7 +1,41 @@
 #ifndef COMMINTERFACE_CPP
 #define COMMINTERFACE_CPP
 
-#include "CommInterface.h"
+#include "CommObd2Can.h"
+#include "BoardInterface.h"
 #include "LiveData.h"
 
+/**
+   Connect CAN adapter
+*/
+void CommObd2Can::connectDevice() {
+
+  line = "";
+ 
+  Serial.println("CAN connectDevice");
+}
+
+/**
+   Disconnect device
+*/
+void CommObd2Can::disconnectDevice() {
+
+  Serial.println("COMM disconnectDevice");
+}
+
+/**
+   Scan device list, from menu
+*/
+void CommObd2Can::scanDevices() {
+  
+  Serial.println("COMM scanDevices");
+}
+
+/**
+ * Main loop
+ */
+void CommObd2Can::mainLoop() {
+  
+}
+
 #endif // COMMINTERFACE_CPP

CommObd2Can.h

@@ -2,11 +2,22 @@
 #define COMMOBD2CAN_H
 
 #include "LiveData.h"
+#include "CommInterface.h"
 
 class CommObd2Can : public CommInterface {
-  
+
   protected:
   public:
+    void connectDevice() override;
+    void disconnectDevice() override;
+    void scanDevices() override;
+    void mainLoop() override;
+    //
+   /* void startBleScan();
+    bool connectToServer(BLEAddress pAddress);
+    bool doNextAtCommand();
+    bool parseRow();
+    bool parseRowMerged(); */
 };
 
 #endif // COMMOBD2CAN_H

LiveData.h

@@ -242,6 +242,5 @@ class LiveData {
     float bar2pressure(float inBar);
 };
 
-
 //
 #endif // LIVEDATA_H

evDash.ino

@@ -49,7 +49,6 @@
 #include "BoardM5stackCore.h"
 #endif // BOARD_M5STACK_CORE
 
-#include <BLEDevice.h>
 #include <sys/time.h>
 #include "config.h"
 #include "LiveData.h"
@@ -69,333 +68,11 @@ SIM800L* sim800l;
 HardwareSerial SerialGPRS(2);
 #endif //SIM800L_ENABLED
 
-// Temporary variables
-char ch;
-String line;
-
 // 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->responseRowMerged = "";
-  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) == ':') {
-    liveData->responseRowMerged += liveData->responseRow.substring(2);
-  }
-
-  return true;
-}
-
-/**
-  Parse merged row (after merge completed)
-*/
-bool parseRowMerged() {
-
-  Serial.print("merged:");
-  Serial.println(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
 */
@@ -445,7 +122,7 @@ bool sim800lSetup() {
 bool sendDataViaGPRS() {
   Serial.println("Sending data via GPRS");
 
-  if(liveData->params.socPerc < 0) {
+  if (liveData->params.socPerc < 0) {
     Serial.println("No valid data, skipping data send");
     return false;
   }
@@ -530,7 +207,6 @@ void setup(void) {
   Serial.println("Booting device...");
 
   // Init settings/params, board library
-  line = "";
   liveData = new LiveData();
   liveData->initParams();
 
@@ -575,31 +251,11 @@ void setup(void) {
   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
+  // Finish board setup
   board->afterSetup();
 
   // End
@@ -607,52 +263,10 @@ void setup(void) {
 }
 
 /**
-  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();
@@ -661,8 +275,4 @@ void loop() {
 #endif // SIM800L_ENABLED
 
   board->mainLoop();
-  if (board->scanDevices) {
-    board->scanDevices = false;
-    startBleScan();
-  }
 }