From c799c5a6c516838e65351a9746906edf6fa2f3d2 Mon Sep 17 00:00:00 2001
From: jakubdziworski <jakub.dziworski@gmail.com>
Date: Tue, 7 Jul 2020 19:49:13 +0200
Subject: [PATCH] Added support for responses longer than 4 hex digits

---
 src/ELMduino.cpp | 14 ++++++++------
 1 file changed, 8 insertions(+), 6 deletions(-)

diff --git a/src/ELMduino.cpp b/src/ELMduino.cpp
index f4de2c7..6799a1a 100644
--- a/src/ELMduino.cpp
+++ b/src/ELMduino.cpp
@@ -640,12 +640,14 @@ uint32_t ELM327::findResponse()
 		else
 			payBytes = recBytes - firstDatum;
 
-		// Some PID queries return 4 hex digit values - the
-		// rest return 2 hex digit values
-		if (payBytes >= 4)
-			return (ctoi(payload[firstDatum]) << 12) | (ctoi(payload[firstDatum + 1]) << 8) | (ctoi(payload[firstDatum + 2]) << 4) | ctoi(payload[firstDatum + 3]);
-		else
-			return (ctoi(payload[firstDatum]) << 4) | ctoi(payload[firstDatum + 1]);
+
+		uint32_t response = 0;
+		for(uint8_t i = 0; i < payBytes; i++) {
+			uint8_t payloadIndex = firstDatum + i;
+			uint8_t bitsOffset = 4 * (payBytes - i - 1);
+			response = response | (ctoi(payload[payloadIndex]) << bitsOffset);
+		}
+		return response;
 	}
 
 	return 0;