From a38c047391419171c092a3a9ded633961e1ddfe0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?J=C3=A1n=20M=C3=A1tik?= <jan.matik@gmail.com>
Date: Mon, 28 Dec 2020 09:31:11 +0100
Subject: [PATCH] Added support for delay between commands on CAN

---
 CarBmwI3.cpp    |  2 ++
 CommObd2Can.cpp | 25 ++++++++++++++++++++++---
 CommObd2Can.h   |  1 +
 LiveData.h      |  2 ++
 4 files changed, 27 insertions(+), 3 deletions(-)

diff --git a/CarBmwI3.cpp b/CarBmwI3.cpp
index a0524c4..64d57c3 100644
--- a/CarBmwI3.cpp
+++ b/CarBmwI3.cpp
@@ -77,6 +77,8 @@ void CarBmwI3::activateCommandQueue() {
 	liveData->commandQueueCount = commandQueue.size();
   liveData->rxBuffOffset = 1; // 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 
 }
 
 /**
diff --git a/CommObd2Can.cpp b/CommObd2Can.cpp
index 6cd1767..c6db37b 100644
--- a/CommObd2Can.cpp
+++ b/CommObd2Can.cpp
@@ -78,6 +78,15 @@ 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
@@ -89,7 +98,7 @@ void CommObd2Can::mainLoop() {
         break;
       delay(1);
       // apply timeout for next frames loop too
-      if (lastDataSent != 0 && (unsigned long)(millis() - lastDataSent) > 100) {
+      if (lastDataSent != 0 && (unsigned long)(millis() - lastDataSent) > liveData->rxTimeoutMs) {
         Serial.print("CAN execution timeout (multiframe message).\n");
         break;
       }
@@ -100,7 +109,8 @@ void CommObd2Can::mainLoop() {
       return;
     }
   }
-  if (lastDataSent != 0 && (unsigned long)(millis() - lastDataSent) > 100) {
+  
+  if (lastDataSent != 0 && (unsigned long)(millis() - lastDataSent) > liveData->rxTimeoutMs) {
     Serial.print("CAN execution timeout. Continue with next command.\n");
     liveData->canSendNextAtCommand = true;
     return;
@@ -152,6 +162,8 @@ void CommObd2Can::sendPID(const uint16_t pid, const String& cmd) {
 
     if (cmd == "22402B" || cmd == "22F101") {
       pPacket->startChar = txStartChar = 0x12;
+    } else if (cmd == "22D85C" || cmd == "22D96B") {
+      pPacket->startChar = txStartChar = 0x78;
     } else {
       pPacket->startChar = txStartChar = 0x07;
     }
@@ -181,6 +193,7 @@ void CommObd2Can::sendPID(const uint16_t pid, const String& cmd) {
   }
 
   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)  {
@@ -188,6 +201,7 @@ void CommObd2Can::sendPID(const uint16_t pid, const String& cmd) {
     lastDataSent = millis();
   }   else {
     Serial.print("Error sending PID ");
+    lastDataSent = millis();
   }
   Serial.print(pid);
   for (uint8_t i = 0; i < 8; i++) {
@@ -514,5 +528,10 @@ void CommObd2Can::processMergedResponse() {
   Serial.println(liveData->responseRowMerged);
   board->parseRowMerged();
   liveData->responseRowMerged = "";
-  liveData->canSendNextAtCommand = true;
+  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 578b105..a09f124 100644
--- a/CommObd2Can.h
+++ b/CommObd2Can.h
@@ -26,6 +26,7 @@ class CommObd2Can : public CommInterface {
     
     std::vector<uint8_t> mergedData;
     std::unordered_map<uint16_t, std::vector<uint8_t>> dataRows;
+    bool bResponseProcessed = false;
     
     enum class enFrame_t
     {
diff --git a/LiveData.h b/LiveData.h
index 79c4daa..431e5b7 100644
--- a/LiveData.h
+++ b/LiveData.h
@@ -251,6 +251,8 @@ class LiveData {
     // Canbus
     uint8_t rxBuffOffset = 0;  // offset of processing received data, in some cars needs to be set to 1, like in BMW i3
     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