Fixed connection failure due to too short timeout on slower K-line protocols

1 files changed, 15 insertions, 11 deletions

diff --git a/libraries/OBD/OBD.cpp b/libraries/OBD/OBD.cpp
index f23da93..116a68d 100644
--- a/libraries/OBD/OBD.cpp
+++ b/libraries/OBD/OBD.cpp
@@ -401,17 +401,17 @@ bool COBD::init(OBD_PROTOCOLS protocol)
 	const char *initcmd[] = {"ATZ\r", "ATE0\r", "ATH0\r"};
 	char buffer[64];
 
+	m_state = OBD_DISCONNECTED;
 	for (unsigned char i = 0; i < sizeof(initcmd) / sizeof(initcmd[0]); i++) {
 		write(initcmd[i]);
 		if (receive(buffer, sizeof(buffer), OBD_TIMEOUT_LONG) == 0) {
-			m_state = OBD_DISCONNECTED;
 			return false;
 		}
 	}
 	if (protocol != PROTO_AUTO) {
-		sprintf_P(buffer, PSTR("ATSP%u\r"), protocol);
+		sprintf_P(buffer, PSTR("ATSP %u\r"), protocol);
+		write(buffer);
 		if (receive(buffer, sizeof(buffer), OBD_TIMEOUT_LONG) == 0 && !strstr(buffer, "OK")) {
-			m_state = OBD_DISCONNECTED;
 			return false;
 		}
 	}
@@ -422,15 +422,19 @@ bool COBD::init(OBD_PROTOCOLS protocol)
 	for (byte i = 0; i < 4; i++) {
 		byte pid = i * 0x20;
 		sendQuery(pid);
-		char* data = getResponse(pid, buffer, sizeof(buffer));
-		if (!data) break;
-		data--;
-		for (byte n = 0; n < 4; n++) {
-			if (data[n * 3] != ' ')
-				break;
-			pidmap[i * 4 + n] = hex2uint8(data + n * 3 + 1);
+		if (receive(buffer, sizeof(buffer), OBD_TIMEOUT_LONG) > 0) {
+			char *p = buffer;
+			while ((p = strstr(p, "41 "))) {
+				p += 3;
+				if (hex2uint8(p) == pid) {
+					p += 2;
+					for (byte n = 0; n < 4 && *(p + n * 3) == ' '; n++) {
+						pidmap[i * 4 + n] = hex2uint8(p + n * 3 + 1);
+					}
+					success = true;
+				}
+			}
 		}
-		success = true;
 	}
 
 	if (success) {