From 77ad96edb7957f49cdce61f4e863f67a6f143bb0 Mon Sep 17 00:00:00 2001
From: Jim Whitelaw <jimwhitelaw@gmail.com>
Date: Wed, 31 Jan 2024 16:44:22 -0700
Subject: [PATCH] Fix for Custom PIDs #218 Mode 0x22 response headers always
 zero-pad the pid to 4 chars, even for a 2-char pid. That confused the header
 detection and response parsing. This change checks for the mode and pid in
 findResponse() and adjusts the expected header accordingly.

---
 src/ELMduino.cpp | 26 +++++++++++++++++++-------
 src/ELMduino.h   |  3 ++-
 2 files changed, 21 insertions(+), 8 deletions(-)

diff --git a/src/ELMduino.cpp b/src/ELMduino.cpp
index dd264c4..4c59631 100644
--- a/src/ELMduino.cpp
+++ b/src/ELMduino.cpp
@@ -117,7 +117,7 @@ bool ELM327::initializeELM(const char &protocol, const byte &dataTimeout)
                 // the timeout value to 30 seconds, then restore the previous value.
                 uint16_t prevTimeout = timeout_ms;
                 timeout_ms = 30000;
-                
+
                 int8_t state = sendCommand_Blocking("0100");
 
                 if (state == ELM_SUCCESS)
@@ -203,6 +203,8 @@ void ELM327::formatQueryArray(uint8_t service, uint16_t pid, uint8_t num_respons
         Serial.println(pid);
     }
 
+    isMode0x22Query = (service == 0x22 && pid <= 0xFF); // mode 0x22 responses always zero-pad the pid to 4 chars, even for a 2-char pid
+
     query[0] = ((service >> 4) & 0xF) + '0';
     query[1] = (service & 0xF) + '0';
 
@@ -616,7 +618,7 @@ double ELM327::processPID(const uint8_t &service, const uint16_t &pid, const uin
         {
             nb_query_state = SEND_COMMAND; // Reset the query state machine for next command
 
-            findResponse();
+            findResponse(service, pid);
 
             return conditionResponse(numExpectedBytes, scaleFactor, bias);
         }
@@ -2312,7 +2314,7 @@ int8_t ELM327::get_response(void)
 }
 
 /*
- uint64_t ELM327::findResponse()
+ uint64_t ELM327::findResponse(uint8_t &service)
 
  Description:
  ------------
@@ -2326,7 +2328,7 @@ int8_t ELM327::get_response(void)
  -------
   * uint64_t - Query response value
 */
-uint64_t ELM327::findResponse()
+uint64_t ELM327::findResponse(const uint8_t &service, const uint8_t &pid)
 {
     uint8_t firstDatum = 0;
     char header[7] = {'\0'};
@@ -2344,8 +2346,18 @@ uint64_t ELM327::findResponse()
     {
         header[0] = query[0] + 4;
         header[1] = query[1];
-        header[2] = query[2];
-        header[3] = query[3];
+        if (isMode0x22Query) // mode 0x22 responses always zero-pad the pid to 4 chars, even for a 2-char pid
+        {
+            header[2] = '0';
+            header[3] = '0';
+            header[4] = query[2];
+            header[5] = query[3];
+        }
+        else
+        {
+            header[2] = query[2];
+            header[3] = query[3];
+        }
     }
 
     if (debugMode)
@@ -2359,7 +2371,7 @@ uint64_t ELM327::findResponse()
 
     if (firstHeadIndex >= 0)
     {
-        if (longQuery)
+        if (longQuery | isMode0x22Query)
             firstDatum = firstHeadIndex + 6;
         else
             firstDatum = firstHeadIndex + 4;
diff --git a/src/ELMduino.h b/src/ELMduino.h
index 8040dc1..5516b5a 100644
--- a/src/ELMduino.h
+++ b/src/ELMduino.h
@@ -315,7 +315,7 @@ class ELM327
 	bool begin(Stream& stream, const bool& debug = false, const uint16_t& timeout = 1000, const char& protocol = '0', const uint16_t& payloadLen = 40, const byte& dataTimeout = 0);
 	bool initializeELM(const char& protocol = '0', const byte& dataTimeout = 0);
 	void flushInputBuff();
-	uint64_t findResponse();
+	uint64_t findResponse(const uint8_t &service, const uint8_t &pid);
 	bool queryPID(const uint8_t& service, const uint16_t& pid, const uint8_t& num_responses = 1);
 	bool queryPID(char queryStr[]);
 	double processPID(const uint8_t& service, const uint16_t& pid, const uint8_t& num_responses, const uint8_t& numExpectedBytes, const double& scaleFactor = 1, const float& bias = 0);
@@ -421,6 +421,7 @@ class ELM327
 private:
 	char query[QUERY_LEN] = { '\0' };
 	bool longQuery = false;
+	bool isMode0x22Query = false;
 	uint32_t currentTime;
 	uint32_t previousTime;