From dd16d9e320a1088b6437d2dd41384d4c45431980 Mon Sep 17 00:00:00 2001 From: Stanley Huang Date: Tue, 11 Apr 2017 13:19:42 +1000 Subject: Fixed connection failure due to too short timeout on slower K-line protocols --- libraries/OBD2UART/OBD2UART.cpp | 26 +++++++++++++++----------- 1 file changed, 15 insertions(+), 11 deletions(-) (limited to 'libraries/OBD2UART') diff --git a/libraries/OBD2UART/OBD2UART.cpp b/libraries/OBD2UART/OBD2UART.cpp index fd6b35d..4e9a8de 100644 --- a/libraries/OBD2UART/OBD2UART.cpp +++ b/libraries/OBD2UART/OBD2UART.cpp @@ -399,17 +399,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; } } @@ -420,15 +420,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) { -- cgit v1.2.3